private void DetectSoftSoft(SoftBody body1, SoftBody body2)
{
ResourcePoolItemList <int> my = potentialTriangleLists.GetNew();
ResourcePoolItemList <int> other = potentialTriangleLists.GetNew();
body1.dynamicTree.Query(other, my, body2.dynamicTree);
for (int i = 0; i < other.Count; i++)
{
SoftBody.Triangle myTriangle = body1.dynamicTree.GetUserData(my[i]);
SoftBody.Triangle otherTriangle = body2.dynamicTree.GetUserData(other[i]);
TSVector point, normal;
FP penetration;
bool result;
result = XenoCollide.Detect(myTriangle, otherTriangle, ref TSMatrix.InternalIdentity, ref TSMatrix.InternalIdentity,
ref TSVector.InternalZero, ref TSVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndexMy = FindNearestTrianglePoint(body1, my[i], ref point);
int minIndexOther = FindNearestTrianglePoint(body2, other[i], ref point);
RaiseCollisionDetected(body1.VertexBodies[minIndexMy],
body2.VertexBodies[minIndexOther], ref point, ref point, ref normal, penetration);
}
}
my.Clear();
other.Clear();
potentialTriangleLists.GiveBack(my);
potentialTriangleLists.GiveBack(other);
}
private void AddConnection(RigidBody body1, RigidBody body2)
{
// CUSTOM: Modified to account for kinematic bodies.
var type1 = body1.BodyType;
var type2 = body2.BodyType;
// Kinematic bodies cannot be moved by dynamic bodies.
bool isBody1Movable = type1 <= type2;
bool isBody2Movable = type2 <= type1;
if (!(isBody1Movable || isBody2Movable))
{
return;
}
// Only body 2 is movable.
if (isBody2Movable && !isBody1Movable)
{
if (body2.island == null)
{
CollisionIsland newIsland = Pool.GetNew();
newIsland.islandManager = this;
body2.island = newIsland;
body2.island.bodies.Add(body2);
islands.Add(newIsland);
}
}
// Only body 1 is movable.
else if (!isBody2Movable)
{
if (body1.island == null)
{
CollisionIsland newIsland = Pool.GetNew();
newIsland.islandManager = this;
body1.island = newIsland;
body1.island.bodies.Add(body1);
islands.Add(newIsland);
}
}
// CUSTOM: Modified this as well (to ensure that kinematic bodies can properly wake up dynamic bodies).
bool shouldMerge = (isBody1Movable && isBody2Movable) || (int)type1 + (int)type2 == 1;
// Both are movable.
if (shouldMerge)
{
MergeIslands(body1, body2);
body1.connections.Add(body2);
body2.connections.Add(body1);
}
}
/// <summary>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public Contact AddContact(JVector point1, JVector point2, JVector normal, float penetration,
ContactSettings contactSettings)
{
JVector relPos1;
JVector.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 2)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = poolContact.GetNew();
contact.Prepare(poolContact);
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
private void AddConnection(RigidBody body1, RigidBody body2)
{
System.Diagnostics.Debug.Assert(!(body1.isStatic && body2.isStatic),
"IslandManager Inconsistency.",
"Arbiter detected between two static objects.");
if (body1.isStatic) // <- only body1 is static
{
if (body2.island == null)
{
CollisionIsland newIsland = Pool.GetNew();
newIsland.islandManager = this;
body2.island = newIsland;
body2.island.bodies.Add(body2);
islands.Add(newIsland);
}
}
else if (body2 == null || body2.isStatic) // <- only body2 is static
{
if (body1.island == null)
{
CollisionIsland newIsland = Pool.GetNew();
newIsland.islandManager = this;
body1.island = newIsland;
body1.island.bodies.Add(body1);
islands.Add(newIsland);
}
}
else // both are !static
{
MergeIslands(body1, body2);
body1.connections.Add(body2);
body2.connections.Add(body1);
}
}
private void AddConnection(RigidBody body1, RigidBody body2)
{
System.Diagnostics.Debug.Assert(!(body1.isStatic && body2.isStatic),
"IslandManager Inconsistency: Arbiter detected between two static objects.");
if (body1.isStatic)
{
if (body2.island == null)
{
var newIsland = Pool.GetNew();
newIsland.islandManager = this;
body2.island = newIsland;
body2.island.bodies.Add(body2);
islands.Add(newIsland);
}
}
else if (body2?.isStatic != false)
{
if (body1.island == null)
{
var newIsland = Pool.GetNew();
newIsland.islandManager = this;
body1.island = newIsland;
body1.island.bodies.Add(body1);
islands.Add(newIsland);
}
}
else
{
MergeIslands(body1, body2);
body1.connections.Add(body2);
body2.connections.Add(body1);
}
}
/// <summary>
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
/// </summary>
/// <param name="callback">The callback.</param>
/// <param name="aabb">The aabb.</param>
public void Query(List <int> my, ref BBox aabb)
{
//Stack<int> _stack = new Stack<int>(256);
Stack <int> _stack = stackPool.GetNew();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode <T> node = _nodes[nodeId];
//if (JBBox.TestOverlap(ref node.AABB, ref aabb))
if (aabb.Contains(ref node.AABB) != BBox.ContainmentType.Disjoint)
{
if (node.IsLeaf())
{
my.Add(nodeId);
//bool proceed = callback(nodeId);
//if (proceed == false)
//{
// return;
//}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
stackPool.GiveBack(_stack);
}
/// <summary>
/// Checks if given point is within a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref TSMatrix orientation, ref TSVector position, ref TSVector point)
{
TSVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
TSVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
TSVector r; support.SupportCenter(out r);
TSVector.Transform(ref r, ref orientation, out r);
TSVector.Add(ref position, ref r, out r);
TSVector.Subtract(ref point, ref r, out r);
TSVector x = point;
TSVector w, p;
FP VdotR;
TSVector v; TSVector.Subtract(ref x, ref arbitraryPoint, out v);
FP dist = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
TSVector.Subtract(ref x, ref p, out w);
FP VdotW = TSVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = TSVector.Dot(ref v, ref r);
if (VdotR >= -(TSMath.Epsilon * TSMath.Epsilon))
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
simplexSolver.Reset();
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
dist = v.sqrMagnitude;
}
else
{
dist = FP.Zero;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
