Plugin/Source/SenseGloveCore/Private/SGCore/SGTracking.cpp

/**
 * @file
 *
 * @author  Mamadou Babaei <mamadou@senseglove.com>
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2020 - 2022 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 "SGCore/SGTracking.h"

#include "SGCoreImpl/SGExportedFunctions.h"
#include "SGInterop/SGIC_ESGFinger.h"
#include "SGInterop/SGIC_ESGPositionalTrackingHardware.h"
#include "SGInterop/SGIC_FQuat.h"
#include "SGInterop/SGIC_FString.h"
#include "SGInterop/SGIC_FVector.h"
#include "SGInterop/SGIC_TArray_FQuat.h"
#include "SGInterop/SGIC_TArray_FVector.h"
#include "SGUtils/SGArrayUtils.h"

void FSGTracking::CalculateLocation(const FVector& TrackedPosition, const FQuat& TrackedRotation,
                                    const FVector& PositionOffset, const FQuat& RotationOffset,
                                    FVector& OutNewPosition, FQuat& OutNewRotation)
{
    FSGIC_FVector TrackedPositionContainer{TrackedPosition};
    FSGIC_FQuat TrackedRotationContainer{TrackedRotation};
    FSGIC_FVector PositionOffsetContainer{PositionOffset};
    FSGIC_FQuat RotationOffsetContainer{RotationOffset};
    FSGIC_FVector OutNewPositionContainer;
    FSGIC_FQuat OutNewRotationContainer;

    SGCoreImpl_FSGTrackingImpl_CalculateLocation(
        &TrackedPositionContainer, &TrackedRotationContainer,
        &PositionOffsetContainer, &RotationOffsetContainer,
        &OutNewPositionContainer, &OutNewRotationContainer);

    OutNewPosition = MoveTemp(OutNewPositionContainer.Vector);
    OutNewRotation = MoveTemp(OutNewRotationContainer.Quat);
}

void FSGTracking::CalculateLocation(const FVector& TrackedPosition, const FRotator& TrackedRotation,
                                    const FVector& PositionOffset, const FRotator& RotationOffset,
                                    FVector& OutNewPosition, FRotator& OutNewRotation)
{
    FSGIC_FVector TrackedPositionContainer{TrackedPosition};
    FSGIC_FQuat TrackedRotationContainer{TrackedRotation.Quaternion()};
    FSGIC_FVector PositionOffsetContainer{PositionOffset};
    FSGIC_FQuat RotationOffsetContainer{RotationOffset.Quaternion()};
    FSGIC_FVector OutNewPositionContainer;
    FSGIC_FQuat OutNewRotationContainer;

    SGCoreImpl_FSGTrackingImpl_CalculateLocation(
        &TrackedPositionContainer, &TrackedRotationContainer,
        &PositionOffsetContainer, &RotationOffsetContainer,
        &OutNewPositionContainer, &OutNewRotationContainer);

    OutNewPosition = MoveTemp(OutNewPositionContainer.Vector);
    OutNewRotation = OutNewRotationContainer.Quat.Rotator();
}

TArray<FVector> FSGTracking::GetSenseGloveFingerToGlovePositionOrigin()
{
    FSGIC_TArray_FVector OutOriginContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveFingerToGlovePositionOrigin(&OutOriginContainer);
    return MoveTemp(OutOriginContainer.Array);
}

TArray<FQuat> FSGTracking::GetSenseGloveFingerToGloveRotationOriginQ()
{
    FSGIC_TArray_FQuat OutOriginContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveFingerToGloveRotationOrigin(&OutOriginContainer);
    return MoveTemp(OutOriginContainer.Array);
}

TArray<FRotator> FSGTracking::GetSenseGloveFingerToGloveRotationOrigin()
{
    FSGIC_TArray_FQuat OutOriginContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveFingerToGloveRotationOrigin(&OutOriginContainer);
    return FSGArrayUtils::QuatsToRotators(OutOriginContainer.Array);
}

FVector FSGTracking::GetSenseGloveWristPositionOffset()
{
    FSGIC_FVector OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveWristPositionOffset(&OutOffsetContainer);
    return MoveTemp(OutOffsetContainer.Vector);
}

FQuat FSGTracking::GetSenseGloveWristRotationOffsetQ()
{
    FSGIC_FQuat OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveWristRotationOffset(&OutOffsetContainer);
    return MoveTemp(OutOffsetContainer.Quat);
}

FRotator FSGTracking::GetSenseGloveWristRotationOffset()
{
    FSGIC_FQuat OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveWristRotationOffset(&OutOffsetContainer);
    return OutOffsetContainer.Quat.Rotator();
}

void FSGTracking::GetSenseGloveMountOffset(const bool bRightHanded, const ESGFinger ToFinger,
                                           FVector& OutIPosition, FQuat& OutIRotation)
{
    FSGIC_ESGFinger ToFingerContainer{ToFinger};
    FSGIC_FVector OutIPositionContainer;
    FSGIC_FQuat OutIRotationContainer;

    SGCoreImpl_FSGTrackingImpl_GetSenseGloveMountOffset(bRightHanded, &ToFingerContainer,
                                                        &OutIPositionContainer, &OutIRotationContainer);

    OutIPosition = MoveTemp(OutIPositionContainer.Vector);
    OutIRotation = MoveTemp(OutIRotationContainer.Quat);
}

void FSGTracking::GetSenseGloveMountOffset(const bool bRightHanded, const ESGFinger ToFinger,
                                           FVector& OutIPosition, FRotator& OutIRotation)
{
    FSGIC_ESGFinger ToFingerContainer{ToFinger};
    FSGIC_FVector OutIPositionContainer;
    FSGIC_FQuat OutIRotationContainer;

    SGCoreImpl_FSGTrackingImpl_GetSenseGloveMountOffset(bRightHanded, &ToFingerContainer,
                                                        &OutIPositionContainer, &OutIRotationContainer);

    OutIPosition = MoveTemp(OutIPositionContainer.Vector);
    OutIRotation = OutIRotationContainer.Quat.Rotator();
}

void FSGTracking::GetSenseGloveTrackerMountOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                                  FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGIC_ESGPositionalTrackingHardware HardwareContainer;
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetSenseGloveTrackerMountOffset(&HardwareContainer, bRightHanded,
                                                               &OutPositionOffsetContainer,
                                                               &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = MoveTemp(OutRotationOffsetContainer.Quat);
}

void FSGTracking::GetSenseGloveTrackerMountOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                                  FVector& OutPositionOffset, FRotator& OutRotationOffset)
{
    FSGIC_ESGPositionalTrackingHardware HardwareContainer;
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetSenseGloveTrackerMountOffset(&HardwareContainer, bRightHanded,
                                                               &OutPositionOffsetContainer,
                                                               &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = OutRotationOffsetContainer.Quat.Rotator();
}

FVector FSGTracking::GetSenseGloveViveToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveViveToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetSenseGloveViveToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveViveToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetSenseGloveViveToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveViveToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetSenseGloveOculusTouchToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveOculusTouchToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetSenseGloveOculusTouchToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveOculusTouchToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetSenseGloveOculusTouchToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetSenseGloveOculusTouchToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetNovaToWristPositionOffset()
{
    FSGIC_FVector OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetNovaToWristPositionOffset(&OutOffsetContainer);
    return MoveTemp(OutOffsetContainer.Vector);
}

FQuat FSGTracking::GetNovaToWristRotationOffsetQ()
{
    FSGIC_FQuat OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetNovaToWristRotationOffset(&OutOffsetContainer);
    return MoveTemp(OutOffsetContainer.Quat);
}

FRotator FSGTracking::GetNovaToWristRotationOffset()
{
    FSGIC_FQuat OutOffsetContainer;
    SGCoreImpl_FSGTrackingImpl_GetNovaToWristRotationOffset(&OutOffsetContainer);
    return OutOffsetContainer.Quat.Rotator();
}

void FSGTracking::GetNovaGloveWristOffset(const bool bRightHanded, FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetNovaGloveWristOffset(bRightHanded,
                                                       &OutPositionOffsetContainer, &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = MoveTemp(OutRotationOffsetContainer.Quat);
}

void FSGTracking::GetNovaGloveWristOffset(const bool bRightHanded,
                                          FVector& OutPositionOffset, FRotator& OutRotationOffset)
{
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetNovaGloveWristOffset(bRightHanded,
                                                       &OutPositionOffsetContainer, &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = OutRotationOffsetContainer.Quat.Rotator();
}

FVector FSGTracking::GetRightNovaViveToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaViveToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetRightNovaViveToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaViveToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetRightNovaViveToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaViveToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetLeftNovaViveToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaViveToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetLeftNovaViveToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaViveToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetLeftNovaViveToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaViveToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetRightNovaQuest2ToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaQuest2ToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetRightNovaQuest2ToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaQuest2ToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetRightNovaQuest2ToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaQuest2ToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetLeftNovaQuest2ToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaQuest2ToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetLeftNovaQuest2ToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaQuest2ToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetLeftNovaQuest2ToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaQuest2ToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FVector FSGTracking::GetRightNovaPico2ToAttachPosition()
{
    FSGIC_FVector OutPositionContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaPico2ToAttachPosition(&OutPositionContainer);
    return MoveTemp(OutPositionContainer.Vector);
}

FQuat FSGTracking::GetRightNovaPico2ToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaPico2ToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetRightNovaPico2ToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetRightNovaPico2ToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

FQuat FSGTracking::GetLeftNovaPico2ToAttachRotationQ()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaPico2ToAttachRotation(&OutRotationContainer);
    return MoveTemp(OutRotationContainer.Quat);
}

FRotator FSGTracking::GetLeftNovaPico2ToAttachRotation()
{
    FSGIC_FQuat OutRotationContainer;
    SGCoreImpl_FSGTrackingImpl_GetLeftNovaPico2ToAttachRotation(&OutRotationContainer);
    return OutRotationContainer.Quat.Rotator();
}

void FSGTracking::GetNovaGloveTrackerOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                            FVector& OutPositionOffset, FQuat& OutRotationOffset)
{
    FSGIC_ESGPositionalTrackingHardware HardwareContainer{Hardware};
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetNovaGloveTrackerOffset(&HardwareContainer, bRightHanded,
                                                         &OutPositionOffsetContainer, &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = MoveTemp(OutRotationOffsetContainer.Quat);
}

void FSGTracking::GetNovaGloveTrackerOffset(const ESGPositionalTrackingHardware Hardware, const bool bRightHanded,
                                            FVector& OutPositionOffset, FRotator& OutRotationOffset)
{
    FSGIC_ESGPositionalTrackingHardware HardwareContainer{Hardware};
    FSGIC_FVector OutPositionOffsetContainer;
    FSGIC_FQuat OutRotationOffsetContainer;

    SGCoreImpl_FSGTrackingImpl_GetNovaGloveTrackerOffset(&HardwareContainer, bRightHanded,
                                                         &OutPositionOffsetContainer, &OutRotationOffsetContainer);

    OutPositionOffset = MoveTemp(OutPositionOffsetContainer.Vector);
    OutRotationOffset = OutRotationOffsetContainer.Quat.Rotator();
}

FString FSGTracking::ToString(const ESGPositionalTrackingHardware Hardware)
{
    FSGIC_FString OutStringContainer;
    FSGIC_ESGPositionalTrackingHardware HardwareContainer{Hardware};
    SGCoreImpl_FSGTrackingImpl_ToString(&OutStringContainer, &HardwareContainer);
    return MoveTemp(OutStringContainer.String);
}