But when teleporting the bodies to the new, slightly different state, sometimes i get an assert here:
- Code: Select all
void ndBodyDynamic::IntegrateGyroSubstep(const ndVector& timestep)
{
const ndFloat32 omegaMag2 = m_omega.DotProduct(m_omega).GetScalar();
const ndFloat32 tol = (ndFloat32(0.0125f) * ndDegreeToRad);
if (omegaMag2 > (tol * tol))
{
#ifdef D_USE_FULL_INERTIA
ndAssert(0);
#else
const ndFloat32 omegaAngle = ndSqrt(omegaMag2);
const ndVector omegaAxis(m_omega.Scale(ndFloat32(1.0f) / omegaAngle));
const ndQuaternion rotationStep(omegaAxis, omegaAngle * timestep.GetScalar());
m_gyroRotation = m_gyroRotation * rotationStep;
ndAssert((m_gyroRotation.DotProduct(m_gyroRotation).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-5f)); // <-----------
// calculate new Gyro torque and Gyro acceleration
const ndMatrix matrix(m_gyroRotation, ndVector::m_wOne);
#endif
const ndVector localOmega(matrix.UnrotateVector(m_omega));
const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
}
else
{
m_gyroAlpha = ndVector::m_zero;
m_gyroTorque = ndVector::m_zero;
}
}
So i tried to set everything to zero:
- Code: Select all
void TeleportBodiesToPose ()
{
for (int i=0; i<NUM_BODIES; i++)
{
ndBodyDynamic *body = m_bodies[i]->GetAsBodyDynamic();
const auto &pose = m_controller->bodyPose[i];
ndMatrix matrix (ToNdQuat(pose.orn), ToNdPoint(pose.com));
body->SetMatrix(matrix);
body->SetOmega(ndVector(0.f, 0.f, 0.f, 0.f));
body->SetAlpha(ndVector(0.f, 0.f, 0.f, 0.f));
body->SetVelocity(ndVector(0.f, 0.f, 0.f, 0.f));
body->SetAccel(ndVector(0.f, 0.f, 0.f, 0.f));
body->SetForce(ndVector(0.f, 0.f, 0.f, 0.f));
body->SetTorque(ndVector(0.f, 0.f, 0.f, 0.f));
}
}
But did not help, i still get the assert.
Is there something i forgot to set / change?
It's not really a problem, just asking.