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