public void ComputeCollision()
{
Gjk algo = new Gjk(new CollisionDetection());
CollisionObject a = new CollisionObject
{
GeometricObject = new GeometricObject(new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1)), Pose.Identity),
};
CollisionObject b = new CollisionObject
{
GeometricObject = new GeometricObject(new SphereShape(1), Pose.Identity),
};
ContactSet set;
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(0, set[0].PenetrationDepth);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0.1f, 0.2f));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(-1, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.01f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(1, 0.1f, 0.2f), set[0].PositionBWorld, 0.01f));
}
public void ComputeCollision()
{
Gjk algo = new Gjk(new CollisionDetection());
CollisionObject a = new CollisionObject
{
GeometricObject = new GeometricObject(new TriangleShape(new Vector3(0, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)), Pose.Identity),
};
CollisionObject b = new CollisionObject
{
GeometricObject = new GeometricObject(new SphereShape(1), Pose.Identity),
};
ContactSet set;
set = algo.GetClosestPoints(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(0, set[0].PenetrationDepth);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(2, 0.1f, 0.2f));
algo.UpdateClosestPoints(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(-1, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.01f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(1, 0.1f, 0.2f), set[0].PositionBWorld, 0.01f));
}
private void UpdateDeterminant(int xmIdx)
{
int index1 = 1 << xmIdx;
this.det[index1][xmIdx] = 1f;
int bitsToIndex = Gjk.bitsToIndices[this.simplexBits];
int num1 = bitsToIndex;
int num2 = 0;
while (num1 != 0)
{
int index2 = (num1 & 7) - 1;
int num3 = 1 << index2;
int index3 = num3 | index1;
this.det[index3][index2] = Gjk.Dot(ref this.edges[xmIdx][index2], ref this.y[xmIdx]);
this.det[index3][xmIdx] = Gjk.Dot(ref this.edges[index2][xmIdx], ref this.y[index2]);
int num4 = bitsToIndex;
for (int index4 = 0; index4 < num2; ++index4)
{
int index5 = (num4 & 7) - 1;
int num5 = 1 << index5;
int index6 = index3 | num5;
int index7 = (double)this.edgeLengthSq[index2][index5] < (double)this.edgeLengthSq[xmIdx][index5] ? index2 : xmIdx;
this.det[index6][index5] = (float)((double)this.det[index3][index2] * (double)Gjk.Dot(ref this.edges[index7][index5], ref this.y[index2]) + (double)this.det[index3][xmIdx] * (double)Gjk.Dot(ref this.edges[index7][index5], ref this.y[xmIdx]));
int index8 = (double)this.edgeLengthSq[index5][index2] < (double)this.edgeLengthSq[xmIdx][index2] ? index5 : xmIdx;
this.det[index6][index2] = (float)((double)this.det[num5 | index1][index5] * (double)Gjk.Dot(ref this.edges[index8][index2], ref this.y[index5]) + (double)this.det[num5 | index1][xmIdx] * (double)Gjk.Dot(ref this.edges[index8][index2], ref this.y[xmIdx]));
int index9 = (double)this.edgeLengthSq[index2][xmIdx] < (double)this.edgeLengthSq[index5][xmIdx] ? index2 : index5;
this.det[index6][xmIdx] = (float)((double)this.det[num3 | num5][index5] * (double)Gjk.Dot(ref this.edges[index9][xmIdx], ref this.y[index5]) + (double)this.det[num3 | num5][index2] * (double)Gjk.Dot(ref this.edges[index9][xmIdx], ref this.y[index2]));
num4 >>= 3;
}
num1 >>= 3;
++num2;
}
if ((this.simplexBits | index1) != 15)
{
return;
}
int index10 = (double)this.edgeLengthSq[1][0] < (double)this.edgeLengthSq[2][0] ? ((double)this.edgeLengthSq[1][0] < (double)this.edgeLengthSq[3][0] ? 1 : 3) : ((double)this.edgeLengthSq[2][0] < (double)this.edgeLengthSq[3][0] ? 2 : 3);
this.det[15][0] = (float)((double)this.det[14][1] * (double)Gjk.Dot(ref this.edges[index10][0], ref this.y[1]) + (double)this.det[14][2] * (double)Gjk.Dot(ref this.edges[index10][0], ref this.y[2]) + (double)this.det[14][3] * (double)Gjk.Dot(ref this.edges[index10][0], ref this.y[3]));
int index11 = (double)this.edgeLengthSq[0][1] < (double)this.edgeLengthSq[2][1] ? ((double)this.edgeLengthSq[0][1] < (double)this.edgeLengthSq[3][1] ? 0 : 3) : ((double)this.edgeLengthSq[2][1] < (double)this.edgeLengthSq[3][1] ? 2 : 3);
this.det[15][1] = (float)((double)this.det[13][0] * (double)Gjk.Dot(ref this.edges[index11][1], ref this.y[0]) + (double)this.det[13][2] * (double)Gjk.Dot(ref this.edges[index11][1], ref this.y[2]) + (double)this.det[13][3] * (double)Gjk.Dot(ref this.edges[index11][1], ref this.y[3]));
int index12 = (double)this.edgeLengthSq[0][2] < (double)this.edgeLengthSq[1][2] ? ((double)this.edgeLengthSq[0][2] < (double)this.edgeLengthSq[3][2] ? 0 : 3) : ((double)this.edgeLengthSq[1][2] < (double)this.edgeLengthSq[3][2] ? 1 : 3);
this.det[15][2] = (float)((double)this.det[11][0] * (double)Gjk.Dot(ref this.edges[index12][2], ref this.y[0]) + (double)this.det[11][1] * (double)Gjk.Dot(ref this.edges[index12][2], ref this.y[1]) + (double)this.det[11][3] * (double)Gjk.Dot(ref this.edges[index12][2], ref this.y[3]));
int index13 = (double)this.edgeLengthSq[0][3] < (double)this.edgeLengthSq[1][3] ? ((double)this.edgeLengthSq[0][3] < (double)this.edgeLengthSq[2][3] ? 0 : 2) : ((double)this.edgeLengthSq[1][3] < (double)this.edgeLengthSq[2][3] ? 1 : 2);
this.det[15][3] = (float)((double)this.det[7][0] * (double)Gjk.Dot(ref this.edges[index13][3], ref this.y[0]) + (double)this.det[7][1] * (double)Gjk.Dot(ref this.edges[index13][3], ref this.y[1]) + (double)this.det[7][2] * (double)Gjk.Dot(ref this.edges[index13][3], ref this.y[2]));
}
public void TestCoplanar()
{
var tta = new TriangleTriangleAlgorithm(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
int numberOfContacts = 0;
int numberOfTests = 1000;
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < numberOfTests; i++)
{
// Create two triangles.
var tA = new Triangle();
tA.Vertex0 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex1 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex2 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var tB = new Triangle();
tB.Vertex0 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex1 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex2 = new Vector3(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var ttTest = GeometryHelper.HaveContact(tA, tB);
if (ttTest)
{
numberOfContacts++;
}
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var csTta = tta.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
if (csTta.HaveContact != csGjk.HaveContact)
{
Trace.WriteLine("Test failed: " + i + " GJK: " + csGjk.HaveContact + " TTA: " + csTta);
}
Assert.AreEqual(ttTest, csGjk.HaveContact);
Assert.AreEqual(ttTest, csTta.HaveContact);
}
//Trace.WriteLine("% hits:" + 100f * numberOfContacts / numberOfTests);
}
public void TestTriangles()
{
var mpr = new MinkowskiPortalRefinement(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < 100000; i++)
{
// Create two triangles in a plane.
var tA = new Triangle();
tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
var tB = new Triangle();
tB.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tB.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tB.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var csMpr = mpr.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
// Test again using boolean query.
bool haveMprContact = mpr.HaveContact(coA, coB);
// The exceptions are tested cases - all extreme shallow surface contacts.
if (i != 28487 && i != 47846 && i != 97305)
{
Assert.AreEqual(csGjk.HaveContact, haveMprContact);
Assert.AreEqual(csGjk.HaveContact, csMpr.HaveContact);
}
}
}
public void GjkRealTest()
{
Gjk gjk = new Gjk();
BoundingSphere shape1 = new BoundingSphere(new Vector3(30, 52, 119), 91);
BoundingSphere shape2 = new BoundingSphere(new Vector3(139, 13, 35), 49);
float expected = Collision.DistanceSphereSphere(ref shape1, ref shape2);
//BoundingSphere shape1 = new BoundingSphere(new Vector3(-10.01f, 99f, 2.7f), 0.0001f);
//BoundingSphere shape2 = new BoundingSphere(new Vector3(12.7f, 2f, -120.07f), 0.0001f);
//BoundingSphere shape1 = new BoundingSphere(new Vector3(0, 15, 0), 1);
//BoundingBox shape1 = new BoundingBox(new Vector3(0, 0, 0), new Vector3(1, 1, 1));
//BoundingBox shape2 = new BoundingBox(new Vector3(0, 2, 0), new Vector3(3, 3, 3));
float distance = gjk.GetMinimumDistance(v =>
{
Vector3 a = shape1.SupportMapping(v);
Vector3 b = shape2.SupportMapping(-v);
return(a - b);
}, maxIterations: 50);
}
public CollisionAlgorithmMatrix(CollisionDetection collisionDetection)
{
// Initialize with dummy collision algorithms.
var noAlgo = new NoCollisionAlgorithm(collisionDetection); // Definitely no collision wanted.
var infiniteAlgo = new InfiniteShapeAlgorithm(collisionDetection); // Returns always a collision.
// Build default configuration:
var gjk = new Gjk(collisionDetection);
var gjkBoxAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new BoxBoxAlgorithm(collisionDetection));
var gjkMprAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new MinkowskiPortalRefinement(collisionDetection));
var gjkTriTriAlgorithm = new CombinedCollisionAlgorithm(collisionDetection, gjk, new TriangleTriangleAlgorithm(collisionDetection));
BoxSphereAlgorithm boxSphereAlgorithm = new BoxSphereAlgorithm(collisionDetection);
CompositeShapeAlgorithm compositeAlgorithm = new CompositeShapeAlgorithm(collisionDetection);
HeightFieldAlgorithm heightFieldAlgorithm = new HeightFieldAlgorithm(collisionDetection);
LineAlgorithm lineAlgorithm = new LineAlgorithm(collisionDetection);
PlaneBoxAlgorithm planeBoxAlgorithm = new PlaneBoxAlgorithm(collisionDetection);
PlaneConvexAlgorithm planeConvexAlgorithm = new PlaneConvexAlgorithm(collisionDetection);
PlaneRayAlgorithm planeRayAlgorithm = new PlaneRayAlgorithm(collisionDetection);
PlaneSphereAlgorithm planeSphereAlgorithm = new PlaneSphereAlgorithm(collisionDetection);
RayBoxAlgorithm rayBoxAlgorithm = new RayBoxAlgorithm(collisionDetection);
RayConvexAlgorithm rayConvexAlgorithm = new RayConvexAlgorithm(collisionDetection);
RaySphereAlgorithm raySphereAlgorithm = new RaySphereAlgorithm(collisionDetection);
RayTriangleAlgorithm rayTriangleAlgorithm = new RayTriangleAlgorithm(collisionDetection);
SphereSphereAlgorithm sphereSphereAlgorithm = new SphereSphereAlgorithm(collisionDetection);
TransformedShapeAlgorithm transformedShapeAlgorithm = new TransformedShapeAlgorithm(collisionDetection);
TriangleMeshAlgorithm triangleMeshAlgorithm = new TriangleMeshAlgorithm(collisionDetection);
RayCompositeAlgorithm rayCompositeAlgorithm = new RayCompositeAlgorithm(collisionDetection);
RayTriangleMeshAlgorithm rayTriangleMeshAlgorithm = new RayTriangleMeshAlgorithm(collisionDetection);
RayHeightFieldAlgorithm rayHeightFieldAlgorithm = new RayHeightFieldAlgorithm(collisionDetection);
this[typeof(PointShape), typeof(PointShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(PointShape), typeof(RayShape)] = rayConvexAlgorithm;
this[typeof(PointShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(PointShape), typeof(EmptyShape)] = noAlgo;
this[typeof(PointShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(PointShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(PointShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(PointShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PointShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PointShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(LineShape), typeof(LineShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RayShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(LineSegmentShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TriangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(RectangleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(BoxShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ScaledConvexShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CircleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(SphereShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CapsuleShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(ConeShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CylinderShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineShape), typeof(PlaneShape)] = noAlgo;
this[typeof(LineShape), typeof(HeightField)] = noAlgo;
this[typeof(LineShape), typeof(TriangleMeshShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(TransformedShape)] = lineAlgorithm;
this[typeof(LineShape), typeof(CompositeShape)] = lineAlgorithm;
this[typeof(RayShape), typeof(RayShape)] = noAlgo;
this[typeof(RayShape), typeof(LineSegmentShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(TriangleShape)] = rayTriangleAlgorithm;
this[typeof(RayShape), typeof(RectangleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(BoxShape)] = rayBoxAlgorithm;
this[typeof(RayShape), typeof(ConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ScaledConvexShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CircleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(SphereShape)] = raySphereAlgorithm;
this[typeof(RayShape), typeof(CapsuleShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(ConeShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(CylinderShape)] = rayConvexAlgorithm;
this[typeof(RayShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RayShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RayShape), typeof(PlaneShape)] = planeRayAlgorithm;
this[typeof(RayShape), typeof(HeightField)] = rayHeightFieldAlgorithm;
this[typeof(RayShape), typeof(TriangleMeshShape)] = rayTriangleMeshAlgorithm;
this[typeof(RayShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RayShape), typeof(CompositeShape)] = rayCompositeAlgorithm;
this[typeof(LineSegmentShape), typeof(LineSegmentShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(LineSegmentShape), typeof(EmptyShape)] = noAlgo;
this[typeof(LineSegmentShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(LineSegmentShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(LineSegmentShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(LineSegmentShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(LineSegmentShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(LineSegmentShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TriangleShape), typeof(TriangleShape)] = gjkTriTriAlgorithm;
this[typeof(TriangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(TriangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(TriangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(TriangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(TriangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(TriangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(RectangleShape), typeof(RectangleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(BoxShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(RectangleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(RectangleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(RectangleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(RectangleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(RectangleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(RectangleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(RectangleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(BoxShape), typeof(BoxShape)] = gjkBoxAlgorithm;
this[typeof(BoxShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(SphereShape)] = boxSphereAlgorithm;
this[typeof(BoxShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(BoxShape), typeof(EmptyShape)] = noAlgo;
this[typeof(BoxShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(BoxShape), typeof(PlaneShape)] = planeBoxAlgorithm;
this[typeof(BoxShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(BoxShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(BoxShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(BoxShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConvexShape), typeof(ConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ScaledConvexShape), typeof(ScaledConvexShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ScaledConvexShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ScaledConvexShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ScaledConvexShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ScaledConvexShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ScaledConvexShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ScaledConvexShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ScaledConvexShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CircleShape), typeof(CircleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(SphereShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CircleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CircleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CircleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CircleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CircleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CircleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CircleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(SphereShape), typeof(SphereShape)] = sphereSphereAlgorithm;
this[typeof(SphereShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(SphereShape), typeof(EmptyShape)] = noAlgo;
this[typeof(SphereShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(SphereShape), typeof(PlaneShape)] = planeSphereAlgorithm;
this[typeof(SphereShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(SphereShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(SphereShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(SphereShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CapsuleShape), typeof(CapsuleShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CapsuleShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CapsuleShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CapsuleShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CapsuleShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CapsuleShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CapsuleShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CapsuleShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(ConeShape), typeof(ConeShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(ConeShape), typeof(EmptyShape)] = noAlgo;
this[typeof(ConeShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(ConeShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(ConeShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(ConeShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(ConeShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(ConeShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(CylinderShape), typeof(CylinderShape)] = gjkMprAlgorithm;
this[typeof(CylinderShape), typeof(EmptyShape)] = noAlgo;
this[typeof(CylinderShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(CylinderShape), typeof(PlaneShape)] = planeConvexAlgorithm;
this[typeof(CylinderShape), typeof(HeightField)] = heightFieldAlgorithm;
this[typeof(CylinderShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(CylinderShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(CylinderShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(EmptyShape), typeof(EmptyShape)] = noAlgo;
this[typeof(EmptyShape), typeof(InfiniteShape)] = noAlgo; // No collision between Empty and Infinite.
this[typeof(EmptyShape), typeof(PlaneShape)] = noAlgo;
this[typeof(EmptyShape), typeof(HeightField)] = noAlgo;
this[typeof(EmptyShape), typeof(TriangleMeshShape)] = noAlgo;
this[typeof(EmptyShape), typeof(TransformedShape)] = noAlgo;
this[typeof(EmptyShape), typeof(CompositeShape)] = noAlgo;
this[typeof(InfiniteShape), typeof(InfiniteShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(PlaneShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(HeightField)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TriangleMeshShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(TransformedShape)] = infiniteAlgo;
this[typeof(InfiniteShape), typeof(CompositeShape)] = infiniteAlgo;
this[typeof(PlaneShape), typeof(PlaneShape)] = noAlgo;
this[typeof(PlaneShape), typeof(HeightField)] = noAlgo;
this[typeof(PlaneShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(PlaneShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(PlaneShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(HeightField), typeof(HeightField)] = noAlgo;
// We could also call triangleMeshAlgorithm. But since HeightField has usually larger parts it
// is better to call the heightFieldAlgorithm. The heightFieldAlgorithm will cull all but a
// few height field cells very quickly.
this[typeof(HeightField), typeof(TriangleMeshShape)] = heightFieldAlgorithm;
this[typeof(HeightField), typeof(TransformedShape)] = transformedShapeAlgorithm;
// Same as for triangle meshes: Call height field algorithm first.
this[typeof(HeightField), typeof(CompositeShape)] = heightFieldAlgorithm;
this[typeof(TriangleMeshShape), typeof(TriangleMeshShape)] = triangleMeshAlgorithm;
this[typeof(TriangleMeshShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TriangleMeshShape), typeof(CompositeShape)] = compositeAlgorithm;
this[typeof(TransformedShape), typeof(TransformedShape)] = transformedShapeAlgorithm;
this[typeof(TransformedShape), typeof(CompositeShape)] = transformedShapeAlgorithm;
this[typeof(CompositeShape), typeof(CompositeShape)] = compositeAlgorithm;
}
public void TestCoplanar()
{
var tta = new TriangleTriangleAlgorithm(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
int numberOfContacts = 0;
int numberOfTests = 1000;
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < numberOfTests; i++)
{
// Create two triangles.
var tA = new Triangle();
tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var tB = new Triangle();
tB.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var ttTest = GeometryHelper.HaveContact(tA, tB);
if (ttTest)
numberOfContacts++;
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var csTta = tta.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
if (csTta.HaveContact != csGjk.HaveContact)
Trace.WriteLine("Test failed: " + i + " GJK: " + csGjk.HaveContact + " TTA: " + csTta);
Assert.AreEqual(ttTest, csGjk.HaveContact);
Assert.AreEqual(ttTest, csTta.HaveContact);
}
//Trace.WriteLine("% hits:" + 100f * numberOfContacts / numberOfTests);
}
public void TestCoplanarTriangles2()
{
var mpr = new MinkowskiPortalRefinement(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < 100000; i++)
{
// Create two triangles in a plane.
var tA = new Triangle();
tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var tB = new Triangle();
tB.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
// Test if 2D triangles have contact
bool haveContact = false;
// Test if vertices are inside the other triangle.
for (int j = 0; j < 2 && haveContact == false; j++)
{
var t0 = tA;
var t1 = tB;
if (j == 1)
{
MathHelper.Swap(ref t0, ref t1);
}
for (int k = 0; k < 3 && haveContact == false; k++)
{
float u, v, w;
GeometryHelper.GetClosestPoint(t0, t1[k], out u, out v, out w);
if (Vector3F.AreNumericallyEqual(t0.Vertex0 * u + t0.Vertex1 * v + t0.Vertex2 * w, t1[k]))
{
haveContact = true;
}
}
}
// Test line segments.
for (int j = 0; j < 3 && haveContact == false; j++)
{
for (int k = 0; k < 3 && haveContact == false; k++)
{
var lsA = new LineSegment(tA[j], tA[(j + 1) % 3]);
var lsB = new LineSegment(tB[k], tB[(k + 1) % 3]);
Vector3F pA, pB;
haveContact = GeometryHelper.GetClosestPoints(lsA, lsB, out pA, out pB);
}
}
var csMpr = mpr.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
// Test false positives.
if (csMpr.HaveContact && !csGjk.HaveContact)
{
Assert.Fail("False positive: MPR reports contact, GJK reports no contact");
}
if (csMpr.HaveContact && !haveContact)
{
Assert.Fail("False positive: MPR reports contact, manual test reports no contact");
}
// Test again using boolean query.
bool haveMprContact = mpr.HaveContact(coA, coB);
csGjk = gjk.GetClosestPoints(coA, coB);
if (haveMprContact && !csGjk.HaveContact)
{
Assert.Fail("False positive: MPR reports contact, GJK reports no contact");
}
if (haveMprContact && !haveContact)
{
Assert.Fail("False positive: MPR reports contact, manual test reports no contact");
}
//if (csMpr.HaveContact != haveContact)
// Debugger.Break();
//Assert.AreEqual(csMpr.HaveContact, haveContact);
//if (csGjk.HaveContact != csMpr.HaveContact)
// Debugger.Break();
//Assert.AreEqual(csGjk.HaveContact, csMpr.HaveContact);
}
}
public void GjkBatteryTest()
{
Gjk gjk = new Gjk();
const int iterations = 10000;
//Point to point test
for (int i = 0; i < iterations; ++i)
{
gjk.Reset();
Vector3 point1 = Utilities.GenerateVector3();
Vector3 point2 = Utilities.GenerateVector3();
float expected = Vector3.Distance(point1, point2);
float actual = gjk.GetMinimumDistance(v =>
{
return(point1 - point2);
});
Utilities.AreEqual(expected, actual);
}
//Sphere to sphere test
for (int i = 0; i < iterations; ++i)
{
gjk.Reset();
BoundingSphere shape1 = new BoundingSphere(Utilities.GenerateVector3(), Utilities.GenerateFloat());
BoundingSphere shape2 = new BoundingSphere(Utilities.GenerateVector3(), Utilities.GenerateFloat());
float expected = Collision.DistanceSphereSphere(ref shape1, ref shape2);
if (Collision.SphereIntersectsSphere(ref shape1, ref shape2))
{
i--;
continue;
}
float actual = gjk.GetMinimumDistance(v =>
{
Vector3 a = shape1.SupportMapping(v);
Vector3 b = shape2.SupportMapping(-v);
return(a - b);
});
Utilities.AreEqual(expected, actual);
}
//Box to box test
for (int i = 0; i < iterations; ++i)
{
gjk.Reset();
BoundingBox shape1 = new BoundingBox(Utilities.GenerateVector3(), Utilities.GenerateVector3());
BoundingBox shape2 = new BoundingBox(Utilities.GenerateVector3(), Utilities.GenerateVector3());
float expected = Collision.DistanceBoxBox(ref shape1, ref shape2);
if (Collision.BoxIntersectsBox(ref shape1, ref shape2))
{
i--;
continue;
}
float actual = gjk.GetMinimumDistance(v =>
{
Vector3 a = shape1.SupportMapping(v);
Vector3 b = shape2.SupportMapping(-v);
return(a - b);
});
Utilities.AreEqual(expected, actual);
}
}
public Epa(Gjk gjk)
{
this._gjk = gjk;
}
public void TestTriangles()
{
var mpr = new MinkowskiPortalRefinement(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < 100000; i++)
{
// Create two triangles in a plane.
var tA = new Triangle();
tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
var tB = new Triangle();
tB.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tB.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
tB.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1), RandomHelper.Random.NextFloat(-1, 1));
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var csMpr = mpr.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
// Test again using boolean query.
bool haveMprContact = mpr.HaveContact(coA, coB);
// The exceptions are tested cases - all extreme shallow surface contacts.
if (i != 28487 && i != 47846 && i != 97305)
{
Assert.AreEqual(csGjk.HaveContact, haveMprContact);
Assert.AreEqual(csGjk.HaveContact, csMpr.HaveContact);
}
}
}
public void TestCoplanarTriangles2()
{
var mpr = new MinkowskiPortalRefinement(new CollisionDetection());
var gjk = new Gjk(new CollisionDetection());
RandomHelper.Random = new Random(1234567);
for (int i = 0; i < 100000; i++)
{
// Create two triangles in a plane.
var tA = new Triangle();
tA.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tA.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var tB = new Triangle();
tB.Vertex0 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex1 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
tB.Vertex2 = new Vector3F(RandomHelper.Random.NextFloat(-100, 100), -290, RandomHelper.Random.NextFloat(-100, 100));
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
// Test if 2D triangles have contact
bool haveContact = false;
// Test if vertices are inside the other triangle.
for (int j = 0; j < 2 && haveContact == false; j++)
{
var t0 = tA;
var t1 = tB;
if (j == 1)
MathHelper.Swap(ref t0, ref t1);
for (int k = 0; k < 3 && haveContact == false; k++)
{
float u, v, w;
GeometryHelper.GetClosestPoint(t0, t1[k], out u, out v, out w);
if (Vector3F.AreNumericallyEqual(t0.Vertex0 * u + t0.Vertex1 * v + t0.Vertex2 * w, t1[k]))
haveContact = true;
}
}
// Test line segments.
for (int j = 0; j < 3 && haveContact == false; j++)
{
for (int k = 0; k < 3 && haveContact == false; k++)
{
var lsA = new LineSegment(tA[j], tA[(j + 1) % 3]);
var lsB = new LineSegment(tB[k], tB[(k + 1) % 3]);
Vector3F pA, pB;
haveContact = GeometryHelper.GetClosestPoints(lsA, lsB, out pA, out pB);
}
}
var csMpr = mpr.GetContacts(coA, coB);
var csGjk = gjk.GetClosestPoints(coA, coB);
// Test false positives.
if (csMpr.HaveContact && !csGjk.HaveContact)
Assert.Fail("False positive: MPR reports contact, GJK reports no contact");
if (csMpr.HaveContact && !haveContact)
Assert.Fail("False positive: MPR reports contact, manual test reports no contact");
// Test again using boolean query.
bool haveMprContact = mpr.HaveContact(coA, coB);
csGjk = gjk.GetClosestPoints(coA, coB);
if (haveMprContact && !csGjk.HaveContact)
Assert.Fail("False positive: MPR reports contact, GJK reports no contact");
if (haveMprContact && !haveContact)
Assert.Fail("False positive: MPR reports contact, manual test reports no contact");
//if (csMpr.HaveContact != haveContact)
// Debugger.Break();
//Assert.AreEqual(csMpr.HaveContact, haveContact);
//if (csGjk.HaveContact != csMpr.HaveContact)
// Debugger.Break();
//Assert.AreEqual(csGjk.HaveContact, csMpr.HaveContact);
}
}