/// <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;
}
/// <summary>
/// Computes the inertia tensor of a rigid body using box inertia definition
/// </summary>
public static void ComputeIntertiaTensor(MyRigidBody rbo)
{
MyCommonDebugUtils.AssertDebug(rbo != null);
MyCommonDebugUtils.AssertDebug(rbo.GetRBElementList().Count > 0);
MyCommonDebugUtils.AssertDebug(rbo.GetMass() > 0);
float mass = rbo.GetMass();
BoundingBox box;
box.Min = new Vector3(FLT_MAX);
box.Max = new Vector3(FLT_MIN);
BoundingBox aabb;
Matrix infTensor = new Matrix();
infTensor.M11 = FLT_MAX;
infTensor.M22 = FLT_MAX;
infTensor.M33 = FLT_MAX;
infTensor.M44 = 1.0f;
if (rbo.IsStatic())
{
rbo.InertiaTensor = infTensor;
return;
}
if (rbo.GetRBElementList().Count > 1)
{
for (int e = 0; e < rbo.GetRBElementList().Count; e++)
{
MyRBElement el = rbo.GetRBElementList()[e];
switch (el.GetElementType())
{
case MyRBElementType.ET_TRIANGLEMESH:
{
rbo.InertiaTensor = infTensor;
return;
}
break;
case MyRBElementType.ET_VOXEL:
{
rbo.InertiaTensor = infTensor;
return;
}
break;
default:
{
aabb = el.GetWorldSpaceAABB();
box = BoundingBox.CreateMerged(box, aabb);
}
break;
}
}
Vector3 size = box.Max - box.Min;
infTensor.M11 = mass * (size.Y * size.Y + size.Z * size.Z) / 12.0f;
infTensor.M22 = mass * (size.X * size.X + size.Z * size.Z) / 12.0f;
infTensor.M33 = mass * (size.X * size.X + size.Y * size.Y) / 12.0f;
infTensor.M44 = 1.0f;
rbo.InertiaTensor = infTensor;
rbo.InvertInertiaTensor = Matrix.Invert(infTensor);
return;
}
MyRBElement elem = rbo.GetRBElementList()[0];
switch (elem.GetElementType())
{
case MyRBElementType.ET_TRIANGLEMESH:
{
rbo.InertiaTensor = infTensor;
infTensor.M11 = 0.0f;
infTensor.M22 = 0.0f;
infTensor.M33 = 0.0f;
infTensor.M44 = 0.0f;
rbo.InvertInertiaTensor = infTensor;
return;
}
break;
case MyRBElementType.ET_VOXEL:
{
rbo.InertiaTensor = infTensor;
infTensor.M11 = 0.0f;
infTensor.M22 = 0.0f;
infTensor.M33 = 0.0f;
infTensor.M44 = 0.0f;
rbo.InvertInertiaTensor = infTensor;
return;
}
case MyRBElementType.ET_SPHERE:
{
float radius = ((MyRBSphereElement)elem).Radius;
infTensor.M11 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M22 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M33 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M44 = 1.0f;
rbo.InertiaTensor = infTensor;
//rbo.InvertInertiaTensor = Matrix.Invert(infTensor);
return;
}
break;
case MyRBElementType.ET_BOX:
{
//Vector3 size = ((MyRBBoxElement)elem).Size;
//infTensor.M11 = mass * (size.Y * size.Y + size.Z * size.Z) / 12.0f;
//infTensor.M22 = mass * (size.X * size.X + size.Z * size.Z) / 12.0f;
//infTensor.M33 = mass * (size.X * size.X + size.Y * size.Y) / 12.0f;
//infTensor.M44 = 1.0f;
//rbo.InertiaTensor = infTensor;
//rbo.InvertInertiaTensor = Matrix.Invert(infTensor);
// HACK: After speaking with PetrM, computing changed like box is sphere
float radius = ((MyRBBoxElement)elem).Size.Length() / 2;
infTensor.M11 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M22 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M33 = 2.0f / 5.0f * mass * radius * radius;
infTensor.M44 = 1.0f;
rbo.InertiaTensor = infTensor;
//rbo.InvertInertiaTensor = Matrix.Invert(infTensor);
return;
}
break;
default:
MyCommonDebugUtils.AssertDebug(false);
break;
}
}
/// <summary>
/// Adding rigid body recursively to check for constraint connections and make sure that its only in 1 island
/// </summary>
private void AddRigidBody(MyRigidBody rbo, MyRigidBody secondRigidBody, MyRigidBodyIsland addIsland)
{
if (rbo.IsStatic())
{
rbo.PutToSleep();
return;
}
if (!m_proccesedList.Add(rbo))
{
return;
}
// add rigid bodies to island recursively
int numInteractions = 0;
for (int j = 0; j < rbo.GetRBElementList().Count; j++)
{
MyRBElement el = rbo.GetRBElementList()[j];
numInteractions += el.GetRBElementInteractions().Count;
for (int k = 0; k < el.GetRBElementInteractions().Count; k++)
{
if (addIsland == null && !rbo.IsStatic())
{
addIsland = m_islandsPool.Allocate();
addIsland.Clear();
addIsland.IterationCount = 0;
addIsland.AddRigidBody(rbo);
m_islands.Add(addIsland);
}
else
{
if (!rbo.IsStatic())
{
addIsland.AddRigidBody(rbo);
}
}
MyRBElementInteraction intr = el.GetRBElementInteractions()[k];
if (intr.GetRigidBody1() != rbo && intr.GetRigidBody2() != secondRigidBody)
{
AddRigidBody(intr.GetRigidBody1(), rbo, addIsland);
}
if (intr.GetRigidBody2() != rbo && intr.GetRigidBody1() != secondRigidBody)
{
AddRigidBody(intr.GetRigidBody2(), rbo, addIsland);
}
}
}
// isolated rbo
if (numInteractions == 0 && !rbo.IsStatic())
{
MyRigidBodyIsland island = m_islandsPool.Allocate();
island.Clear();
island.IterationCount = 0;
island.AddRigidBody(rbo);
m_islands.Add(island);
}
}
/// <summary>
/// Adding rigid body recursively to check for constraint connections and make sure that its only in 1 island
/// </summary>
private void AddRigidBody(MyRigidBody rbo, MyRigidBody secondRigidBody,MyRigidBodyIsland addIsland)
{
if(rbo.IsStatic())
{
rbo.PutToSleep();
return;
}
if (!m_proccesedList.Add(rbo))
return;
// add rigid bodies to island recursively
int numInteractions = 0;
for (int j = 0; j < rbo.GetRBElementList().Count; j++)
{
MyRBElement el = rbo.GetRBElementList()[j];
numInteractions += el.GetRBElementInteractions().Count;
for (int k = 0; k < el.GetRBElementInteractions().Count; k++)
{
if (addIsland == null && !rbo.IsStatic())
{
addIsland = m_islandsPool.Allocate();
addIsland.Clear();
addIsland.IterationCount = 0;
addIsland.AddRigidBody(rbo);
m_islands.Add(addIsland);
}
else
{
if(!rbo.IsStatic())
{
addIsland.AddRigidBody(rbo);
}
}
MyRBElementInteraction intr = el.GetRBElementInteractions()[k];
if(intr.GetRigidBody1() != rbo && intr.GetRigidBody2() != secondRigidBody)
{
AddRigidBody(intr.GetRigidBody1(),rbo,addIsland);
}
if (intr.GetRigidBody2() != rbo && intr.GetRigidBody1() != secondRigidBody)
{
AddRigidBody(intr.GetRigidBody2(), rbo, addIsland);
}
}
}
// isolated rbo
if (numInteractions == 0 && !rbo.IsStatic())
{
MyRigidBodyIsland island = m_islandsPool.Allocate();
island.Clear();
island.IterationCount = 0;
island.AddRigidBody(rbo);
m_islands.Add(island);
}
}
/// <summary>
/// Checks the sleep state of rigids and decides if its possible to sleep the whole island
/// </summary>
private void UpdateSleepState()
{
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("GetIslands");
List <MyRigidBodyIsland> islands = MyPhysics.physicsSystem.GetRigidBodyModule().GetRigidBodyIslandGeneration().GetIslands();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Islands");
int count = 0;
for (int i = 0; i < islands.Count; i++)
{
MyRigidBodyIsland island = islands[i];
bool canDeactivate = true;
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Rigids");
for (int j = 0; j < island.GetRigids().Count; j++)
{
count++;
MyRigidBody rbo = island.GetRigids()[j];
if (rbo.IsStatic())
{
rbo.PutToSleep();
MyPhysics.physicsSystem.GetRigidBodyModule().RemoveActiveRigid(rbo);
}
else
{
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("MoveVolumeFast");
foreach (var el in rbo.GetRBElementList())
{
m_Broadphase.MoveVolumeFast(el);
}
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("NotifyMotionHandler");
if (rbo.NotifyMotionHandler != null)
{
rbo.NotifyMotionHandler.OnMotion(rbo, dt);
}
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
// dynamic rigids use different approach
if (!rbo.CanDeactivate())
{
canDeactivate = false;
}
}
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("canDeactivate");
// check if all are asleep
if (canDeactivate)
{
foreach (var rbo in island.GetRigids())
{
rbo.PutToSleep();
MyPhysics.physicsSystem.GetRigidBodyModule().RemoveActiveRigid(rbo);
}
}
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("UpdateSleepState", count);
}
