void DynamicBody::TimeStepUpdate(const float timeStep)
{
if (m_enabled) {
m_force += m_externalForce;
m_oldOrient = m_orient;
m_vel += double(timeStep) * m_force * (1.0 / m_mass);
m_angVel += double(timeStep) * m_torque * (1.0 / m_angInertia);
// angvel is always relative to non-rotating frame, so need to counter frame angvel
vector3d consideredAngVel = m_angVel - GetFrame()->GetAngVelocity();
vector3d pos = GetPosition();
// applying angular velocity :-/
{
double len = consideredAngVel.Length();
if (len > 1e-16) {
vector3d rotAxis = consideredAngVel * (1.0 / len);
matrix4x4d rotMatrix = matrix4x4d::RotateMatrix(len * timeStep,
rotAxis.x, rotAxis.y, rotAxis.z);
m_orient = rotMatrix * m_orient;
}
}
m_oldAngDisplacement = consideredAngVel * timeStep;
pos += m_vel * double(timeStep);
m_orient[12] = pos.x;
m_orient[13] = pos.y;
m_orient[14] = pos.z;
TriMeshUpdateLastPos(m_orient);
//if (this->IsType(Object::PLAYER))
//printf("pos = %.1f,%.1f,%.1f, vel = %.1f,%.1f,%.1f, force = %.1f,%.1f,%.1f, external = %.1f,%.1f,%.1f\n",
// pos.x, pos.y, pos.z, m_vel.x, m_vel.y, m_vel.z, m_force.x, m_force.y, m_force.z,
// m_externalForce.x, m_externalForce.y, m_externalForce.z);
m_lastForce = m_force;
m_lastTorque = m_torque;
m_force = vector3d(0.0);
m_torque = vector3d(0.0);
CalcExternalForce(); // regenerate for new pos/vel
} else {
m_oldOrient = m_orient;
m_oldAngDisplacement = vector3d(0.0);
}
}
void DynamicBody::UndoTimestep()
{
m_orient = m_oldOrient;
TriMeshUpdateLastPos(m_orient);
TriMeshUpdateLastPos(m_orient);
}
