Plugin/Source/SenseGloveCoreImpl/Private/SGCoreImpl/SGSenseGlovePoseImpl.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 "SGCoreImpl/SGSenseGlovePoseImpl.h"

#include <string>
#include <vector>

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

#include "SGBuildHacks/SGInclude_Core_Quat.h"
#include "SGBuildHacks/SGInclude_Core_SenseGlovePose.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGSenseGloveHandProfileImpl.h"
#include "SGCoreImpl/SGSenseGloveInfoImpl.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"

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

    FSenseGlovePoseType SenseGlovePose;

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

    FSGSenseGlovePoseImpl* Owner;

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

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

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

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

FSGSenseGlovePoseImpl FSGSenseGlovePoseImpl::IdlePose(const FSGSenseGloveInfoImpl& GloveInfo)
{
    FSGSenseGlovePoseImpl GlovePoseImpl(FSenseGlovePoseType::IdlePose(GloveInfo.ToSenseGloveInfo()));
    return MoveTemp(GlovePoseImpl);
}

FSGSenseGlovePoseImpl FSGSenseGlovePoseImpl::Deserialize(const FString& SerializedString)
{
    std::string Serialized(TCHAR_TO_UTF8(*SerializedString));
    FSGSenseGlovePoseImpl GlovePose(FSenseGlovePoseType::Deserialize(MoveTemp(Serialized)));
    return MoveTemp(GlovePose);
}

FSGSenseGlovePoseImpl::FSGSenseGlovePoseImpl()
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}

FSGSenseGlovePoseImpl::FSGSenseGlovePoseImpl(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))
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> JointPositionsVector{
        FSGArrayUtils::ToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(JointPositions)
    };
    std::vector<std::vector<FSGQuatImpl::FQuatType>> JointRotationsVector{
        FSGArrayUtils::ToStdVector<FQuat, FSGQuatImpl::FQuatType, FSGQuatImpl, &FSGQuatImpl::ToQuat>(JointRotations)
    };
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> GloveAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(GloveAngles)
    };

    Pimpl->SenseGlovePose = FSenseGlovePoseType(bRightHanded,
                                                MoveTemp(JointPositionsVector),
                                                MoveTemp(JointRotationsVector),
                                                MoveTemp(GloveAnglesVector));
}

FSGSenseGlovePoseImpl::FSGSenseGlovePoseImpl(const FSenseGlovePoseType& SenseGlovePose)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    Pimpl->SenseGlovePose = SenseGlovePose;
}

FSGSenseGlovePoseImpl::FSGSenseGlovePoseImpl(const FSGSenseGlovePoseImpl& Rhs)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}, PimplDeleter))
{
    Pimpl->Owner = this;
}

FSGSenseGlovePoseImpl::FSGSenseGlovePoseImpl(FSGSenseGlovePoseImpl&& Rhs) SG_NOEXCEPT = default;

FSGSenseGlovePoseImpl::~FSGSenseGlovePoseImpl() = default;

FSGSenseGlovePoseImpl& FSGSenseGlovePoseImpl::operator=(const FSGSenseGlovePoseImpl& Rhs)
{
    *Pimpl = *Rhs.Pimpl;
    Pimpl->Owner = this;
    return *this;
}

FSGSenseGlovePoseImpl& FSGSenseGlovePoseImpl::operator=(FSGSenseGlovePoseImpl&& Rhs) SG_NOEXCEPT = default;

const FSGSenseGlovePoseImpl::FSenseGlovePoseType& FSGSenseGlovePoseImpl::ToSenseGlovePose() const
{
    return Pimpl->SenseGlovePose;
}

bool FSGSenseGlovePoseImpl::IsRight() const
{
    return Pimpl->SenseGlovePose.IsRight();
}

TArray<TArray<FVector>> FSGSenseGlovePoseImpl::GetJointPositions() const
{
    TArray<TArray<FVector>> JointPositions{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->SenseGlovePose.GetJointPositions())
    };
    return MoveTemp(JointPositions);
}

TArray<TArray<FQuat>> FSGSenseGlovePoseImpl::GetJointRotations() const
{
    TArray<TArray<FQuat>> JointRotations{
        FSGArrayUtils::FromStdVector<FSGQuatImpl::FQuatType, FQuat, FSGQuatImpl, &FSGQuatImpl::ToFQuat>(
            Pimpl->SenseGlovePose.GetJointRotations())
    };
    return MoveTemp(JointRotations);
}

TArray<TArray<FVector>> FSGSenseGlovePoseImpl::GetGloveAngles() const
{
    TArray<TArray<FVector>> GloveAngles{
        FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(
            Pimpl->SenseGlovePose.GetGloveAngles())
    };
    return MoveTemp(GloveAngles);
}

TArray<FVector> FSGSenseGlovePoseImpl::GetThimblePositions() const
{
    TArray<FVector> ThimblePositions{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->SenseGlovePose.GetThimblePositions())
    };
    return MoveTemp(ThimblePositions);
}

TArray<FQuat> FSGSenseGlovePoseImpl::GetThimbleRotations() const
{
    TArray<FQuat> ThimbleRotations{
        FSGArrayUtils::FromStdVector<FSGQuatImpl::FQuatType, FQuat, FSGQuatImpl, &FSGQuatImpl::ToFQuat>(
            Pimpl->SenseGlovePose.GetThimbleRotations())
    };
    return MoveTemp(ThimbleRotations);
}

TArray<FVector> FSGSenseGlovePoseImpl::GetTotalGloveAngles() const
{
    TArray<FVector> TotalGloveAngles{
        FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(Pimpl->SenseGlovePose.GetTotalGloveAngles())
    };
    return MoveTemp(TotalGloveAngles);
}

TArray<FVector> FSGSenseGlovePoseImpl::CalculateFingerTips(
    const FSGSenseGloveHandProfileImpl& SenseGloveHandProfileImpl) const
{
    TArray<FVector> FingerTips{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->SenseGlovePose.CalculateFingerTips(SenseGloveHandProfileImpl.ToSenseGloveHandProfile()))
    };
    return MoveTemp(FingerTips);
}

TArray<FVector> FSGSenseGlovePoseImpl::CalculateFingerTips(const TArray<FVector>& FingerOffsets) const
{
    std::vector<FSGVect3DImpl::FVect3DType> FingerOffsetsVector{
        FSGArrayUtils::ToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(FingerOffsets)
    };
    TArray<FVector> FingerTips{
        FSGArrayUtils::FromMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
            Pimpl->SenseGlovePose.CalculateFingerTips(MoveTemp(FingerOffsetsVector)))
    };
    return MoveTemp(FingerTips);
}

bool FSGSenseGlovePoseImpl::Equals(const FSGSenseGlovePoseImpl& SenseGlovePoseImpl) const
{
    return Pimpl->SenseGlovePose.Equals(SenseGlovePoseImpl.ToSenseGlovePose());
}

FString FSGSenseGlovePoseImpl::ToString(bool bShortFormat) const
{
    FString String(UTF8_TO_TCHAR(Pimpl->SenseGlovePose.ToString(bShortFormat).c_str()));
    return MoveTemp(String);
}

FString FSGSenseGlovePoseImpl::Serialize() const
{
    FString Serialized(UTF8_TO_TCHAR(Pimpl->SenseGlovePose.Serialize().c_str()));
    return MoveTemp(Serialized);
}

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

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

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