//http://geomalgorithms.com/
//https://stackoverflow.com/questions/1073336/circle-line-segment-collision-detection-algorithm
public static bool TestRayCircle(LVector2 cPos, LFloat cR, LVector2 rB, LVector2 rDir, ref LFloat t)
{
var d = rDir;
var f = rB - cPos;
var a = LVector2.Dot(d, d);
var b = 2 * LVector2.Dot(f, d);
var c = LVector2.Dot(f, f) - cR * cR;
var discriminant = b * b - 4 * a * c;
if (discriminant < 0)
{
// no intersection
return(false);
}
else
{
discriminant = LMath.Sqrt(discriminant);
var t1 = (-b - discriminant) / (2 * a);
var t2 = (-b + discriminant) / (2 * a);
if (t1 >= 0)
{
t = t1;
return(true);
}
if (t2 >= 0)
{
t = t2;
return(true);
}
return(false);
}
}
public static LVector3 normalizesafe(LVector3 x)
{
LFloat len = LMath.Dot(x, x);
if (len > LFloat.EPSILON)
{
return(x * (LFloat.one / LMath.Sqrt(len)));
}
return(LVector3.zero);
}
public LFloat Estimate(Triangle node, Triangle endNode)
{
LFloat dst2;
LFloat minDst2 = LFloat.MaxValue;
A_AB = (node.a).Add(node.b) * LFloat.half;
A_AB = (node.b).Add(node.c) * LFloat.half;
A_AB = (node.c).Add(node.a) * LFloat.half;
B_AB = (endNode.a).Add(endNode.b) * LFloat.half;
B_BC = (endNode.b).Add(endNode.c) * LFloat.half;
B_CA = (endNode.c).Add(endNode.a) * LFloat.half;
if ((dst2 = A_AB.dst2(B_AB)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_AB.dst2(B_BC)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_AB.dst2(B_CA)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_BC.dst2(B_AB)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_BC.dst2(B_BC)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_BC.dst2(B_CA)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_CA.dst2(B_AB)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_CA.dst2(B_BC)) < minDst2)
{
minDst2 = dst2;
}
if ((dst2 = A_CA.dst2(B_CA)) < minDst2)
{
minDst2 = dst2;
}
return((LFloat)LMath.Sqrt(minDst2));
}
private static LQuaternion MatrixToQuaternion(LMatrix33 m)
{
LQuaternion quat = new LQuaternion();
LFloat fTrace = m[0, 0] + m[1, 1] + m[2, 2];
LFloat root;
if (fTrace > 0)
{
root = LMath.Sqrt(fTrace + 1);
quat.w = LFloat.half * root;
root = LFloat.half / root;
quat.x = (m[2, 1] - m[1, 2]) * root;
quat.y = (m[0, 2] - m[2, 0]) * root;
quat.z = (m[1, 0] - m[0, 1]) * root;
}
else
{
int[] s_iNext = new int[] { 1, 2, 0 };
int i = 0;
if (m[1, 1] > m[0, 0])
{
i = 1;
}
if (m[2, 2] > m[i, i])
{
i = 2;
}
int j = s_iNext[i];
int k = s_iNext[j];
root = LMath.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
if (root < 0)
{
throw new IndexOutOfRangeException("error!");
}
quat[i] = LFloat.half * root;
root = LFloat.half / root;
quat.w = (m[k, j] - m[j, k]) * root;
quat[j] = (m[j, i] + m[i, j]) * root;
quat[k] = (m[k, i] + m[i, k]) * root;
}
LFloat nor = LMath.Sqrt(Dot(quat, quat));
quat = new LQuaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor);
return(quat);
}
public void Normalize()
{
LFloat sqrtm = sqrtMagnitude;
if (sqrtm != 1)
{
LFloat m = LMath.Sqrt(sqrtm);
x /= m;
y /= m;
z /= m;
w /= m;
}
}
/// <summary>
/// 转换为角轴
/// </summary>
/// <param name="angle"></param>
/// <param name="axis"></param>
public void ToAngleAxis(out LFloat angle, out LVector3 axis)
{
angle = 2 * LMath.Acos(w);
if (angle == 0)
{
axis = LVector3.right;
return;
}
LFloat div = 1 / LMath.Sqrt(1 - w * w);
axis = new LVector3(x * div, y * div, z * div);
angle = angle * 180 / LMath.PI;
}
void NearestPosition(NativeArray <LVector3> targets, LVector3 position,
out int nearestPositionIndex, out LFloat nearestDistance)
{
nearestPositionIndex = 0;
var ptr = targets.GetPointer(0);
var len = targets.m_Length;
nearestDistance = math.lengthsq(position - *ptr);
++ptr;
for (int i = 1; i < len; i++, ++ptr)
{
var distance = math.lengthsq(position - *ptr);
var nearest = distance < nearestDistance;
nearestDistance = math.select(nearestDistance, distance, nearest);
nearestPositionIndex = math.select(nearestPositionIndex, new LFloat(i), nearest);
}
nearestDistance = LMath.Sqrt(nearestDistance);
}
/// <summary>
/// sqrt(a^2 + b^2) without under/overflow.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static LFloat Hypot(LFloat a, LFloat b)
{
LFloat r;
if (LMath.Abs(a) > LMath.Abs(b))
{
r = b / a;
r = LMath.Abs(a) * LMath.Sqrt(1 + r * r);
}
else if (b != 0)
{
r = a / b;
r = LMath.Abs(b) * LMath.Sqrt(1 + r * r);
}
else
{
r = 0d;
}
return(r);
}
/// <summary>
/// 线性插值(无限制)
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="t"></param>
/// <returns></returns>
public static LQuaternion LerpUnclamped(LQuaternion a, LQuaternion b, LFloat t)
{
LQuaternion tmpQuat = new LQuaternion();
if (Dot(a, b) < 0)
{
tmpQuat.Set(a.x + t * (-b.x - a.x),
a.y + t * (-b.y - a.y),
a.z + t * (-b.z - a.z),
a.w + t * (-b.w - a.w));
}
else
{
tmpQuat.Set(a.x + t * (b.x - a.x),
a.y + t * (b.y - a.y),
a.z + t * (b.z - a.z),
a.w + t * (b.w - a.w));
}
LFloat nor = LMath.Sqrt(Dot(tmpQuat, tmpQuat));
return(new LQuaternion(tmpQuat.x / nor, tmpQuat.y / nor, tmpQuat.z / nor, tmpQuat.w / nor));
}
