void jfMatrix3::copyHere(const jfMatrix3& other)
{
unsigned i;
for (i = 0; i < 9; i++) {
m_Elems[i] = other.getElem(i);
}
}
void jfAeroControlForceGenerator_x86::updateForceFromTensor(jfRigidBody* body,
jfReal timeStep,
jfMatrix3& tensor) const
{
//@REF: Millington code "fgen.cpp"
jfMatrix4_x86 bodyTransformMatrix;
jfVector3_x86 velocity;
jfVector3_x86 bodyVel;
jfVector3_x86 bodyForce;
jfVector3_x86 force;
body->getVelocity(&velocity);
velocity += (*m_WindSpeed);
body->getTransformMatrix(&bodyTransformMatrix);
bodyTransformMatrix.transformInverseDirection(velocity, &bodyVel);
tensor.transform(bodyVel, &bodyForce);
bodyTransformMatrix.transformDirection(bodyForce, &force);
body->addForceAtBodyPoint(force, (*m_Pos));
}
void jfRigidBody_x86::transformInertiaTensor(jfMatrix3* iitWorld
, const jfMatrix3& iitBody
, const jfMatrix4& rotMat) const
{
//@REF:Millington p.203
//Generated using an optimising compiler
jfReal t4 = (rotMat.getElem(0)*iitBody.getElem(0)) +
(rotMat.getElem(1)*iitBody.getElem(3)) +
(rotMat.getElem(2)*iitBody.getElem(6));
jfReal t9 = (rotMat.getElem(0)*iitBody.getElem(1)) +
(rotMat.getElem(1)*iitBody.getElem(4)) +
(rotMat.getElem(2)*iitBody.getElem(7));
jfReal t14 = (rotMat.getElem(0)*iitBody.getElem(2)) +
(rotMat.getElem(1)*iitBody.getElem(5)) +
(rotMat.getElem(2)*iitBody.getElem(8));
jfReal t28 = (rotMat.getElem(4)*iitBody.getElem(0)) +
(rotMat.getElem(5)*iitBody.getElem(3)) +
(rotMat.getElem(6)*iitBody.getElem(6));
jfReal t33 = (rotMat.getElem(4)*iitBody.getElem(1)) +
(rotMat.getElem(5)*iitBody.getElem(4)) +
(rotMat.getElem(6)*iitBody.getElem(7));
jfReal t38 = (rotMat.getElem(4)*iitBody.getElem(2)) +
(rotMat.getElem(5)*iitBody.getElem(5)) +
(rotMat.getElem(6)*iitBody.getElem(8));
jfReal t52 = (rotMat.getElem(8)*iitBody.getElem(0)) +
(rotMat.getElem(9)*iitBody.getElem(3)) +
(rotMat.getElem(10)*iitBody.getElem(6));
jfReal t57 = (rotMat.getElem(8)*iitBody.getElem(1)) +
(rotMat.getElem(9)*iitBody.getElem(4)) +
(rotMat.getElem(10)*iitBody.getElem(7));
jfReal 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)));
}
