void DynamicBody::TimeStepUpdate(const float timeStep)
{
m_oldPos = GetPosition();
if (m_isMoving) {
m_force += m_externalForce;
m_vel += double(timeStep) * m_force * (1.0 / m_mass);
m_angVel += double(timeStep) * m_torque * (1.0 / m_angInertia);
double len = m_angVel.Length();
if (len > 1e-16) {
vector3d axis = m_angVel * (1.0 / len);
matrix3x3d r = matrix3x3d::Rotate(len * timeStep, axis);
SetOrient(r * GetOrient());
}
m_oldAngDisplacement = m_angVel * timeStep;
SetPosition(GetPosition() + m_vel * double(timeStep));
//if (this->IsType(Object::PLAYER))
//Output("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_oldAngDisplacement = vector3d(0.0);
}
ModelBody::TimeStepUpdate(timeStep);
}
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::PostLoadFixup()
{
CalcExternalForce();
}
void DynamicBody::PostLoadFixup(Space *space)
{
CalcExternalForce();
}
void DynamicBody::PostLoadFixup(Space *space)
{
Body::PostLoadFixup(space);
m_oldPos = GetPosition();
if (GetFrame()) CalcExternalForce(); // not for hyperspace
}
