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