/// <summary> /// Creates a solver body from a rbo and prepares solver data /// </summary> private MyRBSolverBody AddRigidBody(MyRigidBody rbo) { MyRBSolverBody sb = m_SolverBodiesPool.Allocate(); sb.Clear(); sb.m_RigidBody = rbo; sb.m_Matrix = rbo.Matrix; sb.m_LinearVelocity = rbo.LinearVelocity; sb.m_AngularVelocity = rbo.AngularVelocity; MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.AngularVelocity); sb.m_LinearAcceleration = rbo.ExternalLinearAcceleration; sb.m_AngularAcceleration = rbo.ExternalAngularAcceleration; MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.ExternalAngularAcceleration); sb.m_MaxAngularVelocity = rbo.MaxAngularVelocity; sb.m_MaxLinearVelocity = rbo.MaxLinearVelocity; if (rbo.IsStatic() || rbo.IsKinematic()) { sb.m_OneOverMass = 0; sb.m_InertiaTensor = MyPhysicsUtils.ZeroInertiaTensor; sb.m_InvertedInertiaTensor = MyPhysicsUtils.ZeroInertiaTensor; if (rbo.IsStatic()) { sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Static; } else { sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Kinematic; } } else { sb.m_OneOverMass = rbo.GetOneOverMass(); Matrix matrix = rbo.Matrix; matrix.Translation = Vector3.Zero; sb.m_InertiaTensor = Matrix.Transpose(matrix) * rbo.InertiaTensor * matrix; // not sure if I can use directly inverted sb.m_InvertedInertiaTensor = Matrix.Transpose(matrix) * rbo.InvertInertiaTensor * matrix; sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Dynamic; } m_SolverBodies.Add(rbo, sb); return(sb); }
/// <summary> /// Collects all constraints /// </summary> private void PrepareConstraints() { var cl = MyPhysics.physicsSystem.GetContactConstraintModule().GetActiveRBContactConstraints(); for (int i = 0; i < cl.Count; i++) { MyRBContactConstraint cc = cl[i]; MyRigidBody testRbo = cc.GetRBElementInteraction().GetRigidBody1(); if (testRbo.IsStatic() || testRbo.IsKinematic()) { testRbo = cc.GetRBElementInteraction().GetRigidBody2(); } for (int j = 0; j < m_Island.GetRigids().Count; j++) { MyRigidBody rbo = m_Island.GetRigids()[j]; if (rbo == testRbo) { AddConstraint(cc); break; } } } }
/// <summary> /// Creates a solver body from a rbo and prepares solver data /// </summary> private MyRBSolverBody AddRigidBody(MyRigidBody rbo) { MyRBSolverBody sb = m_SolverBodiesPool.Allocate(); sb.Clear(); sb.m_RigidBody = rbo; sb.m_Matrix = rbo.Matrix; sb.m_LinearVelocity = rbo.LinearVelocity; sb.m_AngularVelocity = rbo.AngularVelocity; MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.AngularVelocity); sb.m_LinearAcceleration = rbo.ExternalLinearAcceleration; sb.m_AngularAcceleration = rbo.ExternalAngularAcceleration; MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.ExternalAngularAcceleration); sb.m_MaxAngularVelocity = rbo.MaxAngularVelocity; sb.m_MaxLinearVelocity = rbo.MaxLinearVelocity; if (rbo.IsStatic() || rbo.IsKinematic()) { sb.m_OneOverMass = 0; sb.m_InertiaTensor = MyPhysicsUtils.ZeroInertiaTensor; sb.m_InvertedInertiaTensor = MyPhysicsUtils.ZeroInertiaTensor; if (rbo.IsStatic()) { sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Static; } else { sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Kinematic; } } else { sb.m_OneOverMass = rbo.GetOneOverMass(); Matrix matrix = rbo.Matrix; matrix.Translation = Vector3.Zero; sb.m_InertiaTensor = Matrix.Transpose(matrix) * rbo.InertiaTensor * matrix; // not sure if I can use directly inverted sb.m_InvertedInertiaTensor = Matrix.Transpose(matrix) * rbo.InvertInertiaTensor * matrix; sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Dynamic; } m_SolverBodies.Add(rbo, sb); return sb; }