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

#include <string>

#include "Containers/StringConv.h"

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

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

    FHandPoseType HandPose;

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

    FSGHandPoseImpl* Owner;

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

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

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

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

FSGHandPoseImpl FSGHandPoseImpl::Deserialize(const FString& SerializedString)
{
    std::string Serialized(TCHAR_TO_UTF8(*SerializedString));
    const FSGHandPoseImpl HandPose(FHandPoseType::Deserialize(MoveTemp(Serialized)));
    return HandPose;
}

FSGHandPoseImpl FSGHandPoseImpl::FromHandAngles(const TArray<TArray<FVector>>& HandAngles, const bool bRightHanded,
                                                const FSGBasicHandModelImpl& HandDimensions)
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> HandAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(HandAngles)
    };
    return FSGHandPoseImpl(FHandPoseType::FromHandAngles(
        MoveTemp(HandAnglesVector), bRightHanded, HandDimensions.ToBasicHandModel()));
}

FSGHandPoseImpl FSGHandPoseImpl::FromHandAngles(const TArray<TArray<FVector>>& HandAngles, const bool bRightHanded)
{
    std::vector<std::vector<FSGVect3DImpl::FVect3DType>> HandAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(HandAngles)
    };
    return FSGHandPoseImpl(FHandPoseType::FromHandAngles(MoveTemp(HandAnglesVector), bRightHanded));
}

FSGHandPoseImpl FSGHandPoseImpl::DefaultIdle(const bool bRightHanded, const FSGBasicHandModelImpl& HandDimensions)
{
    return FSGHandPoseImpl(FHandPoseType::DefaultIdle(bRightHanded, HandDimensions.ToBasicHandModel()));
}

FSGHandPoseImpl FSGHandPoseImpl::DefaultIdle(const bool bRightHanded)
{
    return FSGHandPoseImpl(FHandPoseType::DefaultIdle(bRightHanded));
}

FSGHandPoseImpl FSGHandPoseImpl::FlatHand(const bool bRightHanded, const FSGBasicHandModelImpl& HandDimensions)
{
    return FSGHandPoseImpl(FHandPoseType::FlatHand(bRightHanded, HandDimensions.ToBasicHandModel()));
}

FSGHandPoseImpl FSGHandPoseImpl::FlatHand(const bool bRightHanded)
{
    return FSGHandPoseImpl(FHandPoseType::FlatHand(bRightHanded));
}

FSGHandPoseImpl FSGHandPoseImpl::ThumbsUp(const bool bRightHanded, const FSGBasicHandModelImpl& HandDimensions)
{
    return FSGHandPoseImpl(FHandPoseType::ThumbsUp(bRightHanded, HandDimensions.ToBasicHandModel()));
}

FSGHandPoseImpl FSGHandPoseImpl::ThumbsUp(const bool bRightHanded)
{
    return FSGHandPoseImpl(FHandPoseType::ThumbsUp(bRightHanded));
}

FSGHandPoseImpl FSGHandPoseImpl::Fist(const bool bRightHanded, const FSGBasicHandModelImpl& HandDimensions)
{
    return FSGHandPoseImpl(FHandPoseType::Fist(bRightHanded, HandDimensions.ToBasicHandModel()));
}

FSGHandPoseImpl FSGHandPoseImpl::Fist(const bool bRightHanded)
{
    return FSGHandPoseImpl(FHandPoseType::Fist(bRightHanded));
}

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

FSGHandPoseImpl::FSGHandPoseImpl(const bool bRightHanded, const TArray<TArray<FVector>>& JointPositions,
                                 const TArray<TArray<FQuat>>& JointRotations, const TArray<TArray<FVector>>& HandAngles)
    : 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>> HandAnglesVector{
        FSGArrayUtils::ToSenseGloveAngles<FSGVect3DImpl::FVect3DType>(HandAngles)
    };

    Pimpl->HandPose = FHandPoseType(
        bRightHanded, MoveTemp(JointPositionsVector), MoveTemp(JointRotationsVector), MoveTemp(HandAnglesVector));
}

FSGHandPoseImpl::FSGHandPoseImpl(const FHandPoseType& SenseGloveHandPose)
    : Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
    Pimpl->HandPose = SenseGloveHandPose;
}

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

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

FSGHandPoseImpl::~FSGHandPoseImpl() = default;

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

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

const FSGHandPoseImpl::FHandPoseType& FSGHandPoseImpl::ToHandPose() const
{
    return Pimpl->HandPose;
}

bool FSGHandPoseImpl::IsRight() const
{
    return Pimpl->HandPose.IsRight();
}

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

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

TArray<TArray<FVector>> FSGHandPoseImpl::GetHandAngles() const
{
    TArray<TArray<FVector>> HandAngles{
        FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(Pimpl->HandPose.GetHandAngles())
    };
    return MoveTemp(HandAngles);
}

bool FSGHandPoseImpl::Equals(const FSGHandPoseImpl& HandPoseImpl) const
{
    return Pimpl->HandPose.Equals(HandPoseImpl.ToHandPose());
}

float FSGHandPoseImpl::GetNormalizedFlexion(ESGFinger Finger, bool bClamp01) const
{
    return Pimpl->HandPose.GetNormalizedFlexion(FSGCoreEnumCast::ToEFinger(Finger), bClamp01);
}

TArray<float> FSGHandPoseImpl::GetNormalizedFlexion(bool bClamp01) const
{
    TArray<float> NormalizedFlexion{
        FSGArrayUtils::FromStdVector<float>(Pimpl->HandPose.GetNormalizedFlexion(bClamp01))
    };
    return MoveTemp(NormalizedFlexion);
}

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

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

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

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

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