public void ComputeCollisionLineOther()
{
CollisionObject line0 = new CollisionObject();
//line0.Name = "line0";
((GeometricObject)line0.GeometricObject).Shape = new LineShape(new Vector3(0, 0, 1), new Vector3(1, 0, 0));
((GeometricObject)line0.GeometricObject).Pose = new Pose(new Vector3(0, 0, 2));
CollisionObject sphere = new CollisionObject();
//sphere.Name = "sphere";
((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)sphere.GeometricObject).Pose = Pose.Identity;
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
set = algo.GetClosestPoints(line0, sphere);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.IsTrue(Vector3.AreNumericallyEqual(-Vector3.UnitZ, set[0].Normal));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 2), set[0].Position, 0.001f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionALocal, 0.001f));
Assert.AreEqual(false, algo.HaveContact(line0, sphere));
set = set.Swapped;
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3(0, 0, 2.1f));
algo.UpdateContacts(set, 0);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3.AreNumericallyEqual(Vector3.UnitZ, set[0].Normal, 0.1f)); // Large epsilon because MPR for spheres is not very accurate.
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 3), set[0].Position, 0.1f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionALocal, 0.1f));
Assert.IsTrue(Vector3.AreNumericallyEqual(new Vector3(0, 0, 1), set[0].PositionBLocal, 0.1f));
Assert.AreEqual(true, algo.HaveContact(line0, sphere));
}
public void ComputeCollisionLineLine()
{
CollisionObject line0 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3F(1, 2, 3), new Vector3F(1, 0, 0)),
});
//line0.Name = "line0";
CollisionObject line1 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3F(0, 0, 1), new Vector3F(1, 0, 0)),
});
//line1.Name = "line1";
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 2), line1.GeometricObject.Pose.Orientation);
set = algo.GetClosestPoints(line0, line1);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(Vector3F.UnitY, set[0].Normal);
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(line1.GeometricObject.Pose.Position, QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line0, line1);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3F(1, 2, 3), set[0].Position);
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, set[0].Normal));
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3F(1, 2, 4), QuaternionF.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line1, line0);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3F(1, 2, 4), set[0].Position);
Assert.AreEqual(-Vector3F.UnitZ, set[0].Normal);
Assert.AreEqual(false, algo.HaveContact(line0, line1));
algo.UpdateContacts(set, 0.01f);
Assert.AreEqual(0, set.Count);
}
public void ComputeCollisionLineLine()
{
CollisionObject line0 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3(1, 2, 3), new Vector3(1, 0, 0)),
});
//line0.Name = "line0";
CollisionObject line1 = new CollisionObject(new GeometricObject
{
Shape = new LineShape(new Vector3(0, 0, 1), new Vector3(1, 0, 0)),
});
//line1.Name = "line1";
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3(1, 2, 2), line1.GeometricObject.Pose.Orientation);
set = algo.GetClosestPoints(line0, line1);
Assert.AreEqual(0, set[0].PenetrationDepth);
Assert.AreEqual(Vector3.UnitY, set[0].Normal);
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(line1.GeometricObject.Pose.Position, Quaternion.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line0, line1);
Assert.IsTrue(Numeric.AreEqual(0, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3(1, 2, 3), set[0].Position);
Assert.IsTrue(Vector3.AreNumericallyEqual(Vector3.UnitZ, set[0].Normal));
Assert.AreEqual(true, algo.HaveContact(line0, line1));
((GeometricObject)line1.GeometricObject).Pose = new Pose(new Vector3(1, 2, 4), Quaternion.CreateRotationZ(ConstantsF.PiOver2));
set = algo.GetClosestPoints(line1, line0);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.AreEqual(new Vector3(1, 2, 4), set[0].Position);
Assert.AreEqual(-Vector3.UnitZ, set[0].Normal);
Assert.AreEqual(false, algo.HaveContact(line0, line1));
algo.UpdateContacts(set, 0.01f);
Assert.AreEqual(0, set.Count);
}
public void ComputeCollisionLineOther()
{
CollisionObject line0 = new CollisionObject();
//line0.Name = "line0";
((GeometricObject)line0.GeometricObject).Shape = new LineShape(new Vector3F(0, 0, 1), new Vector3F(1, 0, 0));
((GeometricObject)line0.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 2));
CollisionObject sphere = new CollisionObject();
//sphere.Name = "sphere";
((GeometricObject)sphere.GeometricObject).Shape = new SphereShape(1);
((GeometricObject)sphere.GeometricObject).Pose = Pose.Identity;
LineAlgorithm algo = new LineAlgorithm(new CollisionDetection());
ContactSet set;
set = algo.GetClosestPoints(line0, sphere);
Assert.IsTrue(Numeric.AreEqual(-2, set[0].PenetrationDepth));
Assert.IsTrue(Vector3F.AreNumericallyEqual(-Vector3F.UnitZ, set[0].Normal));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 2), set[0].Position, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionALocal, 0.001f));
Assert.AreEqual(false, algo.HaveContact(line0, sphere));
set = set.Swapped;
((GeometricObject)sphere.GeometricObject).Pose = new Pose(new Vector3F(0, 0, 2.1f));
algo.UpdateContacts(set, 0);
Assert.IsTrue(Numeric.AreEqual(0.1f, set[0].PenetrationDepth, 0.001f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(Vector3F.UnitZ, set[0].Normal, 0.1f)); // Large epsilon because MPR for spheres is not very accurate.
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 3), set[0].Position, 0.1f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionALocal, 0.1f));
Assert.IsTrue(Vector3F.AreNumericallyEqual(new Vector3F(0, 0, 1), set[0].PositionBLocal, 0.1f));
Assert.AreEqual(true, algo.HaveContact(line0, sphere));
}