TestProgram

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;


namespace quaternionTest
{
    class Program
    {
        static void Main(string[] args)
        {
            Quaternion q1 = new Quaternion(1, 0, 0, 0);
            double[] q1RPY;
            Quaternion q2 = new Quaternion(1, 0, 0, 0);
            double[] q2RPY;
            Quaternion result;
            double[] resultRPY;
            //Quaternion result = Quaternion.Slerp(q1, q2, 0.2);

            //Console.WriteLine("W: " + result.W + " X: " + result.X + " Y: " + result.Y + " Z: " + result.Z);
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q2.setRPY(0, Math.PI / 2.0, 0); //q2 pitch 轉90度
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q2RPY = q2.getRPY();
            Console.WriteLine("Roll: " + q2RPY[0] + " Pitch: " + q2RPY[1] + " Yaw: " + q2RPY[2]);
            q1.setRPY(0, -Math.PI / 3.0, 0); //q1 pitch 轉-60度
            q1RPY = q1.getRPY();
            Console.WriteLine("Roll: " + q1RPY[0] + " Pitch: " + q1RPY[1] + " Yaw: " + q1RPY[2]);
            result = q2 / q1;
            resultRPY = result.getRPY();
            Console.WriteLine("Roll: " + resultRPY[0] + " Pitch: " + resultRPY[1] + " Yaw: " + resultRPY[2]);

#if false//true
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q1.setRPY(0, Math.PI / 2.0, 0); //Y轉90度
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q1.setRPY(0, -Math.PI / 2.0, 0); //Y轉-90度
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q1.setRPY(Math.PI / 2.0, 0, 0); //X轉90度
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
            q1.setRPY(-Math.PI / 2.0, 0, 0); //X轉-90度
            Console.WriteLine("W: " + q1.W + " X: " + q1.X + " Y: " + q1.Y + " Z: " + q1.Z);
#endif
            Console.ReadLine();
        }
    }
}