public void Containment()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(2),
Pose = new Pose(new Vector3F(1, 2, 3)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(new Vector3F(1, 2, 3)),
});
ContactSet set;
set = algo.GetContacts(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(3, set[0].PenetrationDepth);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 2, 3));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(2, set[0].PenetrationDepth);
}
public void Containment()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(2),
Pose = new Pose(new Vector3F(1, 2, 3)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(new Vector3F(1, 2, 3)),
});
ContactSet set;
set = algo.GetContacts(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(3, set[0].PenetrationDepth);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 2, 3));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(2, set[0].PenetrationDepth);
}
public void TestNormals()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject box = new CollisionObject(new GeometricObject
{
Pose = new Pose(new Vector3F(1.99999f, 0, 0)),
Shape = new BoxShape(2, 2, 2),
});
CollisionObject sphere = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(box, sphere);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(0.2f, 0, 0));
algo.UpdateContacts(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.2f, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
}
public void ComputeCollision()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1))
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(1, set[0].PenetrationDepth));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(0, set.Count);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0.9f, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.02f));
}
public void ComputeCollision()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new TriangleShape(new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(0, 0, 1))
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(a, b);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.IsTrue(Numeric.AreEqual(1, set[0].PenetrationDepth));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(2, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(0, set.Count);
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0.9f, 0.1f, 0.2f));
algo.UpdateContacts(set, 0);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0.1f, 0.2f), set[0].PositionAWorld, 0.02f));
}
public void TestCoplanarTriangles()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
var tA = new Triangle();
tA.Vertex0 = new Vector3F(23.99746f, -2.72f, 1.486926f);
tA.Vertex1 = new Vector3F(24.00217f, -2.72f, 1.459832f);
tA.Vertex2 = new Vector3F(23.9906f, -2.72f, 1.484784f);
var tB = new Triangle();
tB.Vertex0 = new Vector3F(23.03683f, -2.72f, 1.473877f);
tB.Vertex1 = new Vector3F(24.0843f, -2.72f, 1.339786f);
tB.Vertex2 = new Vector3F(23.73688f, -2.72f, 1.398957f);
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var cs = algo.GetContacts(coA, coB);
Assert.IsFalse(cs.HaveContact);
}
public void TestCoplanarTriangles()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
var tA = new Triangle();
tA.Vertex0 = new Vector3F(23.99746f, -2.72f, 1.486926f);
tA.Vertex1 = new Vector3F(24.00217f, -2.72f, 1.459832f);
tA.Vertex2 = new Vector3F(23.9906f, -2.72f, 1.484784f);
var tB = new Triangle();
tB.Vertex0 = new Vector3F(23.03683f, -2.72f, 1.473877f);
tB.Vertex1 = new Vector3F(24.0843f, -2.72f, 1.339786f);
tB.Vertex2 = new Vector3F(23.73688f, -2.72f, 1.398957f);
var coA = new CollisionObject(new GeometricObject(new TriangleShape(tA)));
var coB = new CollisionObject(new GeometricObject(new TriangleShape(tB)));
var cs = algo.GetContacts(coA, coB);
Assert.IsFalse(cs.HaveContact);
}
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);
}
}
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 TestNormals()
{
MinkowskiPortalRefinement algo = new MinkowskiPortalRefinement(new CollisionDetection());
CollisionObject box = new CollisionObject(new GeometricObject
{
Pose = new Pose(new Vector3F(1.99999f, 0, 0)),
Shape = new BoxShape(2, 2, 2),
});
CollisionObject sphere = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1)
});
ContactSet set;
set = algo.GetContacts(box, sphere);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(0.2f, 0, 0));
algo.UpdateContacts(set, 0);
Assert.AreEqual(1, set.Count);
Assert.IsTrue(Numeric.AreEqual(0.2f, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(-1, 0, 0), set[0].Normal, 0.001f));
}
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);
}
}
