Quaternion.cs

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

namespace quaternionTest
{
    class Quaternion
    {
        public double X, Y, Z, W;
        public double qs = 1.0;
        public double [] qv = new double[3] { 0.0, 0.0, 0.0 };
        double roll, pitch, yaw;
        //Quaternion Initialization
        public Quaternion(double w, double x, double y, double z)
        {
            W = w;
            X = x;
            Y = y;
            Z = z;
        }

        //Use Axis and Angle to get a Quaternion
        public Quaternion(double [] Axis, double Angle)
        {

        }

        //Normalise
        public void Normalise()
        {
            double m = W * W + X * X + Y * Y + Z * Z;
            if (m > 0.001)
            {
                m = Math.Sqrt(m);
                W /= m;
                X /= m;
                Y /= m;
                Z /= m;
            }
            else
            {
                W = 1; X = 0; Y = 0; Z = 0;
            }
        }

        //Quaternion Conjugate
        public static Quaternion Conjugate(Quaternion q)
        {
            double nw = q.W;
            double nx = -(q.X);
            double ny = -(q.Y);
            double nz = -(q.Z);
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Addition
        public static Quaternion operator +(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W + q2.W;
            double nx = q1.X + q2.X;
            double ny = q1.Y + q2.Y;
            double nz = q1.Z + q2.Z;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Substration
        public static Quaternion operator -(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W - q2.W;
            double nx = q1.X - q2.X;
            double ny = q1.Y - q2.Y;
            double nz = q1.Z - q2.Z;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Multiplication
        // Multiplying q1 with q2 is meaning of doing q2 firstly then q1
        public static Quaternion operator *(Quaternion q1, Quaternion q2)
        {
            double nw = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
            double nx = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
            double ny = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X;
            double nz = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Quaternion Division (Multiply by the inverse)
        public static Quaternion operator /(Quaternion q1, Quaternion q2)
        {
            Quaternion q2Conj = Conjugate(q2);
            double norm = Math.Sqrt(q2.W * q2.W + q2.X * q2.X + q2.Y * q2.Y + q2.Z * q2.Z);
            Quaternion newQ = q1 * q2Conj;
            double nw = newQ.W / norm;
            double nx = newQ.X / norm;
            double ny = newQ.Y / norm;
            double nz = newQ.Z / norm;
            return new Quaternion(nw, nx, ny, nz);
        }

        //Update the quaternion using Euler angles(roll, pitch, yaw)
        /*
         * heading     theta    yaw
           attitude    phi      pitch
           bank        psi      roll
         */
        public void setRPY(double roll, double pitch, double yaw)
        {
            double cY = Math.Cos(yaw / 2.0);
            double sY = Math.Sin(yaw / 2.0);
            double cR = Math.Cos(roll / 2.0);
            double sR = Math.Sin(roll / 2.0);
            double cP = Math.Cos(pitch / 2.0);
            double sP = Math.Sin(pitch / 2.0);
            W = cR * cP * cY + sR * sP * sY;
            X = sR * cP * cY - cR * sP * sY;
            Y = cR * sP * cY + sR * cP * sY;
            Z = cR * cP * sY - sR * sP * cY;
        }

        //Get the RPY from Quaternion
        public double [] getRPY()
        {
            double sqw = W * W;
            double sqx = X * X;
            double sqy = Y * Y;
            double sqz = Z * Z;
            double unit = X + Y + Z + W; //if normalised is one, otherwise is correction factor
            double test = X * Y + Z * W;
            double [] result = new double[3];
#if true
            if (test > 0.499 * unit)
            {
                pitch = 2 * Math.Atan2(X, W);
                yaw = Math.PI / 2.0;
                roll = 0;
                result[0] = (roll * 180.0) / Math.PI;
                result[1] = (pitch * 180.0) / Math.PI;
                result[2] = (yaw * 180.0) / Math.PI;
                return result;
            }
            if (test < -0.499 * unit)
            {
                pitch = -2 * Math.Atan2(X, W);
                yaw = -Math.PI / 2.0;
                roll = 0;
                result[0] = (roll * 180.0) / Math.PI;
                result[1] = (pitch * 180.0) / Math.PI;
                result[2] = (yaw * 180.0) / Math.PI;
                return result;
            }
#endif
            yaw = Math.Atan2(2.0 * (W * Z + X * Y), sqw + sqx - sqy - sqz);
            pitch = Math.Asin(2.0 * (W * Y - X * Z));
            roll = Math.Atan2(2.0 * (W * X + Y * Z), sqw - sqx - sqy + sqz);
            result[0] = (roll * 180.0) / Math.PI;
            result[1] = (pitch * 180.0) / Math.PI;
            result[2] = (yaw * 180.0) / Math.PI;

            return result;
        }

        //Do the slerp
        public static Quaternion Slerp(Quaternion q1, Quaternion q2, double t)
        {
            //w, x, y, z
            double nw, nx, ny, nz;
            //Calculate angle between them
            double cosHalfTheta = q1.W * q2.W + q1.X * q2.X + q1.Y + q2.Y + q1.Z + q1.Z;


            //if q1 = q2 or q1 = -q2 then theta = 0, return q1
            if (Math.Abs(cosHalfTheta) >= 1.0)
            {
                nw = q2.W;
                nx = q2.X;
                ny = q2.Y;
                nz = q2.Z;
                return new Quaternion(nw, nx, ny, nz);
            }

            //Calculate temporary values.
            double halfTheta = Math.Acos(cosHalfTheta);
            double sinHalfTheta = Math.Sqrt(1.0 - cosHalfTheta * cosHalfTheta);

            //if theta = 180 degrees then result is not fully defined
            if(Math.Abs(sinHalfTheta) < 0.001){
                nw = (q1.W * 0.5 + q2.W * 0.5);
                nx = (q1.X * 0.5 + q2.X * 0.5);
                ny = (q1.Y * 0.5 + q2.Y * 0.5);
                nz = (q1.Z * 0.5 + q2.Z * 0.5);
                return new Quaternion(nw, nx, ny, nz);
            }

            double ratioA = Math.Sin((1 - t) * halfTheta) / sinHalfTheta;
            double ratioB = Math.Sin(t * halfTheta) / sinHalfTheta;
            //Calculate Quaternion
            nw = (q1.W * ratioA + q2.W * ratioB);
            nx = (q1.X * ratioA + q2.X * ratioB);
            ny = (q1.Y * ratioA + q2.Y * ratioB);
            nz = (q1.Z * ratioA + q2.Z * ratioB);
            return new Quaternion(nw, nx, ny, nz);
        }

    }
}