예제 #1
0
        public void TestAngleShortestPath(tf.Quaternion quaternion1, tf.Quaternion quaternion2, double expectedAngleShortestPath)
        {
            var angleShortestPath = quaternion1.AngleShortestPath(quaternion2);
            var tolerance         = 0.00001;

            Assert.InRange(Math.Abs(angleShortestPath - expectedAngleShortestPath), 0, tolerance);
        }
예제 #2
0
        public void Should_SetRotationMatrixFromQuaternion(tf.Quaternion quaternion, tf.Vector3 vector, tf.Vector3 expectedResult = null)
        {
            //Utils.WaitForDebugger();

            Console.WriteLine("");
            Console.WriteLine("Test: SetRotationMatrixFromQuaternion");
            Console.WriteLine("=====================================");
            //var matrix = new tf.Matrix3x3(quaternion);
            var matrix = new tf.Matrix3x3();

            matrix.SetRotation(quaternion);
            Console.WriteLine("Quaternion for rotation:");
            Console.WriteLine(quaternion.ToString());
            Console.WriteLine("Corresponding rotation matrix:");
            Console.WriteLine(matrix.ToString());
            Console.WriteLine("Vector to be rotated:");
            Console.WriteLine(vector.ToString());

            Console.WriteLine("Result of rotation via quaternion-vector-mult (qpq^-1):");
            var resultQuaternion = quaternion * vector;
            //Console.WriteLine(resultQuaternion.ToString());
            var resultVector1 = new tf.Vector3(resultQuaternion.X, resultQuaternion.Y, resultQuaternion.Z);

            Console.WriteLine(resultVector1.ToString());

            Console.WriteLine("Result of rotation via quaternion conversion into rotation matrix:");
            var resultVector2 = matrix * vector;

            Console.WriteLine(resultVector2.ToString());

            var tolerance = 1E-05;

            Assert.InRange(Math.Abs(resultVector1.X - resultVector2.X), 0.0, tolerance);
            Assert.InRange(Math.Abs(resultVector1.Y - resultVector2.Y), 0.0, tolerance);
            Assert.InRange(Math.Abs(resultVector1.Z - resultVector2.Z), 0.0, tolerance);
            if (expectedResult != null)
            {
                Assert.InRange(Math.Abs(resultVector1.X - expectedResult.X), 0.0, tolerance);
                Assert.InRange(Math.Abs(resultVector1.Y - expectedResult.Y), 0.0, tolerance);
                Assert.InRange(Math.Abs(resultVector1.Z - expectedResult.Z), 0.0, tolerance);
            }
        }
예제 #3
0
        public void Should_QuaternionToRPYToQuaternion(tf.Quaternion quaternion, tf.Vector3 expectedResult = null)
        {
            //Utils.WaitForDebugger();

            Console.WriteLine("");
            Console.WriteLine("Test: QuaternionToRPYToQuaternion");
            Console.WriteLine("=================================");
            var rpy = quaternion.RPY;
            var resultQuaternion = tf.Quaternion.FromRPY(rpy);

            Console.WriteLine("Input quaternion:");
            Console.WriteLine(quaternion.ToString());
            Console.WriteLine("Roll, Pitch, Yaw:");
            Console.WriteLine(rpy.ToString());
            Console.WriteLine("Output quaternion:");
            Console.WriteLine(resultQuaternion.ToString());

            // The output of conversion to rpy and back is a unit quaternion.
            // Hence, for comparison of input and output quaternion,
            // the input quaternion has to be normalized.
            var unitQuaternion = quaternion / quaternion.Abs;

            Console.WriteLine("Normalized input quaternion:");
            Console.WriteLine(unitQuaternion.ToString());
            var tolerance = 1E-05;

            Assert.InRange(Math.Abs(unitQuaternion.X - resultQuaternion.X), 0.0, tolerance);
            Assert.InRange(Math.Abs(unitQuaternion.Y - resultQuaternion.Y), 0.0, tolerance);
            Assert.InRange(Math.Abs(unitQuaternion.Z - resultQuaternion.Z), 0.0, tolerance);
            Assert.InRange(Math.Abs(unitQuaternion.W - resultQuaternion.W), 0.0, tolerance);
            if (expectedResult != null)
            {
                Assert.InRange(Math.Abs(rpy.X - expectedResult.X), 0.0, tolerance);
                Assert.InRange(Math.Abs(rpy.Y - expectedResult.Y), 0.0, tolerance);
                Assert.InRange(Math.Abs(rpy.Z - expectedResult.Z), 0.0, tolerance);
            }
        }