void Matrix3::Add(const Matrix3& other, Matrix3* result) const
{
result->SetElem(0, m_Elems[0] + other.GetElem(0));
result->SetElem(1, m_Elems[1] + other.GetElem(1));
result->SetElem(2, m_Elems[2] + other.GetElem(2));
result->SetElem(3, m_Elems[3] + other.GetElem(3));
result->SetElem(4, m_Elems[4] + other.GetElem(4));
result->SetElem(5, m_Elems[5] + other.GetElem(5));
result->SetElem(6, m_Elems[6] + other.GetElem(6));
result->SetElem(7, m_Elems[7] + other.GetElem(7));
result->SetElem(8, m_Elems[8] + other.GetElem(8));
}
void Matrix3::SetTranspose(const Matrix3& other)
{
real a11 = other.GetElem(0);
real a12 = other.GetElem(1);
real a13 = other.GetElem(2);
real a21 = other.GetElem(3);
real a22 = other.GetElem(4);
real a23 = other.GetElem(5);
real a31 = other.GetElem(6);
real a32 = other.GetElem(7);
real a33 = other.GetElem(8);
m_Elems[0] = a11;
m_Elems[1] = a21;
m_Elems[2] = a31;
m_Elems[3] = a12;
m_Elems[4] = a22;
m_Elems[5] = a32;
m_Elems[6] = a13;
m_Elems[7] = a23;
m_Elems[8] = a33;
}
void RigidBody::TransformInertiaTensor(Matrix3* iitWorld
, const Matrix3& iitBody
, const Matrix3& rotMat) const
{
/*
//I(t) = R(t) . I_body . R(t)^T
Matrix3 rotatedIit;
rotMat.Multiply(iitBody, &rotatedIit);
Matrix3 transRotMat;
rotMat.GetTranspose(&transRotMat);
rotatedIit.Multiply(transRotMat, iitWorld);
*/
//Generated using an optimising compiler
real t4 = (rotMat.GetElem(0)*iitBody.GetElem(0)) +
(rotMat.GetElem(1)*iitBody.GetElem(3)) +
(rotMat.GetElem(2)*iitBody.GetElem(6));
real t9 = (rotMat.GetElem(0)*iitBody.GetElem(1)) +
(rotMat.GetElem(1)*iitBody.GetElem(4)) +
(rotMat.GetElem(2)*iitBody.GetElem(7));
real t14 = (rotMat.GetElem(0)*iitBody.GetElem(2)) +
(rotMat.GetElem(1)*iitBody.GetElem(5)) +
(rotMat.GetElem(2)*iitBody.GetElem(8));
real t28 = (rotMat.GetElem(4)*iitBody.GetElem(0)) +
(rotMat.GetElem(5)*iitBody.GetElem(3)) +
(rotMat.GetElem(6)*iitBody.GetElem(6));
real t33 = (rotMat.GetElem(4)*iitBody.GetElem(1)) +
(rotMat.GetElem(5)*iitBody.GetElem(4)) +
(rotMat.GetElem(6)*iitBody.GetElem(7));
real t38 = (rotMat.GetElem(4)*iitBody.GetElem(2)) +
(rotMat.GetElem(5)*iitBody.GetElem(5)) +
(rotMat.GetElem(6)*iitBody.GetElem(8));
real t52 = (rotMat.GetElem(8)*iitBody.GetElem(0)) +
(rotMat.GetElem(9)*iitBody.GetElem(3)) +
(rotMat.GetElem(10)*iitBody.GetElem(6));
real t57 = (rotMat.GetElem(8)*iitBody.GetElem(1)) +
(rotMat.GetElem(9)*iitBody.GetElem(4)) +
(rotMat.GetElem(10)*iitBody.GetElem(7));
real t62 = (rotMat.GetElem(8)*iitBody.GetElem(2)) +
(rotMat.GetElem(9)*iitBody.GetElem(5)) +
(rotMat.GetElem(10)*iitBody.GetElem(8));
iitWorld->SetElem(0, (t4*rotMat.GetElem(0)) +
(t9*rotMat.GetElem(1)) +
(t14*rotMat.GetElem(2)));
iitWorld->SetElem(1, (t4*rotMat.GetElem(4)) +
(t9*rotMat.GetElem(5)) +
(t14*rotMat.GetElem(6)));
iitWorld->SetElem(2, (t4*rotMat.GetElem(8)) +
(t9*rotMat.GetElem(9)) +
(t14*rotMat.GetElem(10)));
iitWorld->SetElem(3, (t28*rotMat.GetElem(0)) +
(t33*rotMat.GetElem(1)) +
(t38*rotMat.GetElem(2)));
iitWorld->SetElem(4, (t28*rotMat.GetElem(4)) +
(t33*rotMat.GetElem(5)) +
(t38*rotMat.GetElem(6)));
iitWorld->SetElem(5, (t28*rotMat.GetElem(8)) +
(t33*rotMat.GetElem(9)) +
(t38*rotMat.GetElem(10)));
iitWorld->SetElem(6, (t52*rotMat.GetElem(0)) +
(t57*rotMat.GetElem(1)) +
(t62*rotMat.GetElem(2)));
iitWorld->SetElem(7, (t52*rotMat.GetElem(4)) +
(t57*rotMat.GetElem(5)) +
(t62*rotMat.GetElem(6)));
iitWorld->SetElem(8, (t52*rotMat.GetElem(8)) +
(t57*rotMat.GetElem(9)) +
(t62*rotMat.GetElem(10)));
}
void Matrix3::Multiply(const Matrix3& other, Matrix3* result) const
{
Matrix3 tempResult;
tempResult.SetElem(0, (m_Elems[0] * other.GetElem(0)) +
(m_Elems[1] * other.GetElem(3)) +
(m_Elems[2] * other.GetElem(6)));
tempResult.SetElem(1, (m_Elems[0] * other.GetElem(1)) +
(m_Elems[1] * other.GetElem(4)) +
(m_Elems[2] * other.GetElem(7)));
tempResult.SetElem(2, (m_Elems[0] * other.GetElem(2)) +
(m_Elems[1] * other.GetElem(5)) +
(m_Elems[2] * other.GetElem(8)));
tempResult.SetElem(3, (m_Elems[3] * other.GetElem(0)) +
(m_Elems[4] * other.GetElem(3)) +
(m_Elems[5] * other.GetElem(6)));
tempResult.SetElem(4, (m_Elems[3] * other.GetElem(1)) +
(m_Elems[4] * other.GetElem(4)) +
(m_Elems[5] * other.GetElem(7)));
tempResult.SetElem(5, (m_Elems[3] * other.GetElem(2)) +
(m_Elems[4] * other.GetElem(5)) +
(m_Elems[5] * other.GetElem(8)));
tempResult.SetElem(6, (m_Elems[6] * other.GetElem(0)) +
(m_Elems[7] * other.GetElem(3)) +
(m_Elems[8] * other.GetElem(6)));
tempResult.SetElem(7, (m_Elems[6] * other.GetElem(1)) +
(m_Elems[7] * other.GetElem(4)) +
(m_Elems[8] * other.GetElem(7)));
tempResult.SetElem(8, (m_Elems[6] * other.GetElem(2)) +
(m_Elems[7] * other.GetElem(5)) +
(m_Elems[8] * other.GetElem(8)));
(*result) = tempResult;
}
