/// <summary>
/// Updates rigid body position after solver has computed new velocities
/// </summary>
private void UpdatePositions(float dt)
{
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
MyRigidBody rbo = kvp.Key;
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbo.LinearAcceleration = rbo.ExternalLinearAcceleration;
rbo.AngularAcceleration = rbo.ExternalAngularAcceleration;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
#if PHYSICS_CHECK
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_LinearVelocity.X) == false);
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_Matrix.Translation.X) == false);
#endif
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
/*Vector3 pos, scale, scale2;Quaternion rot;
* rbo.Matrix.Decompose(out scale, out rot, out pos);
* body.m_Matrix.Decompose(out scale2, out rot, out pos);
* rbo.Matrix = Matrix.CreateScale(scale) * Matrix.CreateFromQuaternion(rot) * Matrix.CreateTranslation(pos);*/
rbo.Matrix = body.m_Matrix;
rbo.ApplyDamping(dt);
foreach (var el in rbo.GetRBElementList())
{
el.UpdateAABB();
}
}
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Kinematic)
{
rbo.LinearAcceleration = Vector3.Zero;
rbo.AngularAcceleration = Vector3.Zero;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
rbo.SetMatrix(body.m_Matrix);
}
}
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
if (sc.m_Magnitude != 0.0f)
{
sc.m_RBConstraint.m_Magnitude = sc.m_Magnitude;
}
}
}
