XRFunctions.h

#pragma once
#include "openxr/openxr.h"
#include "Base/Headers/Vertex.hpp"
#include "Base/Headers/Angle.h"
namespace NDEVR
{
    class XRFunctions
    {
    public:
        static Vertex<3, fltp04> ConvertVertex(const XrVector3f& position)
        {
            return Vertex<3, fltp04>(-position.z, -position.x, position.y);
        }

        static Vertex<3, fltp08> ConvertVertex(fltp08 x, fltp08 y, fltp08 z)
        {
            return Vertex<3, fltp08>(-z, -x, y);
        }

        static Vector<3, Angle<fltp08>> ConvertFromQuaternion(const XrQuaternionf& quaternion)
        {
            return ConvertFromQuaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
        }

        static Vector<3, Angle<fltp08>> ConvertFromQuaternion(fltp08 qx, fltp08 qy, fltp08 qz, fltp08 qw)
        {
            Vector<3, Angle<fltp08>> angles(Angle<fltp08>(0));
            fltp08 val = qx * qy + qz * qw;
            if (val >= .49999)
            {
                angles[YAW] = 2.0 * Angle<fltp08>::atan2(qx, qw);
                angles[PITCH] = Angle<fltp08>(DEGREES, 90.0);
            }
            else if (val <= -.49999)
            {
                angles[YAW] = -2.0 * Angle<fltp08>::atan2(qx, qw);
                angles[PITCH] = Angle<fltp08>(DEGREES, -90.0);
            }
            else
            {
                angles[YAW] = Angle<fltp08>::atan2(2 * qy * qw - 2 * qx * qz, 1 - 2 * pow(qy, 2.0) - 2 * pow(qz, 2.0));
                angles[ROLL] = Angle<fltp08>::asin(2 * qx * qy + 2.0 * qz * qw);
                angles[PITCH] = Angle<fltp08>::atan2(2 * qx * qw - 2 * qy * qz, 1 - 2 * pow(qx, 2.0) - 2 * pow(qz, 2));
            }
            return angles;
        }
    };
}