static QuaternionL Normalize(QuaternionL q)
{
FloatL scale = 1.0f / q.Length;
QuaternionL result = new QuaternionL(q.xyz * scale, q.w * scale);
return(result);
}
public static Matrix4x4L TRS(Vector3L pos, QuaternionL q, Vector3L s)
{
Matrix4x4L posMatrix = Matrix4x4L.identity;
posMatrix.m03 = pos.x;
posMatrix.m13 = pos.y;
posMatrix.m23 = pos.z;
Matrix4x4L rotateMatrix = Matrix4x4L.identity;
rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;
rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;
rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
Matrix4x4L scaleMatrix = Scale(s);
Matrix4x4L ret = posMatrix * rotateMatrix * scaleMatrix;
return(ret);
}
// 四元数转欧拉角
static Vector3L ToEulerRad(QuaternionL rotation)
{
FloatL sqw = rotation.w * rotation.w;
FloatL sqx = rotation.x * rotation.x;
FloatL sqy = rotation.y * rotation.y;
FloatL sqz = rotation.z * rotation.z;
FloatL unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
FloatL test = rotation.x * rotation.w - rotation.y * rotation.z;
Vector3L v;
if (test > 0.4995d * unit)
{ // singularity at north pole
v.y = 2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
if (test < -0.4995d * unit)
{ // singularity at south pole
v.y = -2f * FixPointMath.Atan2(rotation.y, rotation.x);
v.x = -FixPointMath.PI / 2;
v.z = 0;
return(NormalizeAngles(v * FixPointMath.Rad2Deg));
}
QuaternionL q = new QuaternionL(rotation.w, rotation.z, rotation.x, rotation.y);
v.y = FixPointMath.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
v.x = FixPointMath.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
v.z = FixPointMath.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
return(NormalizeAngles(v * FixPointMath.Rad2Deg) * FixPointMath.Deg2Rad);
}
public static QuaternionL LookRotation(Vector3L forward, Vector3L up)
{
forward = Vector3L.Normalize(forward);
Vector3L right = Vector3L.Normalize(Vector3L.Cross(up, forward));
up = Vector3L.Cross(forward, right);
FloatL m00 = right.x;
FloatL m01 = right.y;
FloatL m02 = right.z;
FloatL m10 = up.x;
FloatL m11 = up.y;
FloatL m12 = up.z;
FloatL m20 = forward.x;
FloatL m21 = forward.y;
FloatL m22 = forward.z;
FloatL num8 = (m00 + m11) + m22;
QuaternionL quaternion = new QuaternionL();
if (num8 > 0f)
{
FloatL num = FixPointMath.Sqrt(num8 + 1f);
quaternion.w = num * 0.5f;
num = 0.5f / num;
quaternion.x = (m12 - m21) * num;
quaternion.y = (m20 - m02) * num;
quaternion.z = (m01 - m10) * num;
return(quaternion);
}
if ((m00 >= m11) && (m00 >= m22))
{
FloatL num7 = FixPointMath.Sqrt(((1f + m00) - m11) - m22);
FloatL num4 = 0.5f / num7;
quaternion.x = 0.5f * num7;
quaternion.y = (m01 + m10) * num4;
quaternion.z = (m02 + m20) * num4;
quaternion.w = (m12 - m21) * num4;
return(quaternion);
}
if (m11 > m22)
{
FloatL num6 = FixPointMath.Sqrt(((1f + m11) - m00) - m22);
FloatL num3 = 0.5f / num6;
quaternion.x = (m10 + m01) * num3;
quaternion.y = 0.5f * num6;
quaternion.z = (m21 + m12) * num3;
quaternion.w = (m20 - m02) * num3;
return(quaternion);
}
FloatL num5 = FixPointMath.Sqrt(((1f + m22) - m00) - m11);
FloatL num2 = 0.5f / num5;
quaternion.x = (m20 + m02) * num2;
quaternion.y = (m21 + m12) * num2;
quaternion.z = 0.5f * num5;
quaternion.w = (m01 - m10) * num2;
return(quaternion);
}
public OBB3d(Vector3L pos, QuaternionL qua, Vector3L size)
{
m_pos = pos;
m_rotation = qua;
//m_xLength = size.x;
//m_yLength = size.y;
//m_zLength = size.z;
m_size = size;
}
public override bool Equals(object other)
{
if (!(other is QuaternionL))
{
return(false);
}
QuaternionL quaternion = (QuaternionL)other;
return(this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w));
}
public static QuaternionL Inverse(QuaternionL rotation)
{
FloatL lengthSq = rotation.LengthSquared;
if (lengthSq != 0.0)
{
FloatL i = 1.0f / lengthSq;
return(new QuaternionL(rotation.xyz * -i, rotation.w * i));
}
return(rotation);
}
public OBB3d(Vector3L pos, QuaternionL qua, FloatL xLength, FloatL yLength, FloatL zLength)
{
m_pos = pos;
m_rotation = qua;
//m_xLength = xLength;
//m_yLength = yLength;
//m_zLength = zLength;
m_size.x = xLength;
m_size.y = yLength;
m_size.z = zLength;
}
public static QuaternionL Lerp(QuaternionL from, QuaternionL to, FloatL t)
{
if (t > 1)
{
t = 1;
}
if (t < 0)
{
t = 0;
}
return(Slerp(from, to, t));
}
public static QuaternionL RotateTowards(QuaternionL from, QuaternionL to, FloatL maxDegreesDelta)
{
FloatL num = QuaternionL.Angle(from, to);
if (num == 0f)
{
return(to);
}
FloatL t = FixPointMath.Min(1f, maxDegreesDelta / num);
return(QuaternionL.SlerpUnclamped(from, to, t));
}
public static QuaternionL Slerp(QuaternionL a, QuaternionL b, FloatL t)
{
if (t > 1)
{
t = 1;
}
if (t < 0)
{
t = 0;
}
return(SlerpUnclamped(a, b, t));
}
public static void Test()
{
Line2d line1 = new Line2d();
line1.m_point1 = new Vector2L(0, 0);
line1.m_point2 = new Vector2L(100, 100);
Line2d line2 = new Line2d();
line2.m_point1 = new Vector2L(0, 100);
line2.m_point2 = new Vector2L(100, 0);
Vector2L intersectionPoint = new Vector2L();
if (Line2dWithLine2d(line1, line2, ref intersectionPoint))
{
Debug.Log("双线相交 交点 " + intersectionPoint);
}
else
{
Debug.Log("双线没有相交");
}
List <Vector2L> pointList = new List <Vector2L> {
new Vector2L(-4.5f, -10f),
new Vector2L(-4.5f, 10f),
new Vector2L(4.5f, 10f),
new Vector2L(4.5f, -10f)
};
Convex2d convex1 = new Convex2d(new Vector2L(-5, 0), QuaternionL.Euler(Vector3L.zero), pointList);
Convex2d convex2 = new Convex2d(new Vector2L(+5, 0), QuaternionL.Euler(Vector3L.zero), pointList);
Debug.Log("凸多边形相交测试1 " + (Convex2dWithConvex2d(convex1, convex2, false, true) != null));
Convex2d convex3 = new Convex2d(new Vector2L(+5, 0), QuaternionL.Euler(new Vector3L(0, -90, 0)), pointList);
Debug.Log("凸多边形相交测试2 " + (Convex2dWithConvex2d(convex1, convex3, false, true) != null));
Circle2d circle = new Circle2d(new Vector3L(-5, 0), 5);
Debug.Log("圆与凸多边形相交测试1 " + (Circle2dWithConvex2d(circle, convex2, false, true) != null));
Debug.Log("圆与凸多边形相交测试2 " + (Circle2dWithConvex2d(circle, convex3, false, true) != null));
}
static void ToAxisAngleRad(QuaternionL q, out Vector3L axis, out FloatL angle)
{
if (FixPointMath.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * FixPointMath.Acos(q.w); // angle
FloatL den = FixPointMath.Sqrt(1.0f - q.w * q.w);
if (den > 0.0001f)
{
axis = q.xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = new Vector3L(1, 0, 0);
}
}
public static QuaternionL AngleAxis(FloatL degress, Vector3L axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
QuaternionL result = identity;
FloatL radians = degress * FixPointMath.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * FixPointMath.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = FixPointMath.Cos(radians);
return(Normalize(result));
}
public Convex2d(Vector2L pos, QuaternionL qua, List <Vector2L> pointList)
{
m_pos = pos;
m_rotation = qua;
m_pointList = pointList;
}
/// <summary>
/// 从矩阵中提取Quaternion
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static QuaternionL GetRotationFromMatrix(Matrix4x4L m)
{
return(QuaternionL.LookRotation(m.GetColumn(2), m.GetColumn(1)));
}
/// <summary>
/// 各个方向转rotation
/// </summary>
/// <param name="dir"></param>
/// <returns></returns>
public static QuaternionL DirectionToRotation(Vector3L dir)
{
return(QuaternionL.FromToRotation(Vector3L.forward, dir));
}
/// <summary>
/// 从水平方向转换rotation
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
public static QuaternionL LookAt(Vector3L dir)
{
return(QuaternionL.LookRotation(dir, Vector3L.up));
}
/// <summary>
/// 从rotation获取方向
/// </summary>
/// <param name="rotation"></param>
/// <returns></returns>
public static Vector3L GetForward(QuaternionL rotation)
{
return(rotation * Vector3L.forward);
}
public static FloatL Angle(QuaternionL a, QuaternionL b)
{
FloatL f = QuaternionL.Dot(a, b);
return(FixPointMath.Acos(FixPointMath.Min(FixPointMath.Abs(f), 1f)) * 2f * 57.29578d);
}
public void ToAngleAxis(out FloatL angle, out Vector3L axis)
{
QuaternionL.ToAxisAngleRad(this, out axis, out angle);
angle *= FixPointMath.Rad2Deg;
}
private static QuaternionL SlerpUnclamped(QuaternionL a, QuaternionL b, FloatL t)
{
// if either input is zero, return the other.
if (a.LengthSquared == 0.0f)
{
if (b.LengthSquared == 0.0f)
{
return(identity);
}
return(b);
}
else if (b.LengthSquared == 0.0f)
{
return(a);
}
FloatL cosHalfAngle = a.w * b.w + Vector3L.Dot(a.xyz, b.xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(a);
}
else if (cosHalfAngle < 0.0f)
{
b.xyz = -b.xyz;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
FloatL blendA;
FloatL blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
FloatL halfAngle = FixPointMath.Acos(cosHalfAngle);
FloatL sinHalfAngle = FixPointMath.Sin(halfAngle);
FloatL oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = FixPointMath.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
blendB = FixPointMath.Sin(halfAngle * t) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
QuaternionL result = new QuaternionL(blendA * a.xyz + blendB * b.xyz, blendA * a.w + blendB * b.w);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
public static FloatL Dot(QuaternionL a, QuaternionL b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
public static bool operator !=(QuaternionL lhs, QuaternionL rhs)
{
return(QuaternionL.Dot(lhs, rhs) <= 0.999999d);
}
public static QuaternionL FromToRotation(Vector3L v1, Vector3L v2)
{
FloatL angle = Vector3L.Angle(v1, v2);
return(QuaternionL.AngleAxis(angle, Vector3L.Cross(v1, v2)));
}
