54 lines
1.7 KiB
C#
54 lines
1.7 KiB
C#
using Unity.Burst;
|
|
using Unity.Mathematics;
|
|
|
|
namespace UnityEngine.U2D.IK
|
|
{
|
|
[BurstCompile]
|
|
internal static class IKMathUtility
|
|
{
|
|
internal const float kEpsilonNormalSqrt = 1e-15F;
|
|
|
|
[BurstCompile]
|
|
internal static float Angle(in float3 from, in float3 to)
|
|
{
|
|
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
|
|
float denominator = math.sqrt(math.lengthsq(from) * math.lengthsq(to));
|
|
if (denominator < kEpsilonNormalSqrt)
|
|
return 0F;
|
|
|
|
float dot = math.clamp(math.dot(from, to) / denominator, -1F, 1F);
|
|
return math.acos(dot);
|
|
}
|
|
|
|
[BurstCompile]
|
|
internal static float Angle(in float2 from, in float2 to)
|
|
{
|
|
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
|
|
float denominator = math.sqrt(math.lengthsq(from) * math.lengthsq(to));
|
|
if (denominator < kEpsilonNormalSqrt)
|
|
return 0F;
|
|
|
|
float dot = math.clamp(math.dot(from, to) / denominator, -1F, 1F);
|
|
return math.acos(dot);
|
|
}
|
|
|
|
[BurstCompile]
|
|
internal static float SignedAngle(in float3 from, in float3 to, in float3 axis)
|
|
{
|
|
float unsignedAngle = Angle(from, to);
|
|
|
|
float3 cross = math.cross(from, to);
|
|
float sign = math.sign(math.dot(axis, cross));
|
|
return unsignedAngle * sign;
|
|
}
|
|
|
|
[BurstCompile]
|
|
internal static float SignedAngle(in float2 from, in float2 to)
|
|
{
|
|
float unsignedAngle = Angle(from, to);
|
|
float sign = math.sign(from.x * to.y - from.y * to.x);
|
|
return unsignedAngle * sign;
|
|
}
|
|
}
|
|
}
|