/// <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; } } }