Plugin/Source/SenseGloveCoreImpl/Private/SGCoreImpl/SGTrackingImpl.cpp

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2024 SenseGlove
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * @section DESCRIPTION
 *
 *
 */


#include "SGCoreImpl/SGTrackingImpl.h"

#include "Containers/StringConv.h"

#include "SGBuildHacks/SGInclude_Core_Quat.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"

void FSGTrackingImpl::CalculateLocation(const FVector& TrackedPosition, const FQuat& TrackedRotation,
                                        const FVector& PositionOffset, const FQuat& RotationOffset,
                                        FVector& OutNewPosition, FQuat& OutNewRotation)
{
    FSGVect3DImpl::FVect3DType TrackedPositionVect3D{
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(TrackedPosition)
    };
    FSGQuatImpl::FQuatType TrackedRotationQuat(FSGQuatImpl(TrackedRotation).ToQuat());
    FSGVect3DImpl::FVect3DType PositionOffsetVect3D{
        FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(PositionOffset)
    };
    FSGQuatImpl::FQuatType RotationOffsetQuat(FSGQuatImpl(RotationOffset).ToQuat());
    FSGVect3DImpl::FVect3DType NewPosition;
    FSGQuatImpl::FQuatType NewRotation;

    FTrackingType::CalculateLocation(MoveTemp(TrackedPositionVect3D),
                                     MoveTemp(TrackedRotationQuat),
                                     MoveTemp(PositionOffsetVect3D),
                                     MoveTemp(RotationOffsetQuat),
                                     NewPosition,
                                     NewRotation);

    OutNewPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(NewPosition));
    OutNewRotation = FSGQuatImpl(MoveTemp(NewRotation)).ToFQuat();
}

const TArray<FVector>& FSGTrackingImpl::GetSenseGloveFingerToGlovePositionOrigin()
{
    static const TArray<FVector> GlovePositionOrigin{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetSenseGloveFingerToGlovePositionOrigin())
    };
    return GlovePositionOrigin;
}

const TArray<FQuat>& FSGTrackingImpl::GetSenseGloveFingerToGloveRotationOrigin()
{
    static const TArray<FQuat> GloveRotationOrigin{
        FSGArrayUtils::FromStdVector<FSGQuatImpl::FQuatType, FQuat, FSGQuatImpl, &FSGQuatImpl::ToFQuat>(
            FTrackingType::GetSenseGloveFingerToGloveRotationOrigin())
    };
    return GloveRotationOrigin;
}

const FVector& FSGTrackingImpl::GetSenseGloveWristPositionOffset()
{
    static const FVector Offset{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetSenseGloveWristPositionOffset())
    };
    return Offset;
}

const FQuat& FSGTrackingImpl::GetSenseGloveWristRotationOffset()
{
    static FQuat Offset{FSGQuatImpl(FTrackingType::GetSenseGloveWristRotationOffset()).ToFQuat()};
    return Offset;
}

void FSGTrackingImpl::GetSenseGloveMountOffset(const bool bRightHanded, const ESGFinger ToFinger,
                                               FVector& OutIPosition, FQuat& OutIRotation)
{
    FSGVect3DImpl::FVect3DType IPosition;
    FSGQuatImpl::FQuatType IRotation;

    FTrackingType::GetSenseGloveMountOffset(bRightHanded, FSGCoreEnumCast::ToEFinger(ToFinger),
                                            IPosition, IRotation);

    OutIPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(IPosition));
    OutIRotation = FSGQuatImpl(MoveTemp(IRotation)).ToFQuat();
}

void FSGTrackingImpl::GetSenseGloveTrackerMountOffset(const ESGPositionalTrackingHardware Hardware,
                                                      const bool bRightHanded,
                                                      FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGVect3DImpl::FVect3DType PositionOffset;
    FSGQuatImpl::FQuatType RotationOffset;

    FTrackingType::GetSenseGloveTrackerMountOffset(FSGCoreEnumCast::ToEPositionalTrackingHardware(Hardware),
                                                   bRightHanded, PositionOffset, RotationOffset);

    OutPositionOffset = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(PositionOffset));
    OutRotationOffset = FSGQuatImpl(MoveTemp(RotationOffset)).ToFQuat();
}

const FVector& FSGTrackingImpl::GetSenseGloveViveToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetSenseGloveViveToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetSenseGloveViveToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetSenseGloveViveToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetSenseGloveOculusTouchToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetSenseGloveOculusTouchToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetSenseGloveOculusTouchToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetSenseGloveOculusTouchToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetNovaToWristPositionOffset()
{
    static const FVector Offset{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetNovaToWristPositionOffset())
    };
    return Offset;
}

const FQuat& FSGTrackingImpl::GetNovaToWristRotationOffset()
{
    static FQuat Offset(FSGQuatImpl(FTrackingType::GetNovaToWristRotationOffset()).ToFQuat());
    return Offset;
}

const FVector& FSGTrackingImpl::GetRightNovaViveToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNovaViveToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNovaViveToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNovaViveToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNovaViveToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNovaViveToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNovaViveToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNovaViveToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNovaQuest2ToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNovaQuest2ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNovaQuest2ToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNovaQuest2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNovaQuest2ToAttachPosition()
{
    static const FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNovaQuest2ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNovaQuest2ToAttachRotation()
{
    static const FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNovaQuest2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNovaPico2ToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNovaPico2ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNovaPico2ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNovaPico2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FQuat& FSGTrackingImpl::GetLeftNovaPico2ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNovaPico2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNovaQuestProToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNovaQuestProToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNovaQuestProToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNovaQuestProToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNovaQuestProToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNovaQuestProToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNovaQuestProToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNovaQuestProToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNovaViveFocus3ToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNovaViveFocus3ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNovaViveFocus3ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNovaViveFocus3ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNovaViveFocus3ToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNovaViveFocus3ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNovaViveFocus3ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNovaViveFocus3ToAttachRotation()).ToFQuat());
    return Rotation;
}

void FSGTrackingImpl::GetNovaGloveWristOffset(const bool bRightHanded,
                                              FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGVect3DImpl::FVect3DType PositionOffset;
    FSGQuatImpl::FQuatType RotationOffset;

    FTrackingType::GetNovaGloveWristOffset(bRightHanded, PositionOffset, RotationOffset);

    OutPositionOffset = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(PositionOffset));
    OutRotationOffset = FSGQuatImpl(MoveTemp(RotationOffset)).ToFQuat();
}

void FSGTrackingImpl::GetNovaGloveTrackerOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                                FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGVect3DImpl::FVect3DType PositionOffset;
    FSGQuatImpl::FQuatType RotationOffset;

    FTrackingType::GetNovaGloveTrackerOffset(FSGCoreEnumCast::ToEPositionalTrackingHardware(Hardware), bRightHanded,
                                             PositionOffset, RotationOffset);

    OutPositionOffset = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(PositionOffset));
    OutRotationOffset = FSGQuatImpl(MoveTemp(RotationOffset)).ToFQuat();
}

const FVector& FSGTrackingImpl::GetRightNova2ToWristPositionOffset()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNova2ToWristPositionOffset())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNova2ToWristRotationOffset()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNova2ToWristRotationOffset()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNova2ToWristPositionOffset()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNova2ToWristPositionOffset())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNova2ToWristRotationOffset()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNova2ToWristRotationOffset()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNova2Quest2ToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNova2Quest2ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNova2Quest2ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNova2Quest2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNova2Quest2ToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNova2Quest2ToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNova2Quest2ToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNova2Quest2ToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNova2QuestProToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNova2QuestProToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNova2QuestProToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNova2QuestProToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNova2QuestProToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNova2QuestProToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNova2QuestProToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNova2QuestProToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNova2ViveTrackerToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNova2ViveTrackerToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNova2ViveTrackerToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNova2ViveTrackerToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNova2ViveTrackerToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNova2ViveTrackerToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNova2ViveTrackerToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNova2ViveTrackerToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetRightNova2ViveWristTrackerToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetRightNova2ViveWristTrackerToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetRightNova2ViveWristTrackerToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetRightNova2ViveWristTrackerToAttachRotation()).ToFQuat());
    return Rotation;
}

const FVector& FSGTrackingImpl::GetLeftNova2ViveWristTrackerToAttachPosition()
{
    static FVector Position{
        FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            FTrackingType::GetLeftNova2ViveWristTrackerToAttachPosition())
    };
    return Position;
}

const FQuat& FSGTrackingImpl::GetLeftNova2ViveWristTrackerToAttachRotation()
{
    static FQuat Rotation(FSGQuatImpl(FTrackingType::GetLeftNova2ViveWristTrackerToAttachRotation()).ToFQuat());
    return Rotation;
}

void FSGTrackingImpl::GetNova2GloveWristOffset(const bool bRightHanded, const FString& HardwareVersion,
                                               FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGVect3DImpl::FVect3DType PositionOffset;
    FSGQuatImpl::FQuatType RotationOffset;

    FTrackingType::GetNova2GloveWristOffset(bRightHanded, TCHAR_TO_UTF8(*HardwareVersion),
                                            PositionOffset, RotationOffset);

    OutPositionOffset = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(PositionOffset));
    OutRotationOffset = FSGQuatImpl(MoveTemp(RotationOffset)).ToFQuat();
}

void FSGTrackingImpl::GetNova2GloveTrackerOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                                 const FString& HardwareVersion,
                                                 FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGVect3DImpl::FVect3DType PositionOffset;
    FSGQuatImpl::FQuatType RotationOffset;

    FTrackingType::GetNova2GloveTrackerOffset(FSGCoreEnumCast::ToEPositionalTrackingHardware(Hardware), bRightHanded,
                                              TCHAR_TO_UTF8(*HardwareVersion),
                                              PositionOffset, RotationOffset);

    OutPositionOffset = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
        MoveTemp(PositionOffset));
    OutRotationOffset = FSGQuatImpl(MoveTemp(RotationOffset)).ToFQuat();
}

FString FSGTrackingImpl::ToString(const ESGPositionalTrackingHardware Hardware)
{
    FString String(UTF8_TO_TCHAR(
        FTrackingType::ToString(FSGCoreEnumCast::ToEPositionalTrackingHardware(Hardware)).c_str()));
    return String;
}