Plugin/Source/ThirdParty/include/SenseGlove/Core/Tracking.hpp

/**
 * @file
 *
 * @author  Max Lammers <max@senseglove.com>
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * Copyright (c) 2020 - 2023 SenseGlove
 *
 * @section DESCRIPTION
 *
 *
 */


#pragma once

#include <cstdint>
#include <string>
#include <vector>

#include "Fingers.hpp"
#include "Platform.hpp"

namespace SGCore
{
    namespace Kinematics
    {
        class Quat;
        class Vect3D;
    }// namespace Kinematics

    /// <summary> Tracking Hardware used for positional tracking. </summary>
    enum class SGCORE_API EPositionalTrackingHardware : uint8_t
    {
        /// <summary> Custom tracking hardware, we don't apply any offsets. </summary>
        Custom = 0,

        /// <summary> HTC Vive Tracker (v1.0 or v2.0). </summary>
        ViveTracker,

        /// <summary> Oculus Touch controller (left or right) for Rift S and Quest 1. </summary>
        OculusTouch,

        /// <summary> Touch controller for Oculus Quest 2. </summary>
        Quest2Controller,

        /// <summary> Pico Neo 2 Controller(s). </summary>
        PicoNeo2,

        /// <summary> Pico Neo 3 Controller(s). </summary>
        PicoNeo3,
    };

    class SGCORE_API Tracking;
}// namespace SGCore

class SGCORE_API SGCore::Tracking
{
public:
    //---------------------------------------------------------------------------
    // Generic Methods

    /// <summary> Knowing a position & rotation in 3D space, calculate a relative location based on offsets. </summary>
    /// <param name="trackedPosition"></param>
    /// <param name="trackedRotation"></param>
    /// <param name="positionOffset"></param>
    /// <param name="rotationOffset"></param>
    /// <param name="out_newPosition"></param>
    /// <param name="out_newRotation"></param>
    static void CalculateLocation(
            const Kinematics::Vect3D& trackedPosition, const Kinematics::Quat& trackedRotation,
            const Kinematics::Vect3D& positionOffset, const Kinematics::Quat& rotationOffset,
            Kinematics::Vect3D& out_newPosition, Kinematics::Quat& out_newRotation);

    //---------------------------------------------------------------------------
    // SenseGlove Related

    //TrackingMount -> SG Glove

    /// <summary> Positional offset from each finger to the glove origin. </summary>
    static const std::vector<Kinematics::Vect3D>& GetSenseGloveFingerToGlovePositionOrigin();

    /// <summary>  </summary>
    static const std::vector<Kinematics::Quat>& GetSenseGloveFingerToGloveRotationOrigin();

    //SG Glove -> Hand

    /// <summary> Default position offset from glove origin to wrist location, in mm. </summary>
    static const Kinematics::Vect3D& GetSenseGloveWristPositionOffset();

    /// <summary> Default rotation offset from glove origin to wrist location. </summary>
    static const Kinematics::Quat& GetSenseGloveWristRotationOffset();

    /// <summary> Get the position and rotation offsets to go from the glove origin to the mounted position. </summary>
    /// <remarks> Based on CAD. needs to be inverted when going from tracker to glove origin. </remarks>
    /// <param name="bRightHanded"></param>
    /// <param name="toFinger"></param>
    /// <param name="out_iPosition"></param>
    /// <param name="out_iRotation"></param>
    static void GetSenseGloveMountOffset(bool bRightHanded, EFinger toFinger,
                                         Kinematics::Vect3D& out_iPosition, Kinematics::Quat& out_iRotation);

    /// <summary> Get the offset to get from the TrackedHardware to a SenseGlove mounting location. Combined with Glove-Mount offset to get the glove location. </summary>
    /// <param name="hardware"></param>
    /// <param name="bRightHanded"></param>
    /// <param name="out_positionOffset"></param>
    /// <param name="out_rotationOffset"></param>
    static void GetSenseGloveTrackerMountOffset(EPositionalTrackingHardware hardware, bool bRightHanded,
                                                Kinematics::Vect3D& out_positionOffset,
                                                Kinematics::Quat& out_rotationOffset);

    // Vive Tracker

    /// <summary> Vive Tracker origin to Finger rotational offset. </summary>
    static const Kinematics::Vect3D& GetSenseGloveViveToAttachPosition();

    /// <summary> Vive Tracker origin to Finger rotational offset. </summary>
    static const Kinematics::Quat& GetSenseGloveViveToAttachRotation();

    //Quest 1 / Rift S

    /// <summary> Oculus Touch origin to Finger rotational offset </summary>
    static const Kinematics::Vect3D& GetSenseGloveOculusTouchToAttachPosition();

    /// <summary> Oculus Touch origin to Finger rotational offset </summary>
    static const Kinematics::Quat& GetSenseGloveOculusTouchToAttachRotation();

    //---------------------------------------------------------------------------
    // Nova Related

    /// <summary> Positional offset between Nova Glove Origin and Wrist. </summary>
    static const Kinematics::Vect3D& GetNovaToWristPositionOffset();

    /// <summary> Rotational offset between Nova Glove Origin and Wrist. </summary>
    static const Kinematics::Quat& GetNovaToWristRotationOffset();

    /// <summary> Returns the Offsets that will bring us from the Nova origin to the wrist position. </summary>
    /// <param name="bRightHanded"></param>
    /// <param name="out_positionOffset"></param>
    /// <param name="out_rotationOffset"></param>
    static void GetNovaGloveWristOffset(bool bRightHanded,
                                        Kinematics::Vect3D& out_positionOffset, Kinematics::Quat& out_rotationOffset);

    static const Kinematics::Vect3D& GetRightNovaViveToAttachPosition();
    static const Kinematics::Quat& GetRightNovaViveToAttachRotation();

    static const Kinematics::Vect3D& GetLeftNovaViveToAttachPosition();
    static const Kinematics::Quat& GetLeftNovaViveToAttachRotation();

    static const Kinematics::Vect3D& GetRightNovaQuest2ToAttachPosition();
    static const Kinematics::Quat& GetRightNovaQuest2ToAttachRotation();

    static const Kinematics::Vect3D& GetLeftNovaQuest2ToAttachPosition();
    static const Kinematics::Quat& GetLeftNovaQuest2ToAttachRotation();

    static const Kinematics::Vect3D& GetRightNovaPico2ToAttachPosition(); // left is a mirror, so we only need R pos.
    static const Kinematics::Quat& GetRightNovaPico2ToAttachRotation();
    static const Kinematics::Quat& GetLeftNovaPico2ToAttachRotation();

    /// <summary> Retrieves Offsets for a particular device. </summary>
    /// <param name="hardware"></param>
    /// <param name="bRightHanded"></param>
    /// <param name="out_positionOffset"></param>
    /// <param name="out_rotationOffset"></param>
    static void GetNovaGloveTrackerOffset(EPositionalTrackingHardware hardware, bool bRightHanded,
                                          Kinematics::Vect3D& out_positionOffset,
                                          Kinematics::Quat& out_rotationOffset);

    static std::string ToString(EPositionalTrackingHardware hardware);

public:
    Tracking() = delete;
    virtual ~Tracking() = delete;
};