Ejemplo n.º 1
0
        public void SerializationJson()
        {
            PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
            PoseD pose2;

            string fileName = "SerializationPoseD.json";

            if (File.Exists(fileName))
            {
                File.Delete(fileName);
            }

            var serializer = new DataContractJsonSerializer(typeof(PoseD));

            using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
                serializer.WriteObject(stream, pose1);

            using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
                pose2 = (PoseD)serializer.ReadObject(stream);

            Assert.AreEqual(pose1, pose2);
        }
Ejemplo n.º 2
0
        public void SerializationXml()
        {
            PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
            PoseD pose2;

            string fileName = "SerializationPoseD.xml";

            if (File.Exists(fileName))
            {
                File.Delete(fileName);
            }

            var serializer = new DataContractSerializer(typeof(PoseD));

            using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
                using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8))
                    serializer.WriteObject(writer, pose1);

            using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
                using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas()))
                    pose2 = (PoseD)serializer.ReadObject(reader);

            Assert.AreEqual(pose1, pose2);
        }
Ejemplo n.º 3
0
        public void AreNumericallyEqual()
        {
            var a = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(1, 2, 3, 4, 5, 6, 7, 8, 9));
            var b = a;

            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));

            b = AddToAllComponents(a, Numeric.EpsilonD / 10);
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));

            b = AddToAllComponents(a, Numeric.EpsilonD * 10);
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD * 100));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD * 100));

            b             = a;
            b.Position.X -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b             = a;
            b.Position.Y -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b             = a;
            b.Position.Z -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M00 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M01 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M02 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M10 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M11 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M12 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M20 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M21 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

            b = a;
            b.Orientation.M22 -= 0.0001;
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
            Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
            Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
        }
Ejemplo n.º 4
0
        public void Interpolate()
        {
            PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
            PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1));

            Assert.IsTrue(Vector3D.AreNumericallyEqual(p1.Position, PoseD.Interpolate(p1, p2, 0).Position));
            Assert.IsTrue(Matrix33D.AreNumericallyEqual(p1.Orientation, PoseD.Interpolate(p1, p2, 0).Orientation));

            Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, PoseD.Interpolate(p1, p2, 1).Position));
            Assert.IsTrue(Matrix33D.AreNumericallyEqual(p2.Orientation, PoseD.Interpolate(p1, p2, 1).Orientation));

            Assert.IsTrue(Vector3D.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3), PoseD.Interpolate(p1, p2, 0.3).Position));
            Assert.IsTrue(
                QuaternionD.AreNumericallyEqual(
                    InterpolationHelper.Lerp(QuaternionD.CreateRotation(p1.Orientation), QuaternionD.CreateRotation(p2.Orientation), 0.3),
                    QuaternionD.CreateRotation(PoseD.Interpolate(p1, p2, 0.3).Orientation)));
        }
Ejemplo n.º 5
0
        private void OnComponentChanged()
        {
            if (_isUpdating)
                return;

            _isUpdating = true;
            try
            {
                if (_valueType == typeof(Pose))
                {
                    Value = new Pose(new Vector3F((float)X, (float)Y, (float)Z),
                                     new Matrix33F((float)M00, (float)M01, (float)M02,
                                                   (float)M10, (float)M11, (float)M12,
                                                   (float)M20, (float)M21, (float)M22));
                }
                else if (_valueType == typeof(PoseD))
                {
                    Value = new PoseD(new Vector3D(X, Y, Z),
                                      new Matrix33D(M00, M01, M02,
                                                    M10, M11, M12,
                                                    M20, M21, M22));

                }
            }
            finally
            {
                _isUpdating = false;
            }
        }
Ejemplo n.º 6
0
        private PoseD AddToAllComponents(PoseD pose, double epsilon)
        {
            var result = pose;
              result.Position.X += epsilon;
              result.Position.Y += epsilon;
              result.Position.Z += epsilon;

              result.Orientation.M00 += epsilon;
              result.Orientation.M01 += epsilon;
              result.Orientation.M02 += epsilon;
              result.Orientation.M10 += epsilon;
              result.Orientation.M11 += epsilon;
              result.Orientation.M12 += epsilon;
              result.Orientation.M20 += epsilon;
              result.Orientation.M21 += epsilon;
              result.Orientation.M22 += epsilon;

              return result;
        }
Ejemplo n.º 7
0
        public void ToPose()
        {
            PoseD poseD = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
              Pose pose = new Pose(new Vector3F(1, 2, 3), new Matrix33F(4, 5, 6, 7, 8, 9, 10, 11, 12));

              Assert.AreEqual(pose, (Pose)poseD);
              Assert.AreEqual(pose, poseD.ToPose());
        }
Ejemplo n.º 8
0
        public void SerializationXml()
        {
            PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
              PoseD pose2;

              string fileName = "SerializationPoseD.xml";

              if (File.Exists(fileName))
            File.Delete(fileName);

              var serializer = new DataContractSerializer(typeof(PoseD));
              using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
              using (var writer = XmlDictionaryWriter.CreateTextWriter(stream, Encoding.UTF8))
            serializer.WriteObject(writer, pose1);

              using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
              using (var reader = XmlDictionaryReader.CreateTextReader(stream, new XmlDictionaryReaderQuotas()))
            pose2 = (PoseD)serializer.ReadObject(reader);

              Assert.AreEqual(pose1, pose2);
        }
Ejemplo n.º 9
0
        public void SerializationJson()
        {
            PoseD pose1 = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(4, 5, 6, 7, 8, 9, 10, 11, 12));
              PoseD pose2;

              string fileName = "SerializationPoseD.json";

              if (File.Exists(fileName))
            File.Delete(fileName);

              var serializer = new DataContractJsonSerializer(typeof(PoseD));
              using (var stream = new FileStream(fileName, FileMode.CreateNew, FileAccess.Write))
            serializer.WriteObject(stream, pose1);

              using (var stream = new FileStream(fileName, FileMode.Open, FileAccess.Read))
            pose2 = (PoseD)serializer.ReadObject(stream);

              Assert.AreEqual(pose1, pose2);
        }
Ejemplo n.º 10
0
        public void MultiplyOperator()
        {
            PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1));

              Assert.IsTrue(Vector4D.AreNumericallyEqual(
                    p1.ToMatrix44D() * p2.ToMatrix44D() * new Vector4D(1, 2, 3, 1),
                    p1 * p2 * new Vector4D(1, 2, 3, 1)));
        }
Ejemplo n.º 11
0
        public void AreNumericallyEqual()
        {
            var a = new PoseD(new Vector3D(1, 2, 3), new Matrix33D(1, 2, 3, 4, 5, 6, 7, 8, 9));
              var b = a;

              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));

              b = AddToAllComponents(a, Numeric.EpsilonD / 10);
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));

              b = AddToAllComponents(a, Numeric.EpsilonD * 10);
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD * 100));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD * 100));

              b = a;
              b.Position.X -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Position.Y -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Position.Z -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M00 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M01 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M02 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M10 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M11 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M12 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M20 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M21 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));

              b = a;
              b.Orientation.M22 -= 0.0001;
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(a, b, Numeric.EpsilonD));
              Assert.AreEqual(false, PoseD.AreNumericallyEqual(b, a, Numeric.EpsilonD));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(a, b, 0.001));
              Assert.AreEqual(true, PoseD.AreNumericallyEqual(b, a, 0.001));
        }
Ejemplo n.º 12
0
        public void Interpolate()
        {
            PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              PoseD p2 = new PoseD(new Vector3D(-4, 5, -6), QuaternionD.CreateRotationZ(-0.1));

              Assert.IsTrue(Vector3D.AreNumericallyEqual(p1.Position, PoseD.Interpolate(p1, p2, 0).Position));
              Assert.IsTrue(Matrix33D.AreNumericallyEqual(p1.Orientation, PoseD.Interpolate(p1, p2, 0).Orientation));

              Assert.IsTrue(Vector3D.AreNumericallyEqual(p2.Position, PoseD.Interpolate(p1, p2, 1).Position));
              Assert.IsTrue(Matrix33D.AreNumericallyEqual(p2.Orientation, PoseD.Interpolate(p1, p2, 1).Orientation));

              Assert.IsTrue(Vector3D.AreNumericallyEqual(InterpolationHelper.Lerp(p1.Position, p2.Position, 0.3), PoseD.Interpolate(p1, p2, 0.3).Position));
              Assert.IsTrue(
            QuaternionD.AreNumericallyEqual(
              InterpolationHelper.Lerp(QuaternionD.CreateRotation(p1.Orientation), QuaternionD.CreateRotation(p2.Orientation), 0.3),
              QuaternionD.CreateRotation(PoseD.Interpolate(p1, p2, 0.3).Orientation)));
        }
Ejemplo n.º 13
0
        public void GetHashCodeTest()
        {
            PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));

              Assert.AreEqual(p1.GetHashCode(), p2.GetHashCode());

              p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              p2 = new PoseD(new Vector3D(2, 1, 3), QuaternionD.CreateRotationY(0.3));
              Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode());

              // Too bad two rotation matrices that differ only by the sign of the angle
              // (+/- angle with same axis) have the same hashcodes. See KB -> .NET --> GetHashCode
              //p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              //p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(-0.3));
              //Assert.AreNotEqual(p1.GetHashCode(), p2.GetHashCode());
        }
Ejemplo n.º 14
0
        public void Equals()
        {
            PoseD p1 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));
              PoseD p2 = new PoseD(new Vector3D(1, 2, 3), QuaternionD.CreateRotationY(0.3));

              Assert.AreEqual(p1, p2);
              Assert.IsTrue(p1.Equals((object)p2));
              Assert.IsTrue(p1.Equals(p2));
              Assert.IsFalse(p1.Equals(p2.ToMatrix44D()));
        }