void
RigidBody::GetState(StateType& State)
{
// Copy the elements of the member variables that describe the state of
// the rigid body into the state vector.
StateType::iterator it = State.begin();
if (State.size() != m_CountStateVectors)
{
State.resize(m_CountStateVectors);
}
*it++ = m_Position;
*it++ = bnu::column(m_Rotate, 0);
*it++ = bnu::column(m_Rotate, 1);
*it++ = bnu::column(m_Rotate, 2);
*it++ = m_AngularMomentum;
*it = m_LinearMomentum;
}
void
RigidBody::GetStateDerivative(StateType& State)
{
//Work out the state derivatives and place them in State
StateType::iterator it = State.begin();
bnu::vector<double> AngularMomentum;
bnu::vector<double> Temp0;
bnu::vector<double> Temp1;
if (State.size() != m_CountStateVectors)
{
State.resize(m_CountStateVectors);
}
bnu::vector<double> LinearVelocity(m_LinearMomentum/m_Mass);
/* Inertia Tensor = R*IBodyInv*RTranspose*/
bnu::matrix<double> InertiaTensorInv((bnu::prod(m_Rotate, m_BodySpaceInertiaTensorInv)));
InertiaTensorInv = bnu::prod(InertiaTensorInv, bnu::trans(m_Rotate));
/* omega = IInv * angularmomentum*/
bnu::vector<double> Omega(bnu::prod(InertiaTensorInv, m_AngularMomentum));
*it++ = LinearVelocity;
/* work out the derivative of R(t) and copy the result into the state array*/
Temp0 = bnu::column(m_Rotate, 0);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
Temp0 = bnu::column(m_Rotate, 1);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
Temp0 = bnu::column(m_Rotate, 2);
Cross(m_AngularMomentum, Temp0, Temp1);
*it++ = Temp1;
/* copy force and torque into the array */
*it++ = m_Force;
*it++ = m_Torque;
}