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))); }