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