using UnityEngine; using System.Collections; public class MyQuaternion : MonoBehaviour { public float X, Y, Z, W; public MyQuaternion() { X = 0.0f; Y = 0.0f; Z = 0.0f; W = 0.0f; } public MyQuaternion(float w, float x, float y, float z) { X = x; Y = y; Z = z; W = w; } public MyQuaternion(float w, Vector3 v) { W = w; X = v.x; Y = v.y; Z = v.z; } public void Conjugate() { this.X = -this.X; this.Y = -this.Y; this.Z = -this.Z; } public float Length(MyQuaternion Q) { float norm = Mathf.Sqrt((Q.W * Q.W) + (Q.X * Q.X) + (Q.Y * Q.Y) + (Q.Z * Q.Z)); return norm; } public void Normalize() { float length = Length(this); this.W /= length; this.X /= length; this.Y /= length; this.Z /= length; } public MyQuaternion Normalized() { Normalize(); return this; } public MyQuaternion Inverse() { Conjugate(); MyQuaternion Q = Normalized(); return Q; } public static MyQuaternion Inverse(MyQuaternion Q) { Q.Inverse(); return Q; } public static MyQuaternion operator +(MyQuaternion Q1, MyQuaternion Q2) { return new MyQuaternion(Q1.W + Q2.W, Q1.X + Q2.X, Q1.Y + Q2.Y, Q1.Z + Q2.Z); } public static MyQuaternion operator -(MyQuaternion Q1, MyQuaternion Q2) { return new MyQuaternion(Q1.W - Q2.W, Q1.X - Q2.X, Q1.Y - Q2.Y, Q1.Z - Q2.Z); } public static MyQuaternion operator -(MyQuaternion Q) { return new MyQuaternion(-Q.W, -Q.X, -Q.Y, -Q.Z); } public static MyQuaternion operator *(MyQuaternion Q1, MyQuaternion Q2) { float w = Q1.W * Q2.W - Q1.X * Q2.X - Q1.Y * Q2.Y - Q1.Z * Q2.Z; float x = Q1.W * Q2.X + Q1.X * Q2.W + Q1.Y * Q2.Z - Q1.Z * Q2.Y; float y = Q1.W * Q2.Y + Q1.Y * Q2.W + Q1.Z * Q2.X - Q1.X * Q2.Z; float z = Q1.W * Q2.Z + Q1.Z * Q2.W + Q1.X * Q2.Y - Q1.Y * Q2.X; return new MyQuaternion(w, x, y, z); } public static Vector3 operator *(MyQuaternion Q, Vector3 V) { MyQuaternion qVec = new MyQuaternion(0.0f, V); MyQuaternion qRes = Q * qVec; return new Vector3(qRes.X, qRes.Y, qRes.Z); } public static MyQuaternion operator *(MyQuaternion Q, float scale) { return new MyQuaternion(Q.W * scale, Q.X * scale, Q.Y * scale, Q.Z * scale); } public static MyQuaternion operator /(MyQuaternion Q, float scale) { return new MyQuaternion(Q.W / scale, Q.X / scale, Q.Y / scale, Q.Z / scale); } public static float Dot(MyQuaternion Q1, MyQuaternion Q2) { return Q1.X * Q2.X + Q1.Y * Q2.Y + Q1.Z * Q2.Z + Q1.W * Q2.W; } public static MyQuaternion lerp(MyQuaternion Q1, MyQuaternion Q2, float t) { MyQuaternion Q = new MyQuaternion(); Q = Q1 * (1 - t) + Q2 * t; Q.Normalized(); return Q; } public static MyQuaternion slerp(MyQuaternion Q1, MyQuaternion Q2, float t) { MyQuaternion Q3; double dot = MyQuaternion.Dot(Q1, Q2); /* dot = cos(theta) if (dot < 0), q1 and q2 are more than 90 degrees apart, so we can invert one to reduce spinning */ if (dot < 0) { dot = -dot; Q3 = -Q2; } else Q3 = Q2; if (dot < 0.95f) { float angle = (float)System.Math.Cos(dot); return (Q1 * Mathf.Sin(angle * (1 - t)) + Q3 * Mathf.Sin(angle * t)) / Mathf.Sin(angle); } else // if the angle is small, use linear interpolation return lerp(Q1, Q3, t); } public static MyQuaternion Euler(float x, float y, float z) { MyQuaternion quaternion = new MyQuaternion(0, 0, 0, 0); float cr, cp, cy, sr, sp, sy, cpcy, spsy; cr = Mathf.Cos(x * Mathf.Deg2Rad * .5f); cp = Mathf.Cos(y * Mathf.Deg2Rad * .5f); cy = Mathf.Cos(z * Mathf.Deg2Rad * .5f); sr = Mathf.Sin(x * Mathf.Deg2Rad * .5f); sp = Mathf.Sin(y * Mathf.Deg2Rad * .5f); sy = Mathf.Sin(z * Mathf.Deg2Rad * .5f); cpcy = cp * cy; spsy = sp * sy; quaternion.W = cr * cpcy + sr * spsy; quaternion.X = sr * cpcy - cr * spsy; quaternion.Y = cr * sp * cy + sr * cp * sy; quaternion.Z = cr * cp * sy - sr * sp * cy; return quaternion; } public static MyQuaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection) { fromDirection.Normalize(); toDirection.Normalize(); float angle = (float) (Mathf.Acos(Vector3.Dot(fromDirection, toDirection)) / 2.0); Vector3 u = Vector3.Cross(fromDirection, toDirection); u.Normalize(); return new MyQuaternion(Mathf.Cos(angle), Mathf.Sin(angle) * u.x, Mathf.Sin(angle) * u.y, Mathf.Sin(angle) * u.z); } public static MyQuaternion LookRotation(Vector3 forward, Vector3 upwards) { Vector3 right = Vector3.Cross(upwards, forward); MyQuaternion quaternion = new MyQuaternion(0, 0, 0, 0); quaternion.W = Mathf.Sqrt(1.0f + right.x + upwards.y + forward.z); float recip = 1.0f / (4.0f * quaternion.W); quaternion.X = (upwards.z - forward.y) * recip; quaternion.Y = (forward.x - right.z) * recip; quaternion.Z = (right.z - upwards.x) * recip; return quaternion; } public static MyQuaternion LookRotation(Vector3 forward) { return MyQuaternion.LookRotation(forward, Vector3.up); } public static void OrthoNormalize(Vector3 forward, ref Vector3 up) { //This method uses the Gram-Schmidt method to orthonormalize the vectors Vector3 u2 = up - ((Vector3.Dot(forward, up) / Vector3.Dot(forward, forward)) * forward); up = u2; } public static MyQuaternion RotateTowards(MyQuaternion qFrom, MyQuaternion qTo, float maxDegreesDelta) { float targetAngle = Angle(qFrom, qTo); if (targetAngle < maxDegreesDelta) return qTo; else return slerp(qFrom, qFrom, maxDegreesDelta / targetAngle); } public void SetLookRotation(Vector3 vLookAt, Vector3 vUp) { Vector3 forward = vLookAt; Vector3 up = vUp; OrthoNormalize(forward, ref up); Vector3 right = Vector3.Cross(up, forward); this.W = Mathf.Sqrt(1.0f + right.x + up.x + forward.x) * 0.5f; float w4_recip = 1.0f / (4.0f * this.W); this.X = (up.z - forward.y) * w4_recip; this.Y = (forward.x - right.z) * w4_recip; this.Z = (right.y - up.x) * w4_recip; } public static float Angle(MyQuaternion Q1, MyQuaternion Q2) { return (float)(Mathf.Acos(Dot(Q1, Q2))); } public Vector3 Rotate(Vector3 v) { Vector3 vRotated; MyQuaternion qRot = new MyQuaternion(0.0f, v); MyQuaternion qInv = this.Inverse(); MyQuaternion qRotated = this * qRot * qInv; vRotated.x = qRotated.X; vRotated.y = qRotated.Y; vRotated.z = qRotated.Z; return vRotated; } //public Point3D Rotate(Point3D p) //{ // Point3D pRotated = new Point3D(); // MyQuaternion qRot = new MyQuaternion(0.0, p.X, p.Y, p.Z); // MyQuaternion qInv = this.Inverse(); // MyQuaternion qRotated = this * qRot * qInv; // pRotated.X = qRotated.X; // pRotated.Y = qRotated.Y; // pRotated.Z = qRotated.Z; // return pRotated; //} public void SetFromToRotation(Vector3 fromDirection, Vector3 toDirection) { fromDirection.Normalize(); toDirection.Normalize(); float angle = (float)(Mathf.Acos(Vector3.Dot(fromDirection, toDirection)) / 2.0); Vector3 u = Vector3.Cross(fromDirection, toDirection); u.Normalize(); this.W = Mathf.Cos(angle); this.X = Mathf.Sin(angle) * u.x; this.Y = Mathf.Sin(angle) * u.y; this.Z = Mathf.Sin(angle) * u.z; } public static MyQuaternion AngleAxis(Vector3 axis, float angle) { MyQuaternion quaternion = new MyQuaternion(0, 0, 0, 0); quaternion.W = Mathf.Cos((angle * Mathf.Deg2Rad) / 2); quaternion.X = axis.x * Mathf.Sin((angle * Mathf.Deg2Rad) / 2); quaternion.Y = axis.y * Mathf.Sin((angle * Mathf.Deg2Rad) / 2); quaternion.Z = axis.z * Mathf.Sin((angle * Mathf.Deg2Rad) / 2); return quaternion; } public void ToAngleAxis(out Vector3 axis, out float angle) { angle = Mathf.Acos(this.W); float sin_theta_inv = (float)(1.0 / Mathf.Sin(angle)); axis = new Vector3(this.X * sin_theta_inv, this.Y * sin_theta_inv, this.Z * sin_theta_inv); angle *= 2.0f; } }