Пример #1
0
        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));
        }
Пример #2
0
        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);
        }
Пример #3
0
        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);
        }
Пример #4
0
        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));
        }