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

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

#include "Runtime/Launch/Resources/Version.h"

#include "SGCore/SGSenseGloveHandProfile.h"
#include "SGCore/SGSenseGloveInfo.h"
#include "SGCoreImpl/SGExportedFunctions.h"
#include "SGCoreImpl/SGSenseGlovePoseImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveHandProfileImpl.h"
#include "SGInterop/SGIC_FSGSenseGloveInfoImpl.h"
#include "SGInterop/SGIC_FSGSenseGlovePoseImpl.h"
#include "SGInterop/SGIC_FString.h"
#include "SGInterop/SGIC_TArray_FQuat.h"
#include "SGInterop/SGIC_TArray_FVector.h"
#include "SGInterop/SGIC_TArray_TArray_FQuat.h"
#include "SGInterop/SGIC_TArray_TArray_FVector.h"
#include "SGUtils/SGArrayUtils.h"

struct USGSenseGlovePose::FImpl
{
    /************************
     * Member variables
     ************************/

    FSGSenseGlovePoseImpl SenseGlovePoseImpl;

    /************************
     * Owner object
     ************************/

    USGSenseGlovePose* Owner;

    /************************
     * Constructor / Destructor
     ************************/

    explicit FImpl(USGSenseGlovePose* InOwner);
    ~FImpl();

    /************************
     * Default copy constructor & copy assignment operator
     ************************/

    FImpl(const FImpl& Rhs) = default;
    FImpl& operator=(const FImpl& Rhs) = default;
};

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer, const FSGSenseGlovePoseImpl& SenseGlovePoseImpl)
{
    USGSenseGlovePose* Instance = NewObject<USGSenseGlovePose>(Outer);
    Instance->SetSenseGlovePoseImpl(SenseGlovePoseImpl);
    return Instance;
}

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor(&OutSenseGlovePoseImplContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer, const bool bRightHanded,
                                                        const TArray<TArray<FVector>>& JointPositions,
                                                        const TArray<TArray<FQuat>>& JointRotations,
                                                        const TArray<TArray<FVector>>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{JointPositions};
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{JointRotations};
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{GloveAngles};
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer, const bool bRightHanded,
                                                        const TArray<TArray<FVector>>& JointPositions,
                                                        const TArray<TArray<FRotator>>& JointRotations,
                                                        const TArray<TArray<FVector>>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{JointPositions};
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{FSGArrayUtils::RotatorsToQuats(JointRotations)};
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{GloveAngles};
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer, const bool bRightHanded,
                                                        const TArray<FSGVectorArray>& JointPositions,
                                                        const TArray<FSGQuatArray>& JointRotations,
                                                        const TArray<FSGVectorArray>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(JointPositions)
    };
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{
        FSGArrayUtils::FromNestedBPArray<FQuat, FSGQuatArray, &FSGQuatArray::QuatArray>(JointRotations)
    };
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(GloveAngles)
    };
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::NewSenseGlovePose(UObject* Outer, const bool bRightHanded,
                                                        const TArray<FSGVectorArray>& JointPositions,
                                                        const TArray<FSGRotatorArray>& JointRotations,
                                                        const TArray<FSGVectorArray>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(JointPositions)
    };
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{
        FSGArrayUtils::RotatorsToQuats<FSGRotatorArray, &FSGRotatorArray::RotatorArray>(JointRotations)
    };
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(GloveAngles)
    };
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::IdlePose(UObject* Outer, const USGSenseGloveInfo* GloveInfo)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_FSGSenseGloveInfoImpl SenseGloveInfoContainer{GloveInfo->ToSenseGloveInfoImpl()};
    SGCoreImpl_FSGSenseGlovePoseImpl_IdlePose(&OutSenseGlovePoseImplContainer, &SenseGloveInfoContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose* USGSenseGlovePose::Deserialize(UObject* Outer, const FString& SerializedString)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_FString SerializedStringContainer{SerializedString};
    SGCoreImpl_FSGSenseGlovePoseImpl_Deserialize(&OutSenseGlovePoseImplContainer, &SerializedStringContainer);

    return NewSenseGlovePose(Outer, MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl));
}

USGSenseGlovePose::USGSenseGlovePose()
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor(&OutSenseGlovePoseImplContainer);

    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl);
}

USGSenseGlovePose::USGSenseGlovePose(const bool bRightHanded,
                                     const TArray<TArray<FVector>>& JointPositions,
                                     const TArray<TArray<FQuat>>& JointRotations,
                                     const TArray<TArray<FVector>>& GloveAngles)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{JointPositions};
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{JointRotations};
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{GloveAngles};
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl);
}

USGSenseGlovePose::USGSenseGlovePose(const bool bRightHanded,
                                     const TArray<TArray<FVector>>& JointPositions,
                                     const TArray<TArray<FRotator>>& JointRotations,
                                     const TArray<TArray<FVector>>& GloveAngles)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{JointPositions};
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{FSGArrayUtils::RotatorsToQuats(JointRotations)};
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{GloveAngles};
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl);
}

USGSenseGlovePose::USGSenseGlovePose(const bool bRightHanded,
                                     const TArray<FSGVectorArray>& JointPositions,
                                     const TArray<FSGQuatArray>& JointRotations,
                                     const TArray<FSGVectorArray>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(JointPositions)
    };
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{
        FSGArrayUtils::FromNestedBPArray<FQuat, FSGQuatArray, &FSGQuatArray::QuatArray>(JointRotations)
    };
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(GloveAngles)
    };
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl);
}

USGSenseGlovePose::USGSenseGlovePose(const bool bRightHanded,
                                     const TArray<FSGVectorArray>& JointPositions,
                                     const TArray<FSGRotatorArray>& JointRotations,
                                     const TArray<FSGVectorArray>& GloveAngles)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseImplContainer;
    FSGIC_TArray_TArray_FVector JointPositionsContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(JointPositions)
    };
    FSGIC_TArray_TArray_FQuat JointRotationsContainer{
        FSGArrayUtils::RotatorsToQuats<FSGRotatorArray, &FSGRotatorArray::RotatorArray>(JointRotations)
    };
    FSGIC_TArray_TArray_FVector GloveAnglesContainer{
        FSGArrayUtils::FromNestedBPArray<FVector, FSGVectorArray, &FSGVectorArray::VectorArray>(GloveAngles)
    };
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_bRightHanded_JointPositions_JointRotations_GloveAngles(
        &OutSenseGlovePoseImplContainer, bRightHanded, &JointPositionsContainer,
        &JointRotationsContainer, &GloveAnglesContainer);

    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseImplContainer.SenseGlovePoseImpl);
}

USGSenseGlovePose::USGSenseGlovePose(const FSGSenseGlovePoseImpl& SenseGlovePoseImpl)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    SetSenseGlovePoseImpl(SenseGlovePoseImpl);
}

USGSenseGlovePose::~USGSenseGlovePose() = default;

void USGSenseGlovePose::SetSenseGlovePoseImpl(const FSGSenseGlovePoseImpl& SenseGlovePoseImpl)
{
    FSGIC_FSGSenseGlovePoseImpl OutSenseGlovePoseContainer;
    FSGIC_FSGSenseGlovePoseImpl RhsContainer{SenseGlovePoseImpl};
    SGCoreImpl_FSGSenseGlovePoseImpl_ctor_copy(&OutSenseGlovePoseContainer, &RhsContainer);
    Pimpl->SenseGlovePoseImpl = MoveTemp(OutSenseGlovePoseContainer.SenseGlovePoseImpl);
}

const FSGSenseGlovePoseImpl& USGSenseGlovePose::ToSenseGlovePoseImpl() const
{
    return Pimpl->SenseGlovePoseImpl;
}

bool USGSenseGlovePose::IsRight() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    return SGCoreImpl_FSGSenseGlovePoseImpl_IsRight(&InstanceContainer);
}

TArray<TArray<FVector>> USGSenseGlovePose::GetJointPositions() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_TArray_FVector OutPositionsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetJointPositions(&InstanceContainer, &OutPositionsContainer);
    return MoveTemp(OutPositionsContainer.Array);
}

TArray<TArray<FQuat>> USGSenseGlovePose::GetJointRotationsQ() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_TArray_FQuat OutRotationsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetJointRotations(&InstanceContainer, &OutRotationsContainer);
    return MoveTemp(OutRotationsContainer.Array);
}

TArray<TArray<FRotator>> USGSenseGlovePose::GetJointRotations() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_TArray_FQuat OutRotationsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetJointRotations(&InstanceContainer, &OutRotationsContainer);
    return FSGArrayUtils::QuatsToRotators(OutRotationsContainer.Array);
}

TArray<TArray<FVector>> USGSenseGlovePose::GetGloveAngles() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_TArray_FVector OutAnglesContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetGloveAngles(&InstanceContainer, &OutAnglesContainer);
    return MoveTemp(OutAnglesContainer.Array);
}

TArray<FVector> USGSenseGlovePose::GetThimblePositions() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FVector OutPositionsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetThimblePositions(&InstanceContainer, &OutPositionsContainer);
    return MoveTemp(OutPositionsContainer.Array);
}

TArray<FQuat> USGSenseGlovePose::GetThimbleRotationsQ() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FQuat OutRotationsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetThimbleRotations(&InstanceContainer, &OutRotationsContainer);
    return MoveTemp(OutRotationsContainer.Array);
}

TArray<FRotator> USGSenseGlovePose::GetThimbleRotations() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FQuat OutRotationsContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetThimbleRotations(&InstanceContainer, &OutRotationsContainer);
    return FSGArrayUtils::QuatsToRotators(OutRotationsContainer.Array);
}

TArray<FVector> USGSenseGlovePose::GetTotalGloveAngles() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FVector OutAnglesContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_GetTotalGloveAngles(&InstanceContainer, &OutAnglesContainer);
    return MoveTemp(OutAnglesContainer.Array);
}

TArray<FVector> USGSenseGlovePose::CalculateFingerTips(const USGSenseGloveHandProfile* SenseGloveHandProfile) const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FVector OutFingerTipsContainer;
    FSGIC_FSGSenseGloveHandProfileImpl SenseGloveHandProfileImplContainer{
        SenseGloveHandProfile->ToSenseGloveHandProfileImpl()};
    SGCoreImpl_FSGSenseGlovePoseImpl_CalculateFingerTips_SenseGloveHandProfileImpl(
        &InstanceContainer, &OutFingerTipsContainer, &SenseGloveHandProfileImplContainer);
    return MoveTemp(OutFingerTipsContainer.Array);
}

TArray<FVector> USGSenseGlovePose::CalculateFingerTips(const TArray<FVector>& FingerOffsets) const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_TArray_FVector OutFingerTipsContainer;
    FSGIC_TArray_FVector FingerOffsetsContainer{FingerOffsets};
    SGCoreImpl_FSGSenseGlovePoseImpl_CalculateFingerTips_FingerOffsets(&InstanceContainer, &OutFingerTipsContainer,
                                                                       &FingerOffsetsContainer);
    return MoveTemp(OutFingerTipsContainer.Array);
}

bool USGSenseGlovePose::Equals(const USGSenseGlovePose* SenseGlovePose) const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_FSGSenseGlovePoseImpl SenseGlovePoseContainer{SenseGlovePose->ToSenseGlovePoseImpl()};
    return SGCoreImpl_FSGSenseGlovePoseImpl_Equals(&InstanceContainer, &SenseGlovePoseContainer);
}

FString USGSenseGlovePose::ToString(const bool bShortFormat) const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_FString OutStringContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_ToString(&InstanceContainer, &OutStringContainer, bShortFormat);
    return MoveTemp(OutStringContainer.String);
}

FString USGSenseGlovePose::SGSerialize() const
{
    FSGIC_FSGSenseGlovePoseImpl InstanceContainer{ToSenseGlovePoseImpl()};
    FSGIC_FString OutStringContainer;
    SGCoreImpl_FSGSenseGlovePoseImpl_Serialize(&InstanceContainer, &OutStringContainer);
    return MoveTemp(OutStringContainer.String);
}

USGSenseGlovePose::FImpl::FImpl(USGSenseGlovePose* InOwner)
    : Owner(InOwner)
{
}

USGSenseGlovePose::FImpl::~FImpl() = default;

void USGSenseGlovePose::FImplDeleter::operator()(const FImpl* P) const
{
    delete P;
}