static public bool AcceptCollision(MyRBElement el0,MyRBElement el1)
{
if (!MyPhysics.physicsSystem.GetRigidBodyModule().IsEnabledCollisionInLayers(el0.CollisionLayer,el1.CollisionLayer))
{
return false;
}
MyGroupMask mask0 = el0.GroupMask;
MyGroupMask mask1 = el1.GroupMask;
if ((mask0.m_Mask0 & mask1.m_Mask0) > 0)
{
return false;
}
if ((mask0.m_Mask1 & mask1.m_Mask1) > 0)
{
return false;
}
if ((mask0.m_Mask2 & mask1.m_Mask2) > 0)
{
return false;
}
if ((mask0.m_Mask3 & mask1.m_Mask3) > 0)
{
return false;
}
return true;
}
/// <summary>
/// Removes element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not remove another one or change mass
/// </summary>
public void RemoveElement(MyRBElement element, bool recomputeInertia)
{
MyPhysicsObjects physobj = MyPhysics.physicsSystem.GetPhysicsObjects();
for (int i = 0; i < m_RBElementList.Count; i++)
{
if (m_RBElementList[i] == element)
{
element.SetRigidBody(null);
physobj.RemoveRBElement(element);
m_RBElementList.RemoveAt(i);
break;
}
}
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().DestroyVolume(element);
}
if (recomputeInertia && m_RBElementList.Count > 0)
{
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
}
public void SwapElements()
{
MyRBElement tempEl = m_Element2;
m_Element2 = m_Element1;
m_Element1 = tempEl;
}
static public bool AcceptCollision(MyRBElement el0, MyRBElement el1)
{
if (!MyPhysics.physicsSystem.GetRigidBodyModule().IsEnabledCollisionInLayers(el0.CollisionLayer, el1.CollisionLayer))
{
return(false);
}
MyGroupMask mask0 = el0.GroupMask;
MyGroupMask mask1 = el1.GroupMask;
if ((mask0.m_Mask0 & mask1.m_Mask0) > 0)
{
return(false);
}
if ((mask0.m_Mask1 & mask1.m_Mask1) > 0)
{
return(false);
}
if ((mask0.m_Mask2 & mask1.m_Mask2) > 0)
{
return(false);
}
if ((mask0.m_Mask3 & mask1.m_Mask3) > 0)
{
return(false);
}
return(true);
}
public override void DestroyVolume(MyElement element)
{
if (element.ProxyData == MyElement.PROXY_UNASSIGNED)
{
return;
}
m_DAABBTree.RemoveProxy(element.ProxyData);
element.ProxyData = MyElement.PROXY_UNASSIGNED;
if ((element.Flags & MyElementFlag.EF_SENSOR_ELEMENT) > 0)
{
MySensorElement se = (MySensorElement)element;
}
if ((element.Flags & MyElementFlag.EF_RB_ELEMENT) > 0)
{
MyRBElement elm = (MyRBElement)element;
//clear all iterations from me and from objects i iterate with
while (elm.GetRBElementInteractions().Count > 0)
{
MyRBElementInteraction intr = elm.GetRBElementInteractions()[0];
MyPhysics.physicsSystem.GetRBInteractionModule().RemoveRBElementInteraction(intr.RBElement1, intr.RBElement2);
}
elm.GetRBElementInteractions().Clear();
}
}
public override void DestroyVolume(MyElement element)
{
if ((element.Flags & MyElementFlag.EF_SENSOR_ELEMENT) > 0)
{
return;
}
MyRBElement elm = (MyRBElement)element;
elm.GetRBElementInteractions().Clear();
m_Elements.Remove(elm);
}
/// <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>
/// Do static Test of intersection
/// </summary>
public bool DoStaticTestInteraction(MyRBElement el1, MyRBElement el2)
{
MyRBElementInteraction myElemInteraction = FindRBElementInteractionForStaticTesting(el1.GetElementType(), el2.GetElementType());
if (myElemInteraction != null)
{
myElemInteraction.RBElement1 = el1;
myElemInteraction.RBElement2 = el2;
return(myElemInteraction.DoStaticInitialTest());
}
return(false);
}
/// <summary>
/// Looks if interaction between those elements already exist
/// </summary>
public MyRBElementInteraction FindRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
// look for interaction on element
for (int i = 0; i < el1.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el1.GetRBElementInteractions()[i];
if (intr.RBElement1 == el2 || intr.RBElement2 == el2)
{
return(intr);
}
}
return(null);
}
/// <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>
/// Adds element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not insert another one or change mass
/// </summary>
public bool AddElement(MyRBElement element, bool recomputeInertia)
{
m_RBElementList.Add(element);
element.SetRigidBody(this);
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().CreateVolume(element);
}
if (recomputeInertia)
{
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
return(true);
}
/// <summary>
/// Removes interaction between these 2 elements
/// </summary>
public void RemoveRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
if (el1 != null)
{
// look for interaction on element
for (int i = 0; i < el1.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el1.GetRBElementInteractions()[i];
if ((intr.RBElement1 == el1 && intr.RBElement2 == el2) || (intr.RBElement1 == el2 && intr.RBElement2 == el1))
{
// add it back
int t1 = (int)el1.GetElementType();
int t2 = (int)el2.GetElementType();
List <MyRBElementInteraction> intrList = null;
if (t1 < t2)
{
intrList = m_IslandsPool[t1, t2];
}
else
{
intrList = m_IslandsPool[t2, t1];
}
intrList.Add(intr);
el1.GetRBElementInteractions().Remove(intr);
break;
}
}
}
if (el2 != null)
{
for (int i = 0; i < el2.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el2.GetRBElementInteractions()[i];
if ((intr.RBElement1 == el1 && intr.RBElement2 == el2) || (intr.RBElement1 == el2 && intr.RBElement2 == el1))
{
intr.RBElement1 = null;
intr.RBElement2 = null;
el2.GetRBElementInteractions().Remove(intr);
break;
}
}
}
}
/// <summary>
/// Adds interaction between 2 given elements
/// </summary>
public MyRBElementInteraction AddRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
// get it
int t1 = (int)el1.GetElementType();
int t2 = (int)el2.GetElementType();
List <MyRBElementInteraction> intrList = null;
if (t1 < t2)
{
intrList = m_IslandsPool[t1, t2];
}
else
{
intrList = m_IslandsPool[t2, t1];
}
//pada to jinak
if (intrList.Count == 0)
{
return(null);
}
MyCommonDebugUtils.AssertDebug(intrList.Count != 0);
if (intrList.Count == 1)
{
MyRBElementInteraction ins = intrList[0].CreateNewInstance();
intrList.Add(ins);
}
MyRBElementInteraction intr = intrList[intrList.Count - 1];
intrList.RemoveAt(intrList.Count - 1);
intr.RBElement1 = el1;
intr.RBElement2 = el2;
el1.GetRBElementInteractions().Add(intr);
el2.GetRBElementInteractions().Add(intr);
return(intr);
}
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 override void CreateVolume(MyElement element)
{
#if PHYSICS_CHECK
for (int i = 0; i < m_Elements.Count; i++)
{
MyRBElement el = m_Elements[i];
if (el == element)
{
// inserting twice!!!
CommonDebugUtils.AssertRelease(false);
return;
}
}
#endif
if ((element.Flags & MyElementFlag.EF_SENSOR_ELEMENT) > 0)
{
return;
}
MyRBElement elm = (MyRBElement)element;
m_Elements.Add(elm);
}
public void RemoveRBElement(MyRBElement element)
{
switch (element.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
m_RBSphereElementPool.Deallocate((MyRBSphereElement)element);
}
break;
case MyRBElementType.ET_BOX:
{
m_RBBoxElementPool.Deallocate((MyRBBoxElement)element);
}
break;
case MyRBElementType.ET_CAPSULE:
{
m_RBCapsuleElementPool.Deallocate((MyRBCapsuleElement)element);
}
break;
case MyRBElementType.ET_TRIANGLEMESH:
{
m_RBTriangleMeshElementPool.Deallocate((MyRBTriangleMeshElement)element);
}
break;
case MyRBElementType.ET_VOXEL:
{
m_RBVoxelElementPool.Deallocate((MyRBVoxelElement)element);
}
break;
default:
// unknown element type
MyCommonDebugUtils.AssertDebug(false);
break;
}
}
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 void Init(MySensorElement sensorElement, MyRBElement rbElement)
{
Debug.Assert(rbElement.GetRigidBody() != null);
Debug.Assert(sensorElement.Sensor != null);
m_SensorElement = sensorElement;
m_RBElement = rbElement;
int guid1 = sensorElement.GUID;
int guid2 = rbElement.GUID;
if (guid1 > guid2)
{
int tm = guid2;
guid2 = guid1;
guid1 = tm;
}
m_Guid = guid1 + (guid2 << 16);
m_IsInside = false;
m_IsInUse = true;
m_RBElement.GetRigidBody().OnDeactivated.Event += m_onRigidBodyDeactivatedEventHandler;
}
/// <summary>
/// we have the aabb updated and just update the tree using the info from velocity as a hint
/// </summary>
public override void MoveVolumeFast(MyElement element)
{
if (element.ProxyData == MyElement.PROXY_UNASSIGNED)
{
return;
}
if ((element.Flags & MyElementFlag.EF_RB_ELEMENT) > 0)
{
MyRBElement rel = (MyRBElement)element;
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
Vector3 movement = rel.GetRigidBody().LinearVelocity *dt;
BoundingBox aabb = element.GetWorldSpaceAABB();
m_DAABBTree.MoveProxy(element.ProxyData, ref aabb, movement);
}
else
{
BoundingBox aabb = element.GetWorldSpaceAABB();
m_DAABBTree.MoveProxy(element.ProxyData, ref aabb, Vector3.Zero);
}
}
public void Init(MySensorElement sensorElement, MyRBElement rbElement)
{
Debug.Assert(rbElement.GetRigidBody() != null);
Debug.Assert(sensorElement.Sensor != null);
m_SensorElement = sensorElement;
m_RBElement = rbElement;
int guid1 = sensorElement.GUID;
int guid2 = rbElement.GUID;
if (guid1 > guid2)
{
int tm = guid2;
guid2 = guid1;
guid1 = tm;
}
m_Guid = guid1 + (guid2 << 16);
m_IsInside = false;
m_IsInUse = true;
m_RBElement.GetRigidBody().OnDeactivated.Event += m_onRigidBodyDeactivatedEventHandler;
}
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);
}
}
}
}
/// <summary>
/// parses all active rigids, updates the aabbs and checks for possible collisions using the DAABB
/// </summary>
public override void DoWork()
{
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("ClearInteractions");
ClearInteractions();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
MyRBInteractionModule module = MyPhysics.physicsSystem.GetRBInteractionModule();
HashSet <MyRigidBody> activeRigids = MyPhysics.physicsSystem.GetRigidBodyModule().GetActiveRigids();
float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep;
BoundingBox aabb;
//Dictionary<string, int> typeStats = new Dictionary<string, int>();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("MoveProxy");
// A.B. this might be expensive, maybe update separate or move somewhere else like in the solve -> update positions !!
foreach (MyRigidBody rbo in activeRigids)
{
/*
* string ts = ((MinerWars.AppCode.Game.Physics.MyPhysicsBody)rbo.m_UserData).Entity.GetType().Name.ToString();
* if (!typeStats.ContainsKey(ts))
* typeStats.Add(ts, 0);
* typeStats[ts]++;
*/
for (int j = 0; j < rbo.GetRBElementList().Count; j++)
{
MyRBElement el = rbo.GetRBElementList()[j];
el.UpdateAABB();
aabb = el.GetWorldSpaceAABB();
m_DAABBTree.MoveProxy(el.ProxyData, ref aabb, el.GetRigidBody().LinearVelocity *dt);
}
}
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("make the AABB test");
// make the AABB test
MyRBElementInteraction interaction = null;
#if RENDER_PROFILING && !MEMORY_PROFILING
int[] heights = new int[activeRigids.Count];
int[] tests = new int[activeRigids.Count];
#endif
int i = 0;
foreach (MyRigidBody rbo in activeRigids)
{
for (int j = 0; j < rbo.GetRBElementList().Count; j++)
{
MyRBElement el = rbo.GetRBElementList()[j];
Vector3 globalPosition = Vector3.Transform(el.LocalPosition, rbo.Matrix);
Vector3 deltaVelocity = rbo.LinearVelocity * dt;
aabb = el.GetWorldSpaceAABB();
if (rbo.ReadFlag(RigidBodyFlag.RBF_COLDET_THROUGH_VOXEL_TRIANGLES) || el is MyRBSphereElement) //because sphere is interpolated for whole path
{
Vector3 v = globalPosition + rbo.LinearVelocity * dt;
//Vector3 v = aabb.GetCenter()+rbo.LinearVelocity * dt;
aabb = aabb.Include(ref v);
}
else
{
aabb.Max += deltaVelocity;
aabb.Min += deltaVelocity;
}
//if (el is MyRBSphereElement)
//{
//MyDebugDraw.AddDrawSphereWireframe(new BoundingSphere(aabb.GetCenter(), (aabb.GetCorners()[0] - aabb.GetCenter()).Length()));
// MyDebugDraw.AddDrawSphereWireframe(new BoundingSphere(aabb.GetCenter()+rbo.LinearVelocity * dt, (aabb.GetCorners()[0] - aabb.GetCenter()).Length()));
//}
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("m_DAABBTree.OverlapAllBoundingBox");
#if RENDER_PROFILING && !MEMORY_PROFILING
m_DAABBTree.OverlapAllBoundingBox(ref aabb, m_overlapElementList, 0);
#else
m_DAABBTree.OverlapAllBoundingBox(ref aabb, m_overlapElementList, 0);
#endif
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Interactions");
foreach (var lEl in m_overlapElementList)
{
if (el == lEl)//optimization?
{
continue;
}
if ((lEl.Flags & MyElementFlag.EF_SENSOR_ELEMENT) > 0)
{
MySensorElement sensorElement = lEl as MySensorElement;
MyRBElement rbElement = el as MyRBElement;
MyPhysics.physicsSystem.GetSensorInteractionModule().AddSensorInteraction(sensorElement, rbElement);
continue;
}
if ((lEl.Flags & MyElementFlag.EF_RB_ELEMENT) > 0)
{
MyRBElement testEl = (MyRBElement)lEl;
if (el.GetRigidBody().IsStatic() && testEl.GetRigidBody().IsStatic())
{
continue;
}
if (el.GetRigidBody().IsKinematic() && testEl.GetRigidBody().IsKinematic())
{
continue;
}
if (el.GetRigidBody().IsKinematic() && testEl.GetRigidBody().IsStatic())
{
continue;
}
if (el.GetRigidBody().IsStatic() && testEl.GetRigidBody().IsKinematic())
{
continue;
}
if (el.GetRigidBody() == testEl.GetRigidBody())
{
continue;
}
if (!MyFiltering.AcceptCollision(el, testEl))
{
continue;
}
interaction = module.FindRBElementInteraction(el, testEl);
if (interaction == null)
{
interaction = module.AddRBElementInteraction(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);
}
}
}
}
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
i++;
}
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("Active rigids", activeRigids.Count);
#if RENDER_PROFILING && !MEMORY_PROFILING
float averageHeight = 0;
float averageTest = 0;
int maxHeight = 0;
int maxTest = 0;
for (int j = 0; j < activeRigids.Count; j++)
{
averageHeight += heights[j];
averageTest += tests[j];
if (maxHeight < heights[j])
{
maxHeight = heights[j];
}
if (maxTest < tests[j])
{
maxTest = tests[j];
}
}
averageHeight /= activeRigids.Count;
averageTest /= activeRigids.Count;
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("Average height", averageHeight);
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("Average test", averageTest);
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("Max height", maxHeight);
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("Max test", maxTest);
#endif
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("handle active sensors");
List <MySensor> activeSensors = MyPhysics.physicsSystem.GetSensorModule().ActiveSensors;
if (activeSensors.Count > 0)
{
if (m_activeSensorIndex >= activeSensors.Count)
{
m_activeSensorIndex = 0;
}
MySensor activeSensor = activeSensors[m_activeSensorIndex];
activeSensor.PrepareSensorInteractions();
MySensorElement sensorElement = activeSensor.GetElement();
BoundingBox sensorElAABB = sensorElement.GetWorldSpaceAABB();
m_sensorInteractonList.Clear();
m_DAABBTree.OverlapAllBoundingBox(ref sensorElAABB, m_sensorInteractonList, (uint)MyElementFlag.EF_RB_ELEMENT);
foreach (MyRBElement rbElement in m_sensorInteractonList)
{
MyPhysics.physicsSystem.GetSensorInteractionModule().AddSensorInteraction(sensorElement, rbElement);
}
activeSensor.Active = false;
m_activeSensorIndex++;
}
//List<MySensor> activeSensors = MyPhysics.physicsSystem.GetSensorModule().ActiveSensors;
//for (int i = activeSensors.Count - 1; i >= 0; i--)
//{
// MySensorElement sensorElement = activeSensors[i].GetElement();
// BoundingBox sensorElAABB = sensorElement.GetWorldSpaceAABB();
// m_sensorInteractonList.Clear();
// m_DAABBTree.OverlapRBAllBoundingBox(ref sensorElAABB, m_sensorInteractonList);
// foreach (MyRBElement rbElement in m_sensorInteractonList)
// {
// MyPhysics.physicsSystem.GetSensorInteractionModule().AddSensorInteraction(sensorElement, rbElement);
// }
// activeSensors[i].IsActive = false;
// activeSensors.RemoveAt(i);
//}
MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock();
}
/// <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>
/// adds new sensor interaction between sensor and rigid
/// </summary>
public void AddSensorInteraction(MySensorElement sensorElement, MyRBElement rbElement)
{
if (sensorElement.DetectRigidBodyTypes != null && rbElement.GetRigidBody().Type != sensorElement.DetectRigidBodyTypes.Value)
{
return;
}
MySensorInteraction si = null;
int guid1 = sensorElement.GUID;
int guid2 = rbElement.GUID;
if (guid1 > guid2)
{
int tm = guid2;
guid2 = guid1;
guid1 = tm;
}
int guid = guid1 + (guid2 << 16);
// if this interaction is in current interactions
if (m_CurrentInteractions.ContainsKey(guid))
{
return;
}
//if (sensorElement.Sensor.m_Interactions.TryGetValue(guid, out si))
//{
// Debug.Assert(guid == si.m_Guid);
// m_CurrentInteractions.Add(guid, si);
// return;
//}
if (m_InteractionsInUse.TryGetValue(guid, out si))
{
Debug.Assert(guid == si.m_Guid);
m_CurrentInteractions.Add(guid, si);
return;
}
switch (sensorElement.GetElementType())
{
case MySensorElementType.ET_SPHERE:
{
switch (rbElement.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
if (m_FreeSSSi.Count == 0)
{
m_FreeSSSi.Push(new MySphereSphereSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSSSi.Pop();
}
break;
case MyRBElementType.ET_BOX:
{
if (m_FreeSBSi.Count == 0)
{
m_FreeSBSi.Push(new MySphereBoxSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSBSi.Pop();
}
break;
default:
{
if (m_FreeSOSi.Count == 0)
{
m_FreeSOSi.Push(new MySphereOtherSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSOSi.Pop();
}
break;
}
}
break;
case MySensorElementType.ET_BOX:
switch (rbElement.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
if (m_FreeBSSi.Count == 0)
{
m_FreeBSSi.Push(new MyBoxSphereSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBSSi.Pop();
}
break;
case MyRBElementType.ET_BOX:
{
if (m_FreeBBSi.Count == 0)
{
m_FreeBBSi.Push(new MyBoxBoxSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBBSi.Pop();
}
break;
default:
{
if (m_FreeBOSi.Count == 0)
{
m_FreeBOSi.Push(new MyBoxOtherSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBOSi.Pop();
}
break;
}
break;
default:
break;
}
if (si == null)
{
return;
}
Debug.Assert(!si.m_IsInUse);
si.Init(sensorElement, rbElement);
Debug.Assert(guid == si.m_Guid);
m_CurrentInteractions.Add(guid, si);
m_InteractionsInUse.Add(guid, si);
m_interactionsInUse++;
if (m_interactionsInUse > m_interactionsInUseMax)
{
m_interactionsInUseMax = m_interactionsInUse;
}
}
/// <summary>
/// adds new sensor interaction between sensor and rigid
/// </summary>
public void AddSensorInteraction(MySensorElement sensorElement, MyRBElement rbElement)
{
if (sensorElement.DetectRigidBodyTypes != null && rbElement.GetRigidBody().Type != sensorElement.DetectRigidBodyTypes.Value)
{
return;
}
MySensorInteraction si = null;
int guid1 = sensorElement.GUID;
int guid2 = rbElement.GUID;
if (guid1 > guid2)
{
int tm = guid2;
guid2 = guid1;
guid1 = tm;
}
int guid = guid1 + (guid2 << 16);
// if this interaction is in current interactions
if (m_CurrentInteractions.ContainsKey(guid))
{
return;
}
//if (sensorElement.Sensor.m_Interactions.TryGetValue(guid, out si))
//{
// Debug.Assert(guid == si.m_Guid);
// m_CurrentInteractions.Add(guid, si);
// return;
//}
if (m_InteractionsInUse.TryGetValue(guid, out si))
{
Debug.Assert(guid == si.m_Guid);
m_CurrentInteractions.Add(guid, si);
return;
}
switch (sensorElement.GetElementType())
{
case MySensorElementType.ET_SPHERE:
{
switch (rbElement.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
if (m_FreeSSSi.Count == 0)
{
m_FreeSSSi.Push(new MySphereSphereSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSSSi.Pop();
}
break;
case MyRBElementType.ET_BOX:
{
if (m_FreeSBSi.Count == 0)
{
m_FreeSBSi.Push(new MySphereBoxSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSBSi.Pop();
}
break;
default:
{
if (m_FreeSOSi.Count == 0)
{
m_FreeSOSi.Push(new MySphereOtherSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeSOSi.Pop();
}
break;
}
}
break;
case MySensorElementType.ET_BOX:
switch (rbElement.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
if (m_FreeBSSi.Count == 0)
{
m_FreeBSSi.Push(new MyBoxSphereSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBSSi.Pop();
}
break;
case MyRBElementType.ET_BOX:
{
if (m_FreeBBSi.Count == 0)
{
m_FreeBBSi.Push(new MyBoxBoxSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBBSi.Pop();
}
break;
default:
{
if (m_FreeBOSi.Count == 0)
{
m_FreeBOSi.Push(new MyBoxOtherSensorInteraction());
m_newAllocatedInteractions++;
}
si = m_FreeBOSi.Pop();
}
break;
}
break;
default:
break;
}
if (si == null)
{
return;
}
Debug.Assert(!si.m_IsInUse);
si.Init(sensorElement, rbElement);
Debug.Assert(guid == si.m_Guid);
m_CurrentInteractions.Add(guid, si);
m_InteractionsInUse.Add(guid, si);
m_interactionsInUse++;
if (m_interactionsInUse > m_interactionsInUseMax)
{
m_interactionsInUseMax = m_interactionsInUse;
}
}
/// <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>
/// Removes element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not remove another one or change mass
/// </summary>
public void RemoveElement(MyRBElement element, bool recomputeInertia)
{
MyPhysicsObjects physobj = MyPhysics.physicsSystem.GetPhysicsObjects();
for (int i = 0; i < m_RBElementList.Count; i++)
{
if (m_RBElementList[i] == element)
{
element.SetRigidBody(null);
physobj.RemoveRBElement(element);
m_RBElementList.RemoveAt(i);
break;
}
}
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().DestroyVolume(element);
}
if (recomputeInertia && m_RBElementList.Count > 0)
MyPhysicsUtils.ComputeIntertiaTensor(this);
}
/// <summary>
/// Removes interaction between these 2 elements
/// </summary>
public void RemoveRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
if (el1 != null)
{
// look for interaction on element
for (int i = 0; i < el1.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el1.GetRBElementInteractions()[i];
if ((intr.RBElement1 == el1 && intr.RBElement2 == el2) || (intr.RBElement1 == el2 && intr.RBElement2 == el1))
{
// add it back
int t1 = (int)el1.GetElementType();
int t2 = (int)el2.GetElementType();
List<MyRBElementInteraction> intrList = null;
if (t1 < t2)
intrList = m_IslandsPool[t1, t2];
else
intrList = m_IslandsPool[t2, t1];
intrList.Add(intr);
el1.GetRBElementInteractions().Remove(intr);
break;
}
}
}
if (el2 != null)
{
for (int i = 0; i < el2.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el2.GetRBElementInteractions()[i];
if ((intr.RBElement1 == el1 && intr.RBElement2 == el2) || (intr.RBElement1 == el2 && intr.RBElement2 == el1))
{
intr.RBElement1 = null;
intr.RBElement2 = null;
el2.GetRBElementInteractions().Remove(intr);
break;
}
}
}
}
/// <summary>
/// Adds interaction between 2 given elements
/// </summary>
public MyRBElementInteraction AddRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
// get it
int t1 = (int)el1.GetElementType();
int t2 = (int)el2.GetElementType();
List<MyRBElementInteraction> intrList = null;
if (t1 < t2)
intrList = m_IslandsPool[t1, t2];
else
intrList = m_IslandsPool[t2, t1];
//pada to jinak
if (intrList.Count == 0)
return null;
MyCommonDebugUtils.AssertDebug(intrList.Count != 0);
if (intrList.Count == 1)
{
MyRBElementInteraction ins = intrList[0].CreateNewInstance();
intrList.Add(ins);
}
MyRBElementInteraction intr = intrList[intrList.Count - 1];
intrList.RemoveAt(intrList.Count - 1);
intr.RBElement1 = el1;
intr.RBElement2 = el2;
el1.GetRBElementInteractions().Add(intr);
el2.GetRBElementInteractions().Add(intr);
return intr;
}
/// <summary>
/// Looks if interaction between those elements already exist
/// </summary>
public MyRBElementInteraction FindRBElementInteraction(MyRBElement el1, MyRBElement el2)
{
// look for interaction on element
for (int i = 0; i < el1.GetRBElementInteractions().Count; i++)
{
MyRBElementInteraction intr = el1.GetRBElementInteractions()[i];
if (intr.RBElement1 == el2 || intr.RBElement2 == el2)
return intr;
}
return null;
}
/// <summary>
/// Do static Test of intersection
/// </summary>
public bool DoStaticTestInteraction(MyRBElement el1, MyRBElement el2)
{
MyRBElementInteraction myElemInteraction = FindRBElementInteractionForStaticTesting(el1.GetElementType(), el2.GetElementType());
if (myElemInteraction != null)
{
myElemInteraction.RBElement1 = el1;
myElemInteraction.RBElement2 = el2;
return myElemInteraction.DoStaticInitialTest();
}
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 RemoveRBElement(MyRBElement element)
{
switch (element.GetElementType())
{
case MyRBElementType.ET_SPHERE:
{
m_RBSphereElementPool.Deallocate((MyRBSphereElement)element);
}
break;
case MyRBElementType.ET_BOX:
{
m_RBBoxElementPool.Deallocate((MyRBBoxElement)element);
}
break;
case MyRBElementType.ET_CAPSULE:
{
m_RBCapsuleElementPool.Deallocate((MyRBCapsuleElement)element);
}
break;
case MyRBElementType.ET_TRIANGLEMESH:
{
m_RBTriangleMeshElementPool.Deallocate((MyRBTriangleMeshElement)element);
}
break;
case MyRBElementType.ET_VOXEL:
{
m_RBVoxelElementPool.Deallocate((MyRBVoxelElement)element);
}
break;
default:
// unknown element type
MyCommonDebugUtils.AssertDebug(false);
break;
}
}
/// <summary>
/// Adds element to the rigid body, you have to specify if you want to compute the inertia tensor, yes in case you will not insert another one or change mass
/// </summary>
public bool AddElement(MyRBElement element, bool recomputeInertia)
{
m_RBElementList.Add(element);
element.SetRigidBody(this);
if ((m_Flags & RigidBodyFlag.RBF_INSERTED) > 0)
{
MyPhysics.physicsSystem.GetRigidBodyModule().GetBroadphase().CreateVolume(element);
}
if (recomputeInertia)
MyPhysicsUtils.ComputeIntertiaTensor(this);
return true;
}
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>
/// Adds the skin element.
/// </summary>
/// <param name="element">The element.</param>
/// <param name="b">if set to <c>true</c> [b].</param>
public void AddElement(MyRBElement element, bool recomputeInertia)
{
Debug.Assert(element != null);
this.rigidBody.AddElement(element, recomputeInertia);
}