public void TestContainment()
{
BoxSphereAlgorithm algo = new BoxSphereAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject();
CollisionObject b = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new BoxShape(1, 2, 3);
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
}
public void TestContainment()
{
BoxSphereAlgorithm algo = new BoxSphereAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject();
CollisionObject b = new CollisionObject();
((GeometricObject)a.GeometricObject).Shape = new BoxShape(1, 2, 3);
((GeometricObject)b.GeometricObject).Shape = new SphereShape(1);
Assert.AreEqual(true, algo.HaveContact(a, b));
Assert.AreEqual(true, algo.HaveContact(b, a));
}
public void TestSeparated()
{
BoxSphereAlgorithm algo = new BoxSphereAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new BoxShape(1, 2, 3),
Pose = new Pose(new Vector3(0, 0, 0)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(new Vector3(1.6f, 0, 0)),
});
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(1, 0, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(0.55f, 0, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(-1.6f, 0, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(-1, 0, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(-0.55f, 0, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(0, 2.1f, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(0, 1, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(0, 1.05f, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(0, -2.1f, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(0, -1, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(0, -1.05f, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(0, 0, 2.6f));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(0, 0, 1), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(0, 0, 1.55f), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3(0, 0, -2.6f));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3(0, 0, -1), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3(0, 0, -1.55f), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
Assert.AreEqual(0, algo.GetContacts(a, b).Count);
}
public void TestSeparated()
{
BoxSphereAlgorithm algo = new BoxSphereAlgorithm(new CollisionDetection());
CollisionObject a = new CollisionObject(new GeometricObject
{
Shape = new BoxShape(1, 2, 3),
Pose = new Pose(new Vector3F(0, 0, 0)),
});
CollisionObject b = new CollisionObject(new GeometricObject
{
Shape = new SphereShape(1),
Pose = new Pose(new Vector3F(1.6f, 0, 0)),
});
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1,algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(1, 0, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0.55f, 0, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(-1.6f, 0, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(-1, 0, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(-0.55f, 0, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 2.1f, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(0, 1, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0, 1.05f, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, -2.1f, 0));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(0, -1, 0), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0, -1.05f, 0), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 2.6f));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(0, 0, 1), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0, 0, 1.55f), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
((GeometricObject)a.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 0));
((GeometricObject)b.GeometricObject).Pose = new Pose(new Vector3F(0, 0, -2.6f));
Assert.AreEqual(false, algo.HaveContact(a, b));
Assert.AreEqual(false, algo.HaveContact(b, a));
Assert.AreEqual(1, algo.GetClosestPoints(a, b).Count);
Assert.AreEqual(new Vector3F(0, 0, -1), algo.GetClosestPoints(a, b)[0].Normal);
Assert.AreEqual(new Vector3F(0, 0, -1.55f), algo.GetClosestPoints(a, b)[0].Position);
Assert.IsTrue(Numeric.AreEqual(-0.1f, algo.GetClosestPoints(a, b)[0].PenetrationDepth));
Assert.AreEqual(0, algo.GetContacts(a, b).Count);
}