// Convert normalised Quaternion into euler Angle - retrieved from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ public static Vector3 GetEulerAngleN(Quarts A) { Vector3 r = new Vector3(0, 0, 0); float test = A.x * A.y + A.z * A.w; if (test > 0.499) { // singularity at north pole r.y = 2 * Mathf.Atan2(A.x, A.w); r.z = Mathf.PI / 2; r.x = 0; return(r); } if (test < -0.499) { // singularity at south pole r.y = -2 * Mathf.Atan2(A.x, A.w); r.z = -Mathf.PI / 2; r.x = 0; return(r); } float sqx = A.x * A.x; float sqy = A.y * A.y; float sqz = A.z * A.z; r.y = Mathf.Atan2(2 * A.y * A.w - 2 * A.x * A.z, 1 - 2 * sqy - 2 * sqz); r.z = Mathf.Asin(2 * test); r.x = Mathf.Atan2(2 * A.x * A.w - 2 * A.y * A.z, 1 - 2 * sqx - 2 * sqz); return(r); }
// Update is called once per frame void Update() { // Set Oorbit speed Operiod = TimeM.timeMult; // Set our Ellipse orbit setEllipseOrigin(); // Handle pause of ellipse movement (since a mult of 0 doesnt stop it completely) if (TimeM.pause == false) { // Move along planets orbit MovingOrbit(); } // Rotate around planets axis (Day/Night Cycle) based on current speed roll += TimeM.timeMult; // Render object MeshFilter MF = GetComponent <MeshFilter>(); // Tells QCombine routine the object its moving and all its data CurrentPosition = (Quarts.QCombine(new Vectors(Vx, Vy, Vz), CurrentPosition, new Vectors(scalex, scaley, scalez), new Vectors(roll, pitch, yaw), ModelSpaceVertices, MF)); transform.localPosition = new Vector3(CurrentPosition.x, CurrentPosition.y, CurrentPosition.z); // Debug - shows xyz position on screen since mesh rendered doesnt move the actual transform x = CurrentPosition.x; y = CurrentPosition.y; z = CurrentPosition.z; }
// Quarternion multiplication public static Quarts operator *(Quarts S, Quarts R) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); // Turn Quarternions into vectors for calculating xyz values later Vectors SV = GetAxis(S); Vectors RV = GetAxis(R); // Calculates the new W value float Cross = Vectors.DotProduct(GetAxis(S), GetAxis(R)); r.w = (S.w * R.w) - Cross; // Calculates the new XYZ values Vectors temp = new Vectors(0, 0, 0); temp = ((RV * S.w) + (SV * R.w) + Vectors.CrossProduct(RV, SV)); // Put the new xyz values into the return quaternion r.x = temp.x; r.y = temp.y; r.z = temp.z; return(r); }
// Convert non normalised Quaternion into euler Angle - retrieved from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ public static Vector3 GetEulerAngle(Quarts A) { Vector3 r = new Vector3(0, 0, 0); float sqw = A.w * A.w; float sqx = A.x * A.x; float sqy = A.y * A.y; float sqz = A.z * A.z; float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor float test = A.x * A.y + A.z * A.w; if (test > 0.499 * unit) { // singularity at north pole r.y = 2 * Mathf.Atan2(A.x, A.w); r.z = Mathf.PI / 2; r.x = 0; return(r); } if (test < -0.499 * unit) { // singularity at south pole r.y = -2 * Mathf.Atan2(A.x, A.w); r.z = -Mathf.PI / 2; r.x = 0; return(r); } r.y = Mathf.Atan2(2 * A.y * A.w - 2 * A.x * A.z, sqx - sqy - sqz + sqw); r.z = Mathf.Asin(2 * test / unit); r.x = Mathf.Atan2(2 * A.x * A.w - 2 * A.y * A.z, -sqx + sqy - sqz + sqw); return(r); }
// Convert Matrix to qauternion - retrieved from gamasutra: https://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php?page=1 public static Quarts MatToQuat(Matrix4X4 M) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); float tr, s; float[] q = new float[4]; int i, j, k; int[] nxt = new int[3] { 1, 2, 0 }; tr = M.values[0, 0] + M.values[1, 1] + M.values[2, 2]; // Check diagonal if (tr > 0.0f) { s = Mathf.Sqrt(tr + 1.0f); r.w = s / 2.0f; s = 0.5f / s; r.x = (M.values[1, 2] - M.values[2, 1]) * s; r.y = (M.values[2, 0] - M.values[0, 2]) * s; r.z = (M.values[0, 1] - M.values[1, 0]) * s; } // Diagonal negative else { i = 0; if (M.values[1, 1] > M.values[0, 0]) { i = 1; } if (M.values[2, 2] > M.values[i, i]) { i = 2; } j = nxt[i]; k = nxt[j]; s = Mathf.Sqrt(M.values[i, i] - M.values[j, j] + M.values[k, k] + 1.0f); q[i] = s * 0.5f; if (s != 0.0f) { s = 0.5f / s; } q[3] = (M.values[j, k] - M.values[k, j]) * s; q[j] = (M.values[i, j] + M.values[j, i]) * s; q[k] = (M.values[i, k] + M.values[k, i]) * s; r.x = q[0]; r.y = q[1]; r.z = q[2]; r.w = q[3]; } return(r); }
// Slerp Quaternion Method public static Quarts SLERP(Quarts A, Quarts B, float C) { C = Mathf.Clamp(C, 0.0f, 1.0f); Quarts temp = B * A.Inverse(); Vectors AxisAngle = temp.GetAxisAngle(); Quarts temp2 = new Quarts(AxisAngle.w * C, new Vectors(AxisAngle.x, AxisAngle.y, AxisAngle.z)); return(temp2 * A); }
public Quarts Inverse() { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); r.w = w; r.x = -x; r.y = -y; r.z = -z; return(r); }
// Update is called once per frame void Update() { // Rotate timeOfDay = (Time.deltaTime / 3600) * timeBoost; yaw += ((timeOfDay / 24) * 360.0f); // Render object MeshFilter MF = GetComponent <MeshFilter>(); // Tells QCombine routine the object its moving and all its data Quarts.QCombine(new Vectors(0, 0, 0), new Vectors(0, 0, 0), new Vectors(scalex, scaley, scalez), new Vectors(roll, pitch, yaw), ModelSpaceVertices, MF); }
// Combine all quarternions without putting them into matrices first (except Scale) - this routine was made because theres an issue with plugging them into matrices that makes them shrink and enlarge when rotating public static Vectors QCombine(Vectors Translation, Vectors CurrentLocation, Vectors Scale, Vectors AxisRots, Vector3[] ModelSpaceVertices, MeshFilter MF) { Vectors r = new Vectors(0, 0, 0); // Create Qauternions from raw data Quarts S = QScale(Scale); Quarts R = new Quarts(AxisRots.x, new Vectors(0, 0, 1)); Quarts P = new Quarts(AxisRots.y, new Vectors(1, 0, 0)); Quarts Y = new Quarts(AxisRots.z, new Vectors(0, 1, 0)); Quarts RPY = R * P * Y; // Remember TRS order (T omitted for now because its being weird) Quarts Combined = RPY * S; // Define array of vertices Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length]; // move vertexs to new position for (int i = 0; i < TransformedVertices.Length; i++) { // Convert Vertexs into Quaternions Vectors temp = new Vectors(ModelSpaceVertices[i].x, ModelSpaceVertices[i].y, ModelSpaceVertices[i].z); Quarts Vertexs = new Quarts(0, new Vectors(0, 0, 0)); Vertexs.SetAxis(temp); // Rotate & Scale vertexs - Using slerp Quarts slerped = SLERP(Combined, Vertexs, Time.deltaTime); slerped = Combined * Vertexs * Combined.Inverse(); Vectors finalV = GetAxis(slerped); //// This is our basic vertex mover - just added onto the result of the Q matrix //// LERP for that nice smooth movement (Dont forget to add vertex position onto translation amount, u dummy) //Vectors NewLocation = new Vectors(Translation.x + finalV.x, Translation.y + finalV.y, Translation.z + finalV.z); //Vectors Move = Vectors.VLerp(NewLocation, finalV, Time.deltaTime); // Move the Vertex to its new position TransformedVertices[i] = new Vector3(finalV.x, finalV.y, finalV.z); } // Assign new Vertices MF.mesh.vertices = TransformedVertices; // Sometimes needed to clean up mesh MF.mesh.RecalculateNormals(); MF.mesh.RecalculateBounds(); /// return world position Vectors NewOriginLocation = CurrentLocation + Translation; Vectors NewOrigin = Vectors.VLerp(NewOriginLocation, CurrentLocation, Time.deltaTime); r = NewOrigin; return(r); }
// Norm public static Quarts Norm(Quarts A) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); r.w = A.w * 2; r.x = A.x * 2; r.y = A.y * 2; r.z = A.z * 2; return(r); }
// Create a quaternion scalar from vector public static Quarts QScale(Vectors A) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); // Create matrices from axes Matrix4X4 S = Matrix4X4.CreateScale(A); // Convert to quaternion Quarts SQ = MatToQuat(S); // Set return value to the new position r = SQ; return(r); }
// Combine RPY angles together into a quaternion public static Quarts QCombinedAxis(Vectors A) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); // Create matrices from axes Matrix4X4 R = Matrix4X4.CreateRoll(A.x); Matrix4X4 P = Matrix4X4.CreatePitch(A.y); Matrix4X4 Y = Matrix4X4.CreateYaw(A.z); // Convert to quaternion Quarts RQ = MatToQuat(R); Quarts PQ = MatToQuat(P); Quarts YQ = MatToQuat(Y); // Multiply together to get combined quarternion r = (RQ * PQ) * YQ; return(r); }
// Combine all quarternions (T * RPY * S) through matrices public static void QCombineM(Vectors Translation, Vectors Scale, Vectors AxisRots, Vector3[] ModelSpaceVertices, MeshFilter MF) { // Create Qauternions from raw data Quarts T = QTrans(Translation); // Not used because not working Quarts S = QScale(Scale); Quarts RPY = QCombinedAxis(AxisRots); // Multiply in correct order //Quarts Order = T * RPY * S; // Define array of vertices Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length]; // move vertexs to new position for (int i = 0; i < TransformedVertices.Length; i++) { // Convert Vertexs into Quaternions Vectors temp = new Vectors(ModelSpaceVertices[i].x, ModelSpaceVertices[i].y, ModelSpaceVertices[i].z); Quarts Vertexs = new Quarts(0, new Vectors(0, 0, 0)); Vertexs.SetAxis(temp); // Remember TRS order (T omitted for now because its being weird) Quarts Combined = RPY * S; // Rotate & Scale vertexs Quarts result = Combined * Vertexs * Combined.Inverse(); Vectors finalV = GetAxis(result); // This is our basic vertex mover - just added onto the result of the Q matrix Vectors Move = new Vectors(Translation.x + finalV.x, Translation.y + finalV.y, Translation.z + finalV.z); // Move the Vertex to its new position TransformedVertices[i] = new Vector3(Move.x, Move.y, Move.z); } // Assign new Vertices MF.mesh.vertices = TransformedVertices; // Sometimes needed to clean up mesh MF.mesh.RecalculateNormals(); MF.mesh.RecalculateBounds(); }
// Create a quaternion translation from vector public static Quarts QTrans(Vectors A) { Quarts r = new Quarts(0, new Vectors(0, 0, 0)); Quarts TQ = new Quarts(0, new Vectors(0, 0, 0)); Quarts Angle = new Quarts(0, new Vectors(0, 1, 0)); // Create matrices from axes //Matrix4X4 T = Matrix4X4.CreateTrans(A); For some reason translation matrix not working //Vectors Translated = T * A; // Convert to quaternion //Quarts TQ = MatToQuat(T); // Create quaternion from vector TQ.SetAxis(A); //TQ = Angle * TQ * Angle.Inverse(); // Set return value to the new position r = TQ; //r.SetAxis(Translated); return(r); }
// Creates a vector from quaternion public static Vectors GetAxis(Quarts A) { return(new Vectors(A.x, A.y, A.z)); }