/// <summary>
/// Inverses the direction of a vector.
/// </summary>
/// <param name="value">The vector to inverse.</param>
/// <returns>The negated vector.</returns>
public static JVector Negate(JVector value)
{
JVector result;
JVector.Negate(ref value, out result);
return(result);
}
//object locker = new object();
private void CollisionDetected(RigidBody body1, RigidBody body2, JVector point1, JVector point2, JVector normal,
float penetration)
{
Arbiter arbiter;
arbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1;
arbiter.body2 = body2;
arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
addedArbiterQueue.Enqueue(arbiter);
events.RaiseBodiesBeginCollide(body1, body2);
}
Contact contact;
if (arbiter.body1 == body1)
{
JVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (contact != null) events.RaiseContactCreated(contact);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector temp;
JVector.Transform(ref halfSize, ref abs, out temp);
box.Max = temp;
JVector.Negate(ref temp, out box.Min);
}
/// <summary>
/// Discrete Circle vs Circle test. Very fast. Generates contact info.
/// NOTE: check distance for collisions. If negative then a collision has occurred.
/// This is done to remove all branches from this test and leave it to the user to decide when to branch.
/// </summary>
public static void CircleCircleTest(JVector centerA, float radiusA, JVector centerB, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// ||A-B|| - (r1+r2) < 0
float d = JVector.DistanceSquared(centerA, centerB);
float r = (radiusA + radiusB);
r *= r;
distance = d - r;
normal = (centerA - centerB) / d;
normal.Normalize();
// calculate closest 2 points
pointA = JVector.Negate(normal) * radiusA + centerA;
pointB = normal * radiusB + centerB;
}
public static bool OriginInTriangle(JVector a, JVector b, JVector c)
{
float pab = JVector.Cross(JVector.Negate(a), b - a);
float pbc = JVector.Cross(JVector.Negate(b), c - b);
if (!SameSign(pab, pbc))
{
return(false);
}
float pca = JVector.Cross(JVector.Negate(c), a - c);
if (!SameSign(pab, pca))
{
return(false);
}
return(true);
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref JVector point, ref JVector normal,
out JVector point1, out JVector point2)
{
JVector mn; JVector.Negate(ref normal, out mn);
JVector sA; SupportMapping(body1, shape1, ref mn, out sA);
JVector sB; SupportMapping(body2, shape2, ref normal, out sB);
JVector.Subtract(ref sA, ref point, out sA);
JVector.Subtract(ref sB, ref point, out sB);
float dot1 = JVector.Dot(ref sA, ref normal);
float dot2 = JVector.Dot(ref sB, ref normal);
JVector.Multiply(ref normal, dot1, out sA);
JVector.Multiply(ref normal, dot2, out sB);
JVector.Add(ref point, ref sA, out point1);
JVector.Add(ref point, ref sB, out point2);
}
internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// get capsule endpoints
var p0 = centerB - axis * (length * 0.5f);
var p1 = centerB + axis * (length * 0.5f);
// get vector from endpoint to circle
var D = centerA - p0;
// project vector onto axis and clamp
var d = JVector.Dot(D, axis);
d = JMath.Clamp(d, 0, length);
// get point on axis
var R = p0 + axis * d;
// distance
var b = Math.Abs((centerA - R).Length());
normal = (centerA - R) / b;
// calculate closest 2 points
var RH = JVector.Normalize(centerA - R);
pointA = JVector.Negate(RH) * radiusA + centerA;
pointB = RH * radiusB + R;
normal.Negate();
distance = b - (radiusA + radiusB);
//
if (b < radiusA + radiusB)
{
return(true);
}
return(false);
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector point, out JVector normal, out float penetration)
{
// Used variables
JVector temp1, temp2;
JVector v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector v41, v42, v4;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
JVector.Transform(ref v01, ref orientation1, out v01);
JVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
JVector.Transform(ref v02, ref orientation2, out v02);
JVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
JVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new JVector(0.00001f, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
JVector.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
JVector.Subtract(ref v12, ref v11, out v1);
if (JVector.Dot(ref v1, ref normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
JVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
JVector.Subtract(ref v1, ref v0, out normal);
normal.Normalize();
point = v11;
JVector.Add(ref point, ref v12, out point);
JVector.Multiply(ref point, 0.5f, out point);
JVector.Subtract(ref v12, ref v11, out temp1);
penetration = JVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
JVector.Subtract(ref v22, ref v21, out v2);
if (JVector.Dot(ref v2, ref normal) <= 0.0f)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
float dist = JVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0.0f)
{
JVector.Swap(ref v1, ref v2);
JVector.Swap(ref v11, ref v21);
JVector.Swap(ref v12, ref v22);
JVector.Negate(ref normal, out normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
JVector.Subtract(ref v32, ref v31, out v3);
if (JVector.Dot(ref v3, ref normal) <= 0.0f)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
JVector.Cross(ref v1, ref v3, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v2 = v3;
v21 = v31;
v22 = v32;
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v3, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
JVector.Cross(ref v3, ref v2, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v1 = v3;
v11 = v31;
v12 = v32;
JVector.Subtract(ref v3, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
// Compute normal of the wedge face
JVector.Subtract(ref v2, ref v1, out temp1);
JVector.Subtract(ref v3, ref v1, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
// Compute distance from origin to wedge face
float d = JVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
JVector.Subtract(ref v42, ref v41, out v4);
JVector.Subtract(ref v4, ref v3, out temp1);
float delta = JVector.Dot(ref temp1, ref normal);
penetration = JVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
{
if (hit)
{
JVector.Cross(ref v1, ref v2, out temp1);
float b0 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v3, ref v2, out temp1);
float b1 = JVector.Dot(ref temp1, ref v0);
JVector.Cross(ref v0, ref v1, out temp1);
float b2 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v2, ref v1, out temp1);
float b3 = JVector.Dot(ref temp1, ref v0);
float sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
JVector.Cross(ref v2, ref v3, out temp1);
b1 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v3, ref v1, out temp1);
b2 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v1, ref v2, out temp1);
b3 = JVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
float inv = 1.0f / sum;
JVector.Multiply(ref v01, b0, out point);
JVector.Multiply(ref v11, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v21, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v31, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v02, b0, out temp2);
JVector.Add(ref temp2, ref point, out point);
JVector.Multiply(ref v12, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v22, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v32, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref point, inv * 0.5f, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
////// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//float d1 = JVector.Dot(ref temp1, ref v0);
////// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//float d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
JVector.Cross(ref v4, ref v0, out temp1);
float dot = JVector.Dot(ref temp1, ref v1);
if (dot >= 0.0f)
{
dot = JVector.Dot(ref temp1, ref v2);
if (dot >= 0.0f)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
dot = JVector.Dot(ref temp1, ref v3);
if (dot >= 0.0f)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction)
{
fraction = float.MaxValue; normal = JVector.Zero;
if (!body.BoundingBox.RayIntersect(rayOrigin, rayDirection))
{
return(false);
}
if (body.Shape is Multishape multishape)
{
multishape = multishape.RequestWorkingClone();
bool multiShapeCollides = false;
JVector.Subtract(rayOrigin, body.position, out var transformedOrigin);
JVector.Transform(transformedOrigin, body.invOrientation, out transformedOrigin);
JVector.Transform(rayDirection, body.invOrientation, out var transformedDirection);
int msLength = multishape.Prepare(transformedOrigin, transformedDirection);
for (int i = 0; i < msLength; i++)
{
multishape.SetCurrentShape(i);
if (GJKCollide.Raycast(
multishape,
body.orientation,
body.position,
rayOrigin,
rayDirection,
out float tempFraction,
out var tempNormal) &&
tempFraction < fraction)
{
if (useTerrainNormal && multishape is TerrainShape terrainShape)
{
terrainShape.CollisionNormal(out tempNormal);
JVector.Transform(tempNormal, body.orientation, out tempNormal);
tempNormal = JVector.Negate(tempNormal);
}
else if (useTriangleMeshNormal && multishape is TriangleMeshShape triangleMeshShape)
{
triangleMeshShape.CollisionNormal(out tempNormal);
JVector.Transform(tempNormal, body.orientation, out tempNormal);
tempNormal = JVector.Negate(tempNormal);
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
multishape.ReturnWorkingClone();
return(multiShapeCollides);
}
else
{
return(GJKCollide.Raycast(
body.Shape,
body.orientation,
body.position,
rayOrigin,
rayDirection,
out fraction,
out normal));
}
}
/*
* private ResourcePool<List<int>> potentialTriangleLists = new ResourcePool<List<int>>();
*
* private void DetectSoftSoft(SoftBody body1, SoftBody body2)
* {
* List<int> my = potentialTriangleLists.GetNew();
* List<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]);
*
* JVector point, normal;
* float penetration;
* bool result;
*
* result = XenoCollide.Detect(myTriangle, otherTriangle, ref JMatrix.InternalIdentity, ref JMatrix.InternalIdentity,
* ref JVector.InternalZero, ref JVector.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);
*
* if (this.RaisePassedNarrowphase(body1.points[minIndexMy], body2.points[minIndexOther],
* ref point, ref normal, penetration))
* {
* RaiseCollisionDetected(body1.points[minIndexMy],
* body2.points[minIndexOther], ref point, ref point, ref normal, penetration);
* }
* }
* }
*
* my.Clear(); other.Clear();
* potentialTriangleLists.GiveBack(my);
* potentialTriangleLists.GiveBack(other);
* }
*/
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
// we don't support multi shapes yet!
bool b1IsMulti = false; // (body1.Shape is Multishape);
bool b2IsMulti = false; // (body2.Shape is Multishape);
bool speculative = speculativeContacts ||
(body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);
JVector point = JVector.Zero;
JVector normal = JVector.Zero;
float penetration = 0;
if (!b1IsMulti && !b2IsMulti)
{
JVector point1 = JVector.Zero;
JVector point2 = JVector.Zero;
JMatrix OA = JMatrix.CreateRotationZ(body1.orientation);
JMatrix OB = JMatrix.CreateRotationZ(body2.orientation);
JVector zero = JVector.Zero;
float t = 0.0f;
if (body1.Shape.type == ShapeType.Box && body2.Shape.type == ShapeType.Box)
{
var A = body1.Shape as BoxShape;
var B = body2.Shape as BoxShape;
A.UpdateAxes(body1.orientation);
B.UpdateAxes(body2.orientation);
JVector[] CA = new JVector[2], CB = new JVector[2];
int Cnum = 0;
if (Collision.BoxBoxTestContact(ref A, ref body1.position, ref OA,
ref B, ref body2.position, ref OB,
out normal, out t, out CA, out CB, out Cnum))
{
normal.Negate();
RaiseCollisionDetected(body1, body2, ref CA[0], ref CB[0], ref normal, -t);
}
}
else if (body1.Shape.type == ShapeType.Circle && body2.Shape.type == ShapeType.Circle)
{
var A = body1.Shape as CircleShape;
var B = body2.Shape as CircleShape;
Collision.CircleCircleTest(body1.position, A.Radius, body2.position, B.Radius, out point1, out point2, out normal, out penetration);
if (penetration < 0)
{
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, -penetration);
}
}
// all shapes (GJK)
else if (speculative)
{
//JVector hit1, hit2;
//if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref OA, ref OB,
// ref body1.position, ref body2.position, out hit1, out hit2, out normal))
//{
// JVector delta = hit2 - hit1;
// if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
// {
// //normal.Negate();
// penetration = delta * normal;
// if (penetration < 0.0f)
// {
// RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
// }
// }
//}
}
}
else if (b1IsMulti && b2IsMulti)
{
/*
* Multishape ms1 = (body1.Shape as Multishape);
* Multishape ms2 = (body2.Shape as Multishape);
*
* ms1 = ms1.RequestWorkingClone();
* ms2 = ms2.RequestWorkingClone();
*
* JBBox transformedBoundingBox = body2.boundingBox;
* transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);
*
* int ms1Length = ms1.Prepare(ref transformedBoundingBox);
*
* transformedBoundingBox = body1.boundingBox;
* transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);
*
* int ms2Length = ms2.Prepare(ref transformedBoundingBox);
*
* if (ms1Length == 0 || ms2Length == 0)
* {
* ms1.ReturnWorkingClone();
* ms2.ReturnWorkingClone();
* return;
* }
*
* for (int i = 0; i < ms1Length; i++)
* {
* ms1.SetCurrentShape(i);
*
* for (int e = 0; e < ms2Length; e++)
* {
* ms2.SetCurrentShape(e);
*
* if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
* ref body2.orientation, ref body1.position, ref body2.position,
* out point, out normal, out penetration))
* {
* if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration))
* {
* JVector point1, point2;
* FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
*
* RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
* }
* }
* }
* }
*
* ms1.ReturnWorkingClone();
* ms2.ReturnWorkingClone();
* */
}
else
{
/*
* RigidBody b1, b2;
*
* if (body2.Shape is Multishape) { b1 = body2; b2 = body1; }
* else { b2 = body2; b1 = body1; }
*
* Multishape ms = (b1.Shape as Multishape);
*
* ms = ms.RequestWorkingClone();
*
* JBBox transformedBoundingBox = b2.boundingBox;
* transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);
*
* int msLength = ms.Prepare(ref transformedBoundingBox);
*
* if (msLength == 0)
* {
* ms.ReturnWorkingClone();
* return;
* }
*
* for (int i = 0; i < msLength; i++)
* {
* ms.SetCurrentShape(i);
*
* if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
* ref b2.orientation, ref b1.position, ref b2.position,
* out point, out normal, out penetration))
* {
* if (this.RaisePassedNarrowphase(b1, b2, ref point, ref normal, penetration))
* {
* JVector point1, point2;
* FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);
*
* if (useTerrainNormal && ms is TerrainShape)
* {
* (ms as TerrainShape).CollisionNormal(out normal);
* JVector.Transform(ref normal, ref b1.orientation, out normal);
* }
* else if (useTriangleMeshNormal && ms is TriangleMeshShape)
* {
* (ms as TriangleMeshShape).CollisionNormal(out normal);
* JVector.Transform(ref normal, ref b1.orientation, out normal);
* }
*
* RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
* }
* }
* }
*
* ms.ReturnWorkingClone();
* */
}
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector p1, out JVector p2, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = JVector.Zero;
JVector r = position1 - position2;
JVector w, v;
JVector supVertexA;
JVector rn, vn;
rn = JVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
JVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = JVector.Zero;
int maxIter = 15;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = JVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector point, out JVector normal, out float penetration)
{
// Used variables
JVector v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
JVector.Transform(ref v01, ref orientation1, out v01);
JVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
JVector.Transform(ref v02, ref orientation2, out v02);
JVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
JVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new JVector(0.00001f, 0);
}
// v1 = support in direction of origin
mn = v0;
JVector.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
JVector.Subtract(ref v12, ref v11, out v1);
if (JVector.Dot(ref v1, ref normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
normal = OutsidePortal(v1, v0);
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
JVector.Subtract(ref v22, ref v21, out v2);
//LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue);
//LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue);
if (JVector.Dot(ref v2, ref normal) <= 0.0f)
{
return(false);
}
// phase two: portal refinement
int maxIterations = 0;
while (true)
{
// find normal direction
if (!IntersectPortal(v0, v2, v1))
{
normal = InsidePortal(v2, v1);
}
else
{
// origin ray crosses the portal
normal = OutsidePortal(v2, v1);
}
// obtain the next support point
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
JVector.Subtract(ref v32, ref v31, out v3);
//LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green);
if (JVector.Dot(v3, normal) <= 0)
{
JVector ab = v3 - v2;
float t = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab));
normal = (v2 + (t * ab));
return(false);
}
// Portal lies on the outside edge of the Minkowski Hull.
// Return contact information
if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations)
{
JVector ab = v2 - v1;
float t = JVector.Dot(JVector.Negate(v1), ab);
if (t <= 0.0f)
{
t = 0.0f;
normal = v1;
}
else
{
float denom = JVector.Dot(ab, ab);
if (t >= denom)
{
normal = v2;
t = 1.0f;
}
else
{
t /= denom;
normal = v1 + t * ab;
}
}
float s = 1 - t;
point = s * v11 + t * v21;
var point2 = s * v12 + t * v22;
// this causes a sq root = bad!
penetration = normal.Length();
normal.Normalize();
return(true);
}
// if origin is inside (v1, v0, v3), refine portal
if (OriginInTriangle(v0, v1, v3))
{
v2 = v3;
v21 = v31;
v22 = v32;
continue;
}
// if origin is inside (v3, v0, v2), refine portal
else if (OriginInTriangle(v0, v2, v3))
{
v1 = v3;
v11 = v31;
v12 = v32;
continue;
}
return(false);
}
}
