public void DestroyRigidBody(MyRigidBody rbo)
{
if ((rbo.ReadFlag(RigidBodyFlag.RBF_INSERTED)))
{
MyPhysics.physicsSystem.GetRigidBodyModule().Remove(rbo);
}
m_RigidsPool.Deallocate(rbo);
}
public MyRBElement()
{
m_RBMaterial = new MyRBMaterial(0.5f, 0.5f, 0.7f, 0);
m_RigidBody = null;
m_ElementInteractions = new List<MyRBElementInteraction>(2);
Flags = MyElementFlag.EF_RB_ELEMENT | MyElementFlag.EF_AABB_DIRTY;
m_GroupMask = new MyGroupMask();
}
/// <summary>
/// Called when rigid body enters sensor.
/// </summary>
/// <param name="rbo">Rigid body that entered.</param>
public void OnEnter(MySensor sensor, MyRigidBody rbo, MyRBElement rbElement)
{
//smallship
var userData = rbo.m_UserData;
var physicsBody = userData as MyPhysicsBody;
if (physicsBody != null && physicsBody.Entity is MySmallShip)
{
m_counter++;
if (m_counter > 0)
m_owner.OrderToOpen();
}
}
/// <summary>
/// Called when rigid body leaves sensor.
/// </summary>
/// <param name="rbo">Rigid body that left.</param>
public void OnLeave(MySensor sensor, MyRigidBody rbo, MyRBElement rbElement)
{
////TODO: Temporary solution - there must not be rbo==null, fix the error and change to assert
//if (rbo == null)
// return;
Debug.Assert(rbo != null);
//smallship
var userData = rbo.m_UserData;
var physicsBody = userData as MyPhysicsBody;
if (physicsBody != null && physicsBody.Entity is MySmallShip)
{
m_counter--;
if (m_counter <= 0)
m_owner.OrderToClose();
}
}
/// <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);
}
public void AddRigidBody(MyRigidBody rbo)
{
// make the add unique
for (int i = 0; i < m_Rigids.Count; i++)
{
if (m_Rigids[i] == rbo)
{
return;
}
}
if (rbo.IterationCount > m_IterationCount)
{
m_IterationCount = rbo.IterationCount;
}
m_Rigids.Add(rbo);
}
public MyRigidBody CreateRigidBody(MyRigidBodyDesc desc)
{
if (!desc.IsValid())
{
// invalid desc
MyCommonDebugUtils.AssertDebug(false);
return(null);
}
MyRigidBody rbo = m_RigidsPool.Allocate();
MyCommonDebugUtils.AssertDebug(rbo != null);
rbo.LoadFromDesc(desc);
return(rbo);
}
public void RemoveActiveRigid(MyRigidBody rbo)
{
if (rbo == null)
{
return;
}
if (!rbo.ReadFlag(RigidBodyFlag.RBF_ACTIVE))
{
return;
}
//if (rbo.RigidBodyEventHandler != null)
//{
// rbo.RigidBodyEventHandler.OnDeactivated();
//}
rbo.ClearFlag(RigidBodyFlag.RBF_ACTIVE);
if (rbo.ReadFlag(RigidBodyFlag.RBF_KINEMATIC))
{
List <MyRigidBodyIsland> islands = MyPhysics.physicsSystem.GetRigidBodyModule().GetRigidBodyIslandGeneration().GetIslands();
for (int i = 0; i < islands.Count; i++)
{
MyRigidBodyIsland island = islands[i];
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Rigids");
for (int j = 0; j < island.GetRigids().Count; j++)
{
MyRigidBody rboi = island.GetRigids()[j];
if (rbo == rboi)
{
island.RemoveRigidBody(rbo);
break;
}
}
}
}
bool found = m_ActiveRigids.Remove(rbo);
//RBO wasnt in the list
Debug.Assert(found);
}
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 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;
}
/// <summary>
/// internal add of to active rigid list
/// </summary>
public void AddActiveRigid(MyRigidBody rbo)
{
if (rbo == null)
{
return;
}
if (rbo.ReadFlag(RigidBodyFlag.RBF_ACTIVE))
{
return;
}
//if (rbo.RigidBodyEventHandler != null)
//{
// rbo.RigidBodyEventHandler.OnActivated();
//}
rbo.RaiseFlag(RigidBodyFlag.RBF_ACTIVE);
m_ActiveRigids.Add(rbo);
}
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);
}
/// <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 and prepare the solver constraint
/// </summary>
private void AddConstraint(MyRBContactConstraint rbc)
{
if (rbc.GetRBElementInteraction().GetRigidBody1().IsStatic() && rbc.GetRBElementInteraction().GetRigidBody2().IsStatic())
{
return;
}
for (int i = 0; i < rbc.m_NumCollPts; i++)
{
MyRBSolverConstraint rbsc = m_SolverConstaintPool.Allocate();
rbsc.Clear();
MyCollPointInfo pointInfo = rbc.m_PointInfo[i];
MyRBSolverBody body1 = null;
MyRBSolverBody body2 = null;
MyRigidBody rbo1 = rbc.GetRBElementInteraction().GetRigidBody1();
MyRigidBody rbo2 = rbc.GetRBElementInteraction().GetRigidBody2();
if (!m_SolverBodies.TryGetValue(rbo1, out body1))
{
body1 = AddRigidBody(rbo1);
}
if (!m_SolverBodies.TryGetValue(rbo2, out body2))
{
body2 = AddRigidBody(rbo2);
}
if ((rbo1.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
if ((rbo2.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
rbsc.m_SolverBody1 = body1;
rbsc.m_SolverBody2 = body2;
MyRBMaterial material1 = rbc.GetRBElementInteraction().RBElement1.RBElementMaterial;
MyRBMaterial material2 = rbc.GetRBElementInteraction().RBElement2.RBElementMaterial;
float restitution = material1.NominalRestitution * material2.NominalRestitution;
rbsc.m_RBConstraint = rbc;
rbsc.m_Magnitude = rbc.Magnitude * m_SolutionDamping;
//This is a contact constraint
Vector3 globalVel1 = new Vector3();
Vector3 globalVel2 = new Vector3();
rbc.GetRBElementInteraction().GetRigidBody1().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel1);
rbc.GetRBElementInteraction().GetRigidBody2().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel2);
Vector3 relativeVelocity = globalVel2 - globalVel1;
float aRelVel = Vector3.Dot(pointInfo.m_Info.m_Normal, relativeVelocity);
float diff = aRelVel;
rbsc.m_Target = diff >= 0.0f ? 0.0f : (-diff * restitution);
if (pointInfo.m_Info.m_InitialPenetration < -0.01f)
{
rbsc.m_Target -= pointInfo.m_Info.m_InitialPenetration * m_DepenetrationCoeficient;
}
rbsc.m_Restitution = restitution;
rbsc.m_Normal = pointInfo.m_Info.m_Normal;
rbsc.m_ContactPoint = pointInfo.m_Info.m_WorldPosition;
rbsc.m_Body1LocalPoint = pointInfo.m_Info.m_R0;
rbsc.m_Body2LocalPoint = pointInfo.m_Info.m_R1;
Vector3 cross1 = Vector3.Cross(pointInfo.m_Info.m_R0, pointInfo.m_Info.m_Normal);
Vector3 cross2 = Vector3.Cross(pointInfo.m_Info.m_R1, pointInfo.m_Info.m_Normal);
rbsc.m_Body1LocalPointCrossedNormal = Vector3.Transform(cross1, body1.m_InvertedInertiaTensor);
rbsc.m_Body2LocalPointCrossedNormal = Vector3.Transform(cross2, body2.m_InvertedInertiaTensor);
rbsc.m_StaticFriction = material1.NominalStaticFriction * material2.NominalStaticFriction;
rbsc.m_DynamicFriction = material1.NominalDynamicFriction * material2.NominalDynamicFriction;
rbsc.m_Affection = 0.0f;
if (body1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body1.m_OneOverMass + Vector3.Cross(rbsc.m_Body1LocalPointCrossedNormal, rbsc.m_Body1LocalPoint)));
body1.m_LinearVelocity -= rbsc.m_Normal * (body1.m_OneOverMass * rbsc.m_Magnitude);
body1.m_AngularVelocity -= rbsc.m_Body1LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body1.m_AngularVelocity);
}
if (body2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body2.m_OneOverMass + Vector3.Cross(rbsc.m_Body2LocalPointCrossedNormal, rbsc.m_Body2LocalPoint)));
body2.m_LinearVelocity -= rbsc.m_Normal * (body2.m_OneOverMass * rbsc.m_Magnitude);
body2.m_AngularVelocity -= rbsc.m_Body2LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body2.m_AngularVelocity);
}
if (rbo1.ContactModifyNotificationHandler != null)
{
if (!rbo1.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
if (rbo2.ContactModifyNotificationHandler != null)
{
if (!rbo2.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
m_SolverConstraints.Add(rbsc);
}
}
public void RemoveActiveRigid(MyRigidBody rbo)
{
if (rbo == null)
return;
if (!rbo.ReadFlag(RigidBodyFlag.RBF_ACTIVE))
return;
//if (rbo.RigidBodyEventHandler != null)
//{
// rbo.RigidBodyEventHandler.OnDeactivated();
//}
rbo.ClearFlag(RigidBodyFlag.RBF_ACTIVE);
if (rbo.ReadFlag(RigidBodyFlag.RBF_KINEMATIC))
{
List<MyRigidBodyIsland> islands = MyPhysics.physicsSystem.GetRigidBodyModule().GetRigidBodyIslandGeneration().GetIslands();
for (int i = 0; i < islands.Count; i++)
{
MyRigidBodyIsland island = islands[i];
//MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Rigids");
for (int j = 0; j < island.GetRigids().Count; j++)
{
MyRigidBody rboi = island.GetRigids()[j];
if (rbo == rboi)
{
island.RemoveRigidBody(rbo);
break;
}
}
}
}
bool found = m_ActiveRigids.Remove(rbo);
//RBO wasnt in the list
Debug.Assert(found);
}
/// <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);
}
/// <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);
}
}
public void RemoveRigidBody(MyRigidBody rbo)
{
m_Rigids.Remove(rbo);
}
private bool DoOverlapBoxTriangleStaticTest(MyBox box, ref MyColDetVoxelTriangle triangle)
{
Matrix dirs0 = box.Orientation;
#region triEdge0
Vector3 pt0;
Vector3 pt1;
triangle.GetPoint(0, out pt0);
triangle.GetPoint(1, out pt1);
Vector3 triEdge0;
Vector3.Subtract(ref pt1, ref pt0, out triEdge0);
if (triEdge0.LengthSquared() < MyPhysicsConfig.Epsilon)
{
return(false);
}
#endregion
#region triEdge1
Vector3 pt2;
triangle.GetPoint(2, out pt2);
Vector3 triEdge1;
Vector3.Subtract(ref pt2, ref pt1, out triEdge1);
if (triEdge1.LengthSquared() < MyPhysicsConfig.Epsilon)
{
return(false);
}
#endregion
#region triEdge2
Vector3 triEdge2;
Vector3.Subtract(ref pt0, ref pt2, out triEdge2);
if (triEdge2.LengthSquared() < MyPhysicsConfig.Epsilon)
{
return(false);
}
#endregion
triEdge0.Normalize();
triEdge1.Normalize();
triEdge2.Normalize();
Vector3 triNormal = triangle.Plane.Normal;
m_axes[0] = triNormal;
m_axes[1] = dirs0.Right;
m_axes[2] = dirs0.Up;
m_axes[3] = dirs0.Backward;
Vector3.Cross(ref m_axes[1], ref triEdge0, out m_axes[4]);
Vector3.Cross(ref m_axes[1], ref triEdge1, out m_axes[5]);
Vector3.Cross(ref m_axes[1], ref triEdge2, out m_axes[6]);
Vector3.Cross(ref m_axes[2], ref triEdge0, out m_axes[7]);
Vector3.Cross(ref m_axes[2], ref triEdge1, out m_axes[8]);
Vector3.Cross(ref m_axes[2], ref triEdge2, out m_axes[9]);
Vector3.Cross(ref m_axes[3], ref triEdge0, out m_axes[10]);
Vector3.Cross(ref m_axes[3], ref triEdge1, out m_axes[11]);
Vector3.Cross(ref m_axes[3], ref triEdge2, out m_axes[12]);
// the overlap depths along each axis
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < numAxes; ++i)
{
m_overlapDepths[i] = 1.0f;
if (Disjoint(out m_overlapDepths[i], m_axes[i], box, triangle, MyPhysicsConfig.CollisionEpsilon))
{
return(false);
}
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < numAxes; ++i)
{
// If we can't normalise the axis, skip it
float l2 = m_axes[i].LengthSquared();
if (l2 < MyPhysicsConfig.Epsilon)
{
continue;
}
// Normalise the separation axis and the depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
m_axes[i] *= invl;
m_overlapDepths[i] *= invl;
// If this axis is the minimum, select it
if (m_overlapDepths[i] < minDepth)
{
minDepth = m_overlapDepths[i];
minAxis = i;
}
}
if (minAxis == -1)
{
return(false);
}
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = box.GetCentre() - triangle.Centre;
Vector3 N = m_axes[minAxis];
float depth = m_overlapDepths[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
{
N *= -1;
}
MyRigidBody rbo0 = GetRigidBody1();
MyRigidBody rbo1 = GetRigidBody2();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 boxOldPos = rbo0.Position;
Vector3 boxNewPos = rbo0.Position + rbo0.LinearVelocity * dt;
Vector3 meshPos = rbo1.Position;
m_CPList.Clear();
GetBoxTriangleIntersectionPoints(m_CPList, box, triangle, MyPhysicsConfig.CollisionEpsilon);
// adjust the depth
#region delta
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
int numPts = m_CPList.Count;
if (numPts > 0)
{
return(true);
}
else
{
return(false);
}
}
public void OnLeave(MySensor sensor, MyRigidBody rbo, MyRBElement rbElement)
{
if (rbo == null)
return;
if (m_isOn && rbo.m_UserData != null)
{
MyEntity entity = (rbo.m_UserData as MyPhysicsBody).Entity;
if (entity != null && (Parent == null || Parent != entity))
{
RemoveEntityFromDetectedAndObservable(entity);
}
}
}
public void OnEnter(MySensor sensor, MyRigidBody rbo, MyRBElement rbElement)
{
if(m_isOn && rbo.m_UserData != null)
{
MyEntity entity = (rbo.m_UserData as MyPhysicsBody).Entity;
if(entity != null && (Parent == null || Parent != entity))
{
int meetCriterias = 0;
bool canRegisteForClose = false;
if (IsEntityMeetCritarias(entity, ref meetCriterias))
{
AddDetectedEntity(entity, meetCriterias);
canRegisteForClose = true;
}
else
{
if (m_containsCriteriumToReCheck)
{
if (CanBeEntityObserved(entity))
{
m_observableEntities.Add(entity);
canRegisteForClose = true;
}
}
}
if (canRegisteForClose)
{
RegisterOnCloseHandlers(entity);
}
}
}
}
public void SetRigidBody(MyRigidBody rbo)
{
m_RigidBody = rbo;
}
/// <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>
/// internal add of to active rigid list
/// </summary>
public void AddActiveRigid(MyRigidBody rbo)
{
if(rbo == null)
return;
if(rbo.ReadFlag(RigidBodyFlag.RBF_ACTIVE))
return;
//if (rbo.RigidBodyEventHandler != null)
//{
// rbo.RigidBodyEventHandler.OnActivated();
//}
rbo.RaiseFlag(RigidBodyFlag.RBF_ACTIVE);
m_ActiveRigids.Add(rbo);
}
/// <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;
}
}
}
public void SetRigidBody(MyRigidBody rbo)
{
m_RigidBody = rbo;
}
private bool DoOverlapBoxTriangleTest(MyBox box, ref MyColDetVoxelTriangle triangle)
{
Matrix dirs0 = box.Orientation;
Vector3 triEdge0;
Vector3 triEdge1;
Vector3 triEdge2;
triEdge0 = MyMwcUtils.Normalize(triangle.Edge0);
triEdge1 = MyMwcUtils.Normalize(triangle.Edge1);
triEdge2 = MyMwcUtils.Normalize(triangle.Edge2);
Vector3 triNormal = triangle.Plane.Normal;
// the 15 potential separating axes (comment by Marek Rosa: note says 15 but code uses 13... I don't know why, mistake in the note??)
const int NUM_AXES = 13;
MyVector3Array13 axes = new MyVector3Array13();
axes[0] = triNormal;
axes[1] = dirs0.Right;
axes[2] = dirs0.Up;
axes[3] = dirs0.Backward;
axes[4] = Vector3.Cross(axes[1], triEdge0);
axes[5] = Vector3.Cross(axes[1], triEdge1);
axes[6] = Vector3.Cross(axes[1], triEdge2);
axes[7] = Vector3.Cross(axes[2], triEdge0);
axes[8] = Vector3.Cross(axes[2], triEdge1);
axes[9] = Vector3.Cross(axes[2], triEdge2);
axes[10] = Vector3.Cross(axes[3], triEdge0);
axes[11] = Vector3.Cross(axes[3], triEdge1);
axes[12] = Vector3.Cross(axes[3], triEdge2);
// the overlap depths along each axis
MyFloatArray13 overlapDepths = new MyFloatArray13();
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
int i;
for (i = 0; i < NUM_AXES; ++i)
{
overlapDepths[i] = 1.0f;
bool b;
overlapDepths[i] = Disjoint(out b, axes[i], box, triangle, MyPhysics.physicsSystem.GetRigidBodyModule().CollisionEpsilon);
if (b)
{
return(false);
}
}
// The box overlap, find the separation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (i = 0; i < NUM_AXES; ++i)
{
// If we can't normalise the axis, skip it
float l2 = axes[i].LengthSquared();
if (l2 < MyPhysicsConfig.Epsilon)
{
continue;
}
// Normalise the separation axis and the depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
axes[i] *= invl;
overlapDepths[i] *= invl;
// If this axis is the minimum, select it
if (overlapDepths[i] < minDepth)
{
minDepth = overlapDepths[i];
minAxis = i;
}
}
if (minAxis == -1)
{
return(false);
}
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = box.GetCentre() - triangle.Centre;
Vector3 N = axes[minAxis];
float depth = overlapDepths[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
{
N *= -1;
}
MyRigidBody rbo0 = GetRigidBody1();
MyRigidBody rbo1 = GetRigidBody2();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 boxOldPos = rbo0.Position;
Vector3 boxNewPos = rbo0.Position + rbo0.LinearVelocity * dt;
Vector3 meshPos = rbo1.Position;
m_CPList.Clear();
GetBoxTriangleIntersectionPoints(m_CPList, box, triangle, MyPhysicsConfig.CollisionEpsilon);
// adjust the depth
#region delta
Vector3 delta;
Vector3.Subtract(ref boxNewPos, ref boxOldPos, out delta);
#endregion
// report collisions
int numPts = m_CPList.Count;
MySmallCollPointInfo[] collPtArray = MyContactInfoCache.SCPIStackAlloc();
{
if (numPts > 0)
{
if (numPts >= MyPhysicsConfig.MaxContactPoints)
{
numPts = MyPhysicsConfig.MaxContactPoints - 1;
}
// adjust positions
for (i = 0; i < numPts; ++i)
{
collPtArray[i] = new MySmallCollPointInfo(m_CPList[i].m_Position - boxOldPos, m_CPList[i].m_Position - meshPos, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, m_CPList[i].m_Normal, m_CPList[i].m_Depth, m_CPList[i].m_Position);
}
MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collPtArray, numPts);
MyContactInfoCache.FreeStackAlloc(collPtArray);
return(true);
}
else
{
MyContactInfoCache.FreeStackAlloc(collPtArray);
return(false);
}
}
}
/// <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;
}
}
protected override bool Interact(bool staticCollision)
{
if (!staticCollision)
{
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("BoxBoxInteraction");
}
try
{
MyRBBoxElement rbbox0 = (MyRBBoxElement)RBElement1;
MyRBBoxElement rbbox1 = (MyRBBoxElement)RBElement2;
MyBox box0 = m_TempBox1;
MyBox box1 = m_TempBox2;
Matrix matrix0 = rbbox0.GetGlobalTransformation();
Matrix matrix1 = rbbox1.GetGlobalTransformation();
box0.Transform.Orientation = matrix0;
box0.Transform.Orientation.Translation = Vector3.Zero;
box0.Transform.Position = matrix0.Translation - Vector3.TransformNormal(rbbox0.Size * 0.5f, matrix0);
box1.Transform.Orientation = matrix1;
box1.Transform.Orientation.Translation = Vector3.Zero;
box1.Transform.Position = matrix1.Translation - Vector3.TransformNormal(rbbox1.Size * 0.5f, matrix1);
box0.SideLengths = rbbox0.Size;
box1.SideLengths = rbbox1.Size;
// see if the boxes are separate along any axis, and if not keep a
// record of the depths along each axis
for (int i = 0; i < 15; ++i)
{
switch (i)
{
case 0: seperatingAxes[0] = box0.Orientation.Right; break;
case 1: seperatingAxes[1] = box0.Orientation.Up; break;
case 2: seperatingAxes[2] = box0.Orientation.Backward; break;
case 3: seperatingAxes[3] = box1.Orientation.Right; break;
case 4: seperatingAxes[4] = box1.Orientation.Up; break;
case 5: seperatingAxes[5] = box1.Orientation.Backward; break;
case 6: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[3], out seperatingAxes[6]); break;
case 7: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[4], out seperatingAxes[7]); break;
case 8: Vector3.Cross(ref seperatingAxes[0], ref seperatingAxes[5], out seperatingAxes[8]); break;
case 9: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[3], out seperatingAxes[9]); break;
case 10: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[4], out seperatingAxes[10]); break;
case 11: Vector3.Cross(ref seperatingAxes[1], ref seperatingAxes[5], out seperatingAxes[11]); break;
case 12: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[3], out seperatingAxes[12]); break;
case 13: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[4], out seperatingAxes[13]); break;
case 14: Vector3.Cross(ref seperatingAxes[2], ref seperatingAxes[5], out seperatingAxes[14]); break;
}
// If we can't normalise the axis, skip it
if (seperatingAxes[i].LengthSquared() < MyPhysicsConfig.CollisionEpsilon)
{
continue;
}
overlapDepth[i] = float.MaxValue;
if (Disjoint(out overlapDepth[i], ref seperatingAxes[i], box0, box1, MyPhysicsConfig.CollisionEpsilon))
{
return(false);
}
}
if (staticCollision)
{
return(true); // Static collision: we're done.
}
// Dynamic collision.
// The boxes overlap, find the seperation depth closest to 0.
float minDepth = float.MaxValue;
int minAxis = -1;
for (int i = 0; i < 15; ++i)
{
// If we can't normalise the axis, skip it
float l2 = seperatingAxes[i].LengthSquared();
if (l2 < MyPhysicsConfig.CollisionEpsilon)
{
continue;
}
// Normalise the separation axis and depth
float invl = 1.0f / (float)System.Math.Sqrt(l2);
seperatingAxes[i] *= invl;
overlapDepth[i] *= invl;
// If this axis is the minmum, select it
if (overlapDepth[i] < minDepth)
{
minDepth = overlapDepth[i];
minAxis = i;
}
}
if (minAxis == -1)
{
return(false);
}
// Make sure the axis is facing towards the 0th box.
// if not, invert it
Vector3 D = box1.GetCentre() - box0.GetCentre();
Vector3 N = seperatingAxes[minAxis];
float depth = overlapDepth[minAxis];
if (Vector3.Dot(D, N) < 0.0f)
{
N *= -1.0f;
}
float minA = MathHelper.Min(box0.SideLengths.X, MathHelper.Min(box0.SideLengths.Y, box0.SideLengths.Z));
float minB = MathHelper.Min(box1.SideLengths.X, MathHelper.Min(box1.SideLengths.Y, box1.SideLengths.Z));
float combinationDist = 0.05f * MathHelper.Min(minA, minB);
// the contact points
contactPts.Clear();
int numPts = contactPts.Count;
GetBoxBoxIntersectionPoints(contactPts, box0, box1, combinationDist, MyPhysicsConfig.CollisionEpsilon);
numPts = contactPts.Count;
MyRigidBody rbo0 = GetRigidBody1();
MyRigidBody rbo1 = GetRigidBody2();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 body0OldPos = rbo0.Position;
Vector3 body1OldPos = rbo1.Position;
Vector3 body0NewPos = (rbo0.Position + rbo0.LinearVelocity * dt);
Vector3 body1NewPos = (rbo1.Position + rbo1.LinearVelocity * dt);
#region REFERENCE: Vector3 bodyDelta = body0NewPos - body0OldPos - body1NewPos + body1OldPos;
Vector3 bodyDelta;
Vector3.Subtract(ref body0NewPos, ref body0OldPos, out bodyDelta);
Vector3.Subtract(ref bodyDelta, ref body1NewPos, out bodyDelta);
Vector3.Add(ref bodyDelta, ref body1OldPos, out bodyDelta);
#endregion
#region REFERENCE: float bodyDeltaLen = Vector3.Dot(bodyDelta,N);
float bodyDeltaLen;
Vector3.Dot(ref bodyDelta, ref N, out bodyDeltaLen);
#endregion
float oldDepth = depth + bodyDeltaLen;
MySmallCollPointInfo[] collPtArray = MyContactInfoCache.SCPIStackAlloc();
{
int numCollPts = 0;
Vector3 SATPoint;
switch (minAxis)
{
// Box0 face, Box1 corner collision
case 0:
case 1:
case 2:
{
// Get the lowest point on the box1 along box1 normal
GetSupportPoint(out SATPoint, box1, -N);
break;
}
// We have a Box2 corner/Box1 face collision
case 3:
case 4:
case 5:
{
// Find with vertex on the triangleVertexes collided
GetSupportPoint(out SATPoint, box0, N);
break;
}
// We have an edge/edge collision
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
{
{
// Retrieve which edges collided.
int i = minAxis - 6;
int ia = i / 3;
int ib = i - ia * 3;
// find two P0, P1 point on both edges.
Vector3 P0, P1;
GetSupportPoint(out P0, box0, N);
GetSupportPoint(out P1, box1, -N);
// Find the edge intersection.
// plane along N and F, and passing through PB
Vector3 box0Orient, box1Orient;
MyPhysicsUtils.MyPhysicsUnsafe.Get(ref box0.Transform.Orientation, ia, out box0Orient);
MyPhysicsUtils.MyPhysicsUnsafe.Get(ref box1.Transform.Orientation, ib, out box1Orient);
#region REFERENCE: Vector3 planeNormal = Vector3.Cross(N, box1Orient[ib]);
Vector3 planeNormal;
Vector3.Cross(ref N, ref box1Orient, out planeNormal);
#endregion
#region REFERENCE: float planeD = Vector3.Dot(planeNormal, P1);
float planeD;
Vector3.Dot(ref planeNormal, ref P1, out planeD);
#endregion
// find the intersection t, where Pintersection = P0 + t*box edge dir
#region REFERENCE: float div = Vector3.Dot(box0Orient, planeNormal);
float div;
Vector3.Dot(ref box0Orient, ref planeNormal, out div);
#endregion
// plane and ray colinear, skip the intersection.
if (System.Math.Abs(div) < MyPhysicsConfig.CollisionEpsilon)
{
return(false);
}
float t = (planeD - Vector3.Dot(P0, planeNormal)) / div;
// point on edge of box0
#region REFERENCE: P0 += box0Orient * t;
P0 = Vector3.Add(Vector3.Multiply(box0Orient, t), P0);
#endregion
#region REFERENCE: SATPoint = (P0 + (0.5f * depth) * N);
Vector3.Multiply(ref N, 0.5f * depth, out SATPoint);
Vector3.Add(ref SATPoint, ref P0, out SATPoint);
#endregion
}
break;
}
default:
{
SATPoint = Vector3.Zero;
Debug.Assert(false);
break;
}
}
// distribute the depth according to the distance to the SAT point
if (numPts > 0)
{
float minDist = float.MaxValue;
float maxDist = float.MinValue;
for (int i = 0; i < numPts; ++i)
{
float dist = MyPhysicsUtils.PointPointDistance(contactPts[i].Pos, SATPoint);
if (dist < minDist)
{
minDist = dist;
}
if (dist > maxDist)
{
maxDist = dist;
}
}
// got some intersection points
for (int i = 0; i < numPts; ++i)
{
float minDepthScale = 0.0f;
float dist = MyPhysicsUtils.PointPointDistance(contactPts[i].Pos, SATPoint);
float safeDivisionDist = (maxDist - minDist);
if ((maxDist - minDist) == 0.0f)
{
safeDivisionDist = MyPhysicsConfig.CollisionEpsilon;
}
float depthScale = (dist - minDist) / safeDivisionDist;
depth = (1.0f - depthScale) * oldDepth + minDepthScale * depthScale * oldDepth;
if (numCollPts < MyPhysicsConfig.MaxContactPoints)
{
collPtArray[numCollPts++] = new MySmallCollPointInfo(contactPts[i].Pos - body0OldPos, contactPts[i].Pos - body1OldPos, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, N, depth, contactPts[i].Pos);
}
}
}
else
{
#region REFERENCE: collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
//collPts.Add(new CollPointInfo(SATPoint - body0NewPos, SATPoint - body1NewPos, oldDepth));
Vector3 cp0;
Vector3.Subtract(ref SATPoint, ref body0NewPos, out cp0);
Vector3 cp1;
Vector3.Subtract(ref SATPoint, ref body1NewPos, out cp1);
if (numCollPts < MyPhysicsConfig.MaxContactPoints)
{
collPtArray[numCollPts++] = new MySmallCollPointInfo(cp0, cp1, GetRigidBody1().LinearVelocity, GetRigidBody2().LinearVelocity, N, oldDepth, SATPoint);
}
#endregion
}
// report Collisions
MyPhysics.physicsSystem.GetContactConstraintModule().AddContactConstraint(this, collPtArray, numCollPts);
}
MyContactInfoCache.FreeStackAlloc(collPtArray);
}
catch
{
throw;
}
finally
{
if (!staticCollision)
{
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
}
return(false);
}
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);
}
}
}
}
}
}
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);
}