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