dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
// this is correct
dQuaternion rotation (*this);
dFloat omegaMag2 = omega % omega;
const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
const dFloat errAngle2 = errAngle * errAngle;
if (omegaMag2 > errAngle2) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion deltaRotation (omegaAxis, omegaAngle);
rotation = rotation * deltaRotation;
rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
}
return rotation;
}
void dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale3 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale3 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
#ifdef _DEBUG
for (dgInt32 i = 0; i < 4; i ++) {
for (dgInt32 j = 0; j < 4; j ++) {
dgAssert (dgCheckFloat(m_matrix[i][j]));
}
}
dgInt32 j0 = 1;
dgInt32 j1 = 2;
for (dgInt32 i = 0; i < 3; i ++) {
dgAssert (m_matrix[i][3] == 0.0f);
dgFloat32 val = m_matrix[i] % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
dgVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dgAssert (dgAbsf (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
void dComplentaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
{
const dFloat D_MAX_ANGLE_STEP = dFloat (45.0f * 3.141592f / 180.0f);
const dFloat D_ANGULAR_TOL = dFloat (0.0125f * 3.141592f / 180.0f);
m_globalCentreOfMass += m_veloc.Scale (timestep);
while (((m_omega % m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
m_omega = m_omega.Scale (dFloat (0.8f));
}
// this is correct
dFloat omegaMag2 = m_omega % m_omega;
if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (m_omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion rotation (omegaAxis, omegaAngle);
dQuaternion rotMatrix (m_matrix);
rotMatrix = rotMatrix * rotation;
rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
#ifdef _DEBUG
int j0 = 1;
int j1 = 2;
for (int i = 0; i < 3; i ++) {
dAssert (m_matrix[i][3] == 0.0f);
dFloat val = m_matrix[i] % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
dVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
void dgBody::IntegrateVelocity (dgFloat32 timestep)
{
m_globalCentreOfMass += m_veloc.Scale3 (timestep);
while (((m_omega % m_omega) * timestep * timestep) > m_maxAngulaRotationPerSet2) {
m_omega = m_omega.Scale4 (dgFloat32 (0.8f));
}
// this is correct
dgFloat32 omegaMag2 = m_omega % m_omega;
if (omegaMag2 > ((dgFloat32 (0.0125f) * dgDEG2RAD) * (dgFloat32 (0.0125f) * dgDEG2RAD))) {
dgFloat32 invOmegaMag = dgRsqrt (omegaMag2);
dgVector omegaAxis (m_omega.Scale4 (invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep;
dgQuaternion rotation (omegaAxis, omegaAngle);
m_rotation = m_rotation * rotation;
m_rotation.Scale(dgRsqrt (m_rotation.DotProduct (m_rotation)));
m_matrix = dgMatrix (m_rotation, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localCentreOfMass);
dgAssert (m_matrix.TestOrthogonal());
}
