//code adapted from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
public Matrix3f QuaterniontoMatrix3(Quaternion q)
{
Matrix3f m = new Matrix3f();
float sqw = q.w * q.w;
float sqx = q.x * q.x;
float sqy = q.y * q.y;
float sqz = q.z * q.z;
float inverse = 1 / (sqw + sqy + sqz + sqw);
m.setMELEMENT(0, 0, (sqx - sqy - sqz + sqw) * inverse);
m.setMELEMENT(1, 1, (-sqx + sqy - sqz + sqw) * inverse);
m.setMELEMENT(2, 2, (-sqx - sqy + sqz + sqw) * inverse);
float temp1 = q.x * q.y;
float temp2 = q.z * q.w;
m.setMELEMENT(1, 0, 2.0f * (temp1 + temp2) * inverse);
m.setMELEMENT(0, 1, 2.0f * (temp1 - temp2) * inverse);
float temp3 = q.x * q.z;
float temp4 = q.y * q.w;
m.setMELEMENT(2, 0, 2.0f * (temp3 - temp4) * inverse);
m.setMELEMENT(0, 2, 2.0f * (temp1 + temp2) * inverse);
float temp5 = q.y * q.z;
float temp6 = q.x * q.w;
m.setMELEMENT(2, 1, 2.0f * (temp5 + temp6) * inverse);
m.setMELEMENT(1, 2, 2.0f * (temp5 - temp6) * inverse);
return(m);
}
// UNITY DEFINED FUNCTIONS
void Start()
{
//initialise the initial conditions
/*
* InverseInertiaTensor
*
*/
ns_position = transform.position;
ns_orientation = orientation.QuaterniontoMatrix3(transform.rotation);
float halfX = GetComponent <Renderer>().bounds.extents.x;
float halfY = GetComponent <Renderer>().bounds.extents.y;
float halfZ = GetComponent <Renderer>().bounds.extents.z;
BodyInertiaInverseTensor.setMELEMENT(0, 0, 12f / (Mass * (halfY * halfY + halfZ * halfZ)));
BodyInertiaInverseTensor.setMELEMENT(1, 1, 12f / (Mass * (halfX * halfX + halfZ * halfZ)));
BodyInertiaInverseTensor.setMELEMENT(2, 2, 12f / (Mass * (halfX * halfX + halfY * halfY)));
}
