void TaylorScheme::UpdateRotationalVariablesOfCluster(
const Node < 3 > & i,
const array_1d<double, 3 >& moments_of_inertia,
array_1d<double, 3 >& rotated_angle,
array_1d<double, 3 >& delta_rotation,
Quaternion<double >& Orientation,
const array_1d<double, 3 >& angular_momentum,
array_1d<double, 3 >& angular_velocity,
const double delta_t,
const bool Fix_Ang_vel[3]) {
for (int k = 0; k < 3; k++) {
delta_rotation[k] = angular_velocity[k] * delta_t;
rotated_angle[k] += delta_rotation[k];
}
array_1d<double, 3 > angular_velocity_aux;
double LocalTensorInv[3][3];
GeometryFunctions::ConstructInvLocalTensor(moments_of_inertia, LocalTensorInv);
GeometryFunctions::UpdateOrientation(Orientation, delta_rotation);
UpdateAngularVelocity(Orientation, LocalTensorInv, angular_momentum, angular_velocity_aux);
for (int j = 0; j < 3; j++) {
if (Fix_Ang_vel[j] == false){
angular_velocity[j] = angular_velocity_aux[j];
}
}
}
/*
* \brief Set a rigid bodies bounding box
*/
void CSpaghettiRigidBody::SetBounds(
CSpaghettiBounds *boundingBox //!< The bounding box to set this bodies bounding box too
)
{
m_bounds = boundingBox;
if (m_bounds != nullptr)
{
m_bounds->SetBody(this);
m_bounds->Transform(m_position, m_rotation);
}
CalculateInertiaBodyTensor();
UpdateInertiaTensor();
UpdateAngularVelocity();
}
/*
* \brief Update all simulation objects in the world
*/
void CSpaghettiRigidBody::UpdateVelocity(
const CSpaghettiWorld *world,
const float deltaTime //!< Delta time (The amount of time past since the last update)
)
{
// Nothing to update if he rigid body is static or disabled
if (m_flags.isStatic || !m_flags.isEnabled)
return;
// add gravity
AddForceAtPoint(world->GetGravity(), m_position);
// update velocity
m_velocity = m_velocity + (m_force * deltaTime);
// update the position of the body
m_lastPosition = m_position;
m_position = m_position + (m_velocity * deltaTime);
// update angular momentum
m_angularMomentum = m_angularMomentum + (m_torque * deltaTime);
m_angularMomentum *= 0.95f;
UpdateInertiaTensor();
UpdateAngularVelocity();
// construct the skew matrix
Ogre::Matrix3 skewMatrix;
skewMatrix[0][0] = 0.0f;
skewMatrix[0][1] = -m_angularVelocity.z * 0.17f;
skewMatrix[0][2] = m_angularVelocity.y * 0.17f;
skewMatrix[1][0] = m_angularVelocity.z * 0.17f;
skewMatrix[1][1] = 0.0f;
skewMatrix[1][2] = -m_angularVelocity.x * 0.17f;
skewMatrix[2][0] = -m_angularVelocity.y * 0.17f;
skewMatrix[2][1] = m_angularVelocity.x * 0.17f;
skewMatrix[2][2] = 0.0f;
// update rotation matrix
m_rotation = m_rotation + ((skewMatrix * deltaTime) * m_rotation);
// update and normalize the quaternion
m_quaternion.FromRotationMatrix(m_rotation);
m_quaternion.normalise();
// stop it crashing...
if (m_quaternion.isNaN())
{
m_quaternion = Ogre::Quaternion::IDENTITY;
m_rotation = Ogre::Matrix3::IDENTITY;
}
// transform the bounding box data
m_bounds->Transform(m_position, m_quaternion);
// Zero out force and torque
m_force = Ogre::Vector3::ZERO;
m_torque = Ogre::Vector3::ZERO;
}
