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