public bool Insert(MyRigidBody rbo)
{
if (rbo.ReadFlag(RigidBodyFlag.RBF_INSERTED))
{
return(true);
}
#if PHYSICS_CHECK
for (int i = 0; i < m_Rigids.Count; i++)
{
MyRigidBody r = m_Rigids[i];
if (r == rbo)
{
// rbo already inserted!
MyCommonDebugUtils.AssertDebug(false);
return(false);
}
}
#endif
MyCommonDebugUtils.AssertDebug(rbo != null);
// m_Rigids.Add(rbo);
// insert to sort into bp
for (int i = 0; i < rbo.GetRBElementList().Count; i++)
{
MyRBElement elem = rbo.GetRBElementList()[i];
elem.GroupMask = MyGroupMask.Empty;
elem.UpdateAABB();
m_BroadPhase.CreateVolume(elem);
}
if (!rbo.ReadFlag(RigidBodyFlag.RBF_RBO_STATIC))
{
AddActiveRigid(rbo);
rbo.ActivateNotification();
}
rbo.RaiseFlag(RigidBodyFlag.RBF_INSERTED);
return(true);
}
public void Remove(MyRigidBody rbo)
{
//Debug.Assert(!MyPhysics.physicsSystem.GetSensorInteractionModule().IsCheckInteractionsActive(), "You can't deactivate rigid body when check sensor's interactions is active!");
if (rbo == null)
{
return;
}
for (int i = 0; i < rbo.GetRBElementList().Count; i++)
{
MyRBElement elem = rbo.GetRBElementList()[i];
m_BroadPhase.DestroyVolume(elem);
}
RemoveActiveRigid(rbo);
rbo.DeactivateNotification();
// m_Rigids.Remove(rbo);
rbo.ClearFlag(RigidBodyFlag.RBF_INSERTED);
}
public override void DoWork()
{
// brute force
MyRBInteractionModule module = MyPhysics.physicsSystem.GetRBInteractionModule();
List <MyRigidBody> activeRigids = MyPhysics.physicsSystem.GetRigidBodyModule().GetActiveRigids();
m_ActiveElements.Clear();
for (int i = 0; i < activeRigids.Count; i++)
{
MyRigidBody rbo = activeRigids[i];
for (int j = 0; j < rbo.GetRBElementList().Count; j++)
{
MyRBElement el = rbo.GetRBElementList()[j];
el.UpdateAABB();
m_ActiveElements.Add(el);
}
}
// parse the elements
BoundingBox bbox;
MyRBElementInteraction interaction = null;
m_InteractionList.Clear();
for (int i = 0; i < m_ActiveElements.Count; i++)
{
MyRBElement testEl = m_ActiveElements[i];
BoundingBox testAABB = testEl.GetWorldSpaceAABB();
for (int j = 0; j < m_Elements.Count; j++)
{
MyRBElement el = m_Elements[j];
interaction = null;
if (el != testEl)
{
if (el.GetRigidBody().IsStatic() && testEl.GetRigidBody().IsStatic())
{
continue;
}
if (el.GetRigidBody().IsKinematic() && testEl.GetRigidBody().IsKinematic())
{
continue;
}
if (el.GetRigidBody() == testEl.GetRigidBody())
{
continue;
}
bbox = el.GetWorldSpaceAABB();
if (bbox.Intersects(testAABB))
{
interaction = module.FindRBElementInteraction(el, testEl);
if (interaction == null)
{
interaction = module.AddRBElementInteraction(el, testEl);
}
}
else
{
interaction = module.FindRBElementInteraction(el, testEl);
if (interaction != null)
{
interaction = null;
module.RemoveRBElementInteraction(el, testEl);
}
}
if (interaction != null)
{
bool iinserted = false;
for (int t = 0; t < m_InteractionList.Count; t++)
{
if (m_InteractionList[t] == interaction)
{
iinserted = true;
break;
}
}
if (!iinserted)
{
m_InteractionList.Add(interaction);
}
}
}
}
}
}
/// <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>
/// 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;
}
}
}
/// <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);
}
public bool Insert(MyRigidBody rbo)
{
if(rbo.ReadFlag(RigidBodyFlag.RBF_INSERTED))
{
return true;
}
#if PHYSICS_CHECK
for (int i = 0; i < m_Rigids.Count; i++)
{
MyRigidBody r = m_Rigids[i];
if (r == rbo)
{
// rbo already inserted!
MyCommonDebugUtils.AssertDebug(false);
return false;
}
}
#endif
MyCommonDebugUtils.AssertDebug(rbo != null);
// m_Rigids.Add(rbo);
// insert to sort into bp
for (int i = 0; i < rbo.GetRBElementList().Count; i++)
{
MyRBElement elem = rbo.GetRBElementList()[i];
elem.GroupMask = MyGroupMask.Empty;
elem.UpdateAABB();
m_BroadPhase.CreateVolume(elem);
}
if (!rbo.ReadFlag(RigidBodyFlag.RBF_RBO_STATIC))
{
AddActiveRigid(rbo);
rbo.ActivateNotification();
}
rbo.RaiseFlag(RigidBodyFlag.RBF_INSERTED);
return true;
}
public void Remove(MyRigidBody rbo)
{
//Debug.Assert(!MyPhysics.physicsSystem.GetSensorInteractionModule().IsCheckInteractionsActive(), "You can't deactivate rigid body when check sensor's interactions is active!");
if (rbo == null)
{
return;
}
for (int i = 0; i < rbo.GetRBElementList().Count; i++)
{
MyRBElement elem = rbo.GetRBElementList()[i];
m_BroadPhase.DestroyVolume(elem);
}
RemoveActiveRigid(rbo);
rbo.DeactivateNotification();
// m_Rigids.Remove(rbo);
rbo.ClearFlag(RigidBodyFlag.RBF_INSERTED);
}
