void RigidBody::Integrate(float timestep)
{
/*
//Euler Integration
//F/m = a, duh.
//Also : v(t) = P(t) / M
Vector3 linearAccel = m_ConstantAccel;
linearAccel.AddScaledVector(m_ForceAccum, m_InverseMass);
//Transform torque into angular accel
//w(t) = I(t)^-1 . L(t).
Vector3 angularAccel;
m_InverseInertiaTensorWorld.Multiply(m_TorqueAccum, &angularAccel);
//Update linear and angular velocity
m_LinearVel.AddScaledVector(linearAccel, timestep);
m_AngularVel.AddScaledVector(angularAccel, timestep);
//Drag
m_LinearVel = m_LinearVel.Multiply(real_pow(m_LinearDamping, timestep));
m_AngularVel = m_AngularVel.Multiply(real_pow(m_AngularDamping, timestep));
//Update pos from linear vel
m_Pos.AddScaledVector(m_LinearVel, timestep);
//Angular velocity update to orientation
// Vector3 scaledAngularVel = m_AngularVel.Multiply(timestep);
m_Orientation.AddScaledVector(m_AngularVel, timestep);
//Drag
m_LinearVel = m_LinearVel.Multiply(real_pow(m_LinearDamping, timestep));
m_AngularVel = m_AngularVel.Multiply(real_pow(m_AngularDamping, timestep));
*/
/*
Matrix3 w_t;
w_t.SetElem(0, 1);
//w_t.SetElem(0, 0);
w_t.SetElem(1, -scaledAngularVel.GetZ());
w_t.SetElem(2, scaledAngularVel.GetY());
w_t.SetElem(3, scaledAngularVel.GetZ());
w_t.SetElem(4, 1);
//w_t.SetElem(4, 0);
w_t.SetElem(5, -scaledAngularVel.GetX());
w_t.SetElem(6, -scaledAngularVel.GetY());
w_t.SetElem(7, scaledAngularVel.GetX());
w_t.SetElem(8, 1);
//w_t.SetElem(8, 0);
w_t.Multiply(m_Orientation, &m_Orientation);
*/
m_Solver.EulerStep(this, timestep);
CalculateInternals();
ClearAccumulators();
}
void RigidBody::Integrate(double DT)
{
if(!m_isAwake || FloatEquality(m_mass, 0.0f))
return;
m_lastFrameAcceleration = m_acceleration;
m_lastFrameAcceleration += m_forceAccunulator * m_invMass;
Vector3D<Real> angularAcceleration = m_invInertiaTensorWorld * m_torqueAccumulator;
m_velocity += m_lastFrameAcceleration * DT;
m_angularVelocity += angularAcceleration * DT;
m_velocity *= pow(m_linearDamp, DT);
m_angularVelocity *= pow(m_angularDamp, DT);
m_position += m_velocity * DT;
m_orientation.AddScaledVector(m_angularVelocity, DT);
CalculatedDerivedData();
UpdateSleepMode();
ClearAccumulators();
}