bool dgDynamicBody::IsInEquilibrium() const
{
if (m_equilibrium) {
dgVector deltaAccel((m_accel - m_prevExternalForce).Scale4(m_invMass.m_w));
dgAssert(deltaAccel.m_w == 0.0f);
dgFloat32 deltaAccel2 = deltaAccel.DotProduct4(deltaAccel).GetScalar();
if (deltaAccel2 > DG_ErrTolerance2) {
return false;
}
dgVector netAccel(m_netForce.Scale4(m_invMass.m_w));
dgAssert(netAccel.m_w == 0.0f);
dgFloat32 netAccel2 = deltaAccel.DotProduct4(deltaAccel).GetScalar();
if (netAccel2 > DG_ErrTolerance2) {
return false;
}
dgVector deltaAlpha(m_matrix.UnrotateVector(m_alpha - m_prevExternalTorque).CompProduct4(m_invMass));
dgAssert(deltaAlpha.m_w == 0.0f);
dgFloat32 deltaAlpha2 = deltaAlpha.DotProduct4(deltaAlpha).GetScalar();
if (deltaAlpha2 > DG_ErrTolerance2) {
return false;
}
dgVector netAlpha(m_matrix.UnrotateVector(m_alpha - m_prevExternalTorque).CompProduct4(m_invMass));
dgAssert(netAlpha.m_w == 0.0f);
dgFloat32 netAlpha2 = netAlpha.DotProduct4(netAlpha).GetScalar();
if (netAlpha2 > DG_ErrTolerance2) {
return false;
}
return true;
}
return false;
}
bool dgDynamicBody::IsInEquilibrium() const
{
if (m_equilibrium) {
dgVector deltaAccel((m_externalForce - m_savedExternalForce).Scale(m_invMass.m_w));
dgAssert(deltaAccel.m_w == dgFloat32 (0.0f));
dgFloat32 deltaAccel2 = deltaAccel.DotProduct(deltaAccel).GetScalar();
if (deltaAccel2 > DG_ERR_TOLERANCE2) {
return false;
}
dgVector deltaAlpha(m_matrix.UnrotateVector(m_externalTorque - m_savedExternalTorque) * m_invMass);
dgAssert(deltaAlpha.m_w == dgFloat32 (0.0f));
dgFloat32 deltaAlpha2 = deltaAlpha.DotProduct(deltaAlpha).GetScalar();
if (deltaAlpha2 > DG_ERR_TOLERANCE2) {
return false;
}
return true;
}
return false;
}
