Files
Plugin/Source/SenseGloveCoreImpl/Private/SGCoreImpl/SGNova2GloveImpl.cpp
T

550 lines
19 KiB
C++

/**
* @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/SGNova2GloveImpl.h"
#include <memory>
#include <vector>
#include "Containers/StringConv.h"
#include "Misc/AssertionMacros.h"
#include "SGBuildHacks/SGInclude_Core_BasicHandModel.h"
#include "SGBuildHacks/SGInclude_Core_CustomWaveform.h"
#include "SGBuildHacks/SGInclude_Core_HandInterpolator.h"
#include "SGBuildHacks/SGInclude_Core_HandPose.h"
#include "SGBuildHacks/SGInclude_Core_HapticGlove.h"
#include "SGBuildHacks/SGInclude_Core_Nova2Glove.h"
#include "SGBuildHacks/SGInclude_Core_Nova2GloveSensorData.h"
#include "SGBuildHacks/SGInclude_Core_NovaGloveInfo.h"
#include "SGBuildHacks/SGInclude_Core_Quat.h"
#include "SGBuildHacks/SGInclude_Core_SensorNormalization.h"
#include "SGBuildHacks/SGInclude_Core_Vect3D.h"
#include "SGCoreImpl/SGBasicHandModelImpl.h"
#include "SGCoreImpl/SGCoreEnumCast.h"
#include "SGCoreImpl/SGCustomWaveformImpl.h"
#include "SGCoreImpl/SGHandInterpolatorImpl.h"
#include "SGCoreImpl/SGHandPoseImpl.h"
#include "SGCoreImpl/SGNova2GloveSensorDataImpl.h"
#include "SGCoreImpl/SGNovaGloveInfoImpl.h"
#include "SGCoreImpl/SGQuatImpl.h"
#include "SGCoreImpl/SGSensorNormalizationImpl.h"
#include "SGCoreImpl/SGVect3DImpl.h"
#include "SGUtils/SGArrayUtils.h"
struct FSGNova2GloveImpl::FImpl
{
/************************
* Owner object
************************/
FSGNova2GloveImpl* Owner;
/************************
* Constructor / Destructor
************************/
explicit FImpl(FSGNova2GloveImpl* InOwner);
~FImpl();
/************************
* Default copy constructor & copy assignment operator
************************/
FImpl(const FImpl& Rhs) = default;
FImpl& operator=(const FImpl& Rhs) = default;
};
TSharedRef<FSGDeviceImpl> FSGNova2GloveImpl::Parse(const FString& ConstantsString)
{
std::string ConstantsStdString{TCHAR_TO_UTF8(*ConstantsString)};
FSGDevicePtrType SGDevicePtr{FNova2GloveType::Parse(MoveTemp(ConstantsStdString))};
ensureAlwaysMsgf(SGDevicePtr, TEXT("Invalid SGDevice!"));
const TSharedRef<FSGNova2GloveImpl> Nova2GloveImpl{
MakeShared<FSGNova2GloveImpl>(std::dynamic_pointer_cast<FNova2GloveType>(MoveTemp(SGDevicePtr)))
};
return Nova2GloveImpl;
}
TArray<FSGNova2GloveImpl> FSGNova2GloveImpl::GetNova2Gloves()
{
FNova2GloveVectorType Vector{FNova2GloveType::GetNova2Gloves()};
const TArray<FSGNova2GloveImpl> Array{
FSGArrayUtils::FromStdVector<FNova2GloveType, FSGNova2GloveImpl>(MoveTemp(Vector))
};
return Array;
}
bool FSGNova2GloveImpl::GetNova2Glove(FSGNova2GloveImpl& OutGlove)
{
FNova2GloveType Glove;
const bool bResult = FNova2GloveType::GetNova2Glove(Glove);
OutGlove = FSGNova2GloveImpl{MoveTemp(Glove)};
return bResult;
}
bool FSGNova2GloveImpl::GetNova2Glove(const bool bRightHanded, FSGNova2GloveImpl& OutGlove)
{
FNova2GloveType Glove;
const bool bResult = FNova2GloveType::GetNova2Glove(bRightHanded, Glove);
OutGlove = FSGNova2GloveImpl{MoveTemp(Glove)};
return bResult;
}
void FSGNova2GloveImpl::CalculateGloveLocation(
const FVector& ReferencePosition, const FQuat& ReferenceRotation,
const ESGPositionalTrackingHardware TrackingHardware, const bool bRightHanded, const FString& HardwareVersion,
FVector& OutGlovePosition, FQuat& OutGloveRotation)
{
FSGVect3DImpl::FVect3DType GlovePosition;
FSGQuatImpl::FQuatType GloveRotation;
FNova2GloveType::CalculateGloveLocation(
FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
FSGQuatImpl{ReferenceRotation}.ToQuat(),
FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware), bRightHanded, TCHAR_TO_UTF8(*HardwareVersion),
GlovePosition, GloveRotation);
OutGlovePosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
MoveTemp(GlovePosition));;
OutGloveRotation = FSGQuatImpl{MoveTemp(GloveRotation)}.ToFQuat();
}
void FSGNova2GloveImpl::CalculateWristLocation(
const FVector& ReferencePosition, const FQuat& ReferenceRotation,
const ESGPositionalTrackingHardware TrackingHardware, const bool bRightHanded, const FString& HardwareVersion,
FVector& OutWristPosition, FQuat& OutWristRotation)
{
FSGVect3DImpl::FVect3DType WristPosition;
FSGQuatImpl::FQuatType WristRotation;
FNova2GloveType::CalculateWristLocation(
FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
FSGQuatImpl{ReferenceRotation}.ToQuat(),
FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware), bRightHanded, TCHAR_TO_UTF8(*HardwareVersion),
WristPosition, WristRotation);
OutWristPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
MoveTemp(WristPosition));
OutWristRotation = FSGQuatImpl{MoveTemp(WristRotation)}.ToFQuat();
}
ANSICHAR FSGNova2GloveImpl::GetStreamByte()
{
static const ANSICHAR Byte = FNova2GloveType::GetStreamByte();
return Byte;
}
const TArray<float>& FSGNova2GloveImpl::GetNova2SensorRanges()
{
static const TArray<float> Ranges{FSGArrayUtils::FromStdVector(FNova2GloveType::GetNova2SensorRanges())};
return Ranges;
}
TArray<float> FSGNova2GloveImpl::ToNormalizedValues(const FSGNova2GloveSensorDataImpl& SensorData)
{
const TArray<float> NormalizedValues{
FSGArrayUtils::FromStdVector(
FNova2GloveType::ToNormalizedValues(SensorData.ToNova2GloveSensorData()))
};
return NormalizedValues;
}
TArray<TArray<FVector>> FSGNova2GloveImpl::CalculateHandAngles(
const TArray<float>& NormalizedValues, const FSGHandInterpolatorImpl& Interpolator, const bool bInfluenceIndex)
{
std::vector<float> NormalizedValuesVector{FSGArrayUtils::ToStdVector(NormalizedValues)};
const TArray<TArray<FVector>> Angles{
FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(
FNova2GloveType::CalculateHandAngles(
MoveTemp(NormalizedValuesVector), Interpolator.ToHandInterpolator(), bInfluenceIndex))
};
return Angles;
}
TArray<TArray<FVector>> FSGNova2GloveImpl::ToNormalizedAngles(
const TArray<float>& NormalizedData, const FSGNovaGloveInfoImpl& GloveInfo)
{
std::vector<float> NormalizedDataVector{FSGArrayUtils::ToStdVector(NormalizedData)};
const TArray<TArray<FVector>> Angles{
FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(
FNova2GloveType::ToNormalizedAngles(MoveTemp(NormalizedDataVector), GloveInfo.ToNovaGloveInfo()))
};
return Angles;
}
TArray<float> FSGNova2GloveImpl::ToNormalizedValues(const FSGNova2GloveSensorDataImpl& SensorData,
FSGSensorNormalizationImpl& OutSensorNormalizer)
{
const TArray<float> NormalizedValues{
FSGArrayUtils::FromStdVector(
FNova2GloveType::ToNormalizedValues(SensorData.ToNova2GloveSensorData(),
OutSensorNormalizer.ToSensorNormalization()))
};
return NormalizedValues;
}
FSGNova2GloveImpl::FSGNova2GloveImpl()
: FSGHapticGloveImpl(),
Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}
FSGNova2GloveImpl::FSGNova2GloveImpl(const FSGNovaGloveInfoImpl& DeviceInfo)
: FSGHapticGloveImpl(FSGNova2GloveImpl(std::make_shared<FNova2GloveType>((DeviceInfo.ToNovaGloveInfo())))),
Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}
FSGNova2GloveImpl::FSGNova2GloveImpl(const FNova2GloveType Nova2Glove)
: FSGHapticGloveImpl(std::make_shared<FNova2GloveType>(Nova2Glove)),
Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}
FSGNova2GloveImpl::FSGNova2GloveImpl(const FNova2GlovePtrType Nova2Glove)
: FSGHapticGloveImpl(Nova2Glove),
Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{this}, PimplDeleter))
{
}
FSGNova2GloveImpl::FSGNova2GloveImpl(const FSGNova2GloveImpl& Rhs)
: FSGHapticGloveImpl(Rhs),
Pimpl(TUniquePtr<FImpl, FImplDeleter>(new FImpl{*Rhs.Pimpl}, PimplDeleter))
{
Pimpl->Owner = this;
}
FSGNova2GloveImpl::FSGNova2GloveImpl(FSGNova2GloveImpl&& Rhs) SG_NOEXCEPT = default;
FSGNova2GloveImpl::~FSGNova2GloveImpl() = default;
FSGNova2GloveImpl& FSGNova2GloveImpl::operator=(const FSGNova2GloveImpl& Rhs)
{
FSGHapticGloveImpl::operator=(Rhs);
*Pimpl = *Rhs.Pimpl;
Pimpl->Owner = this;
return *this;
}
FSGNova2GloveImpl& FSGNova2GloveImpl::operator=(FSGNova2GloveImpl&& Rhs) SG_NOEXCEPT = default;
const FSGNova2GloveImpl::FNova2GloveType& FSGNova2GloveImpl::ToNova2Glove() const
{
return *ToNova2GlovePtrChecked();
}
FSGNova2GloveImpl::FNova2GlovePtrType FSGNova2GloveImpl::ToNova2GlovePtr() const
{
return std::dynamic_pointer_cast<FNova2GloveType>(ToSGDevicePtr());
}
FSGNova2GloveImpl::FNova2GlovePtrType FSGNova2GloveImpl::ToNova2GlovePtrChecked() const
{
FNova2GlovePtrType Nova2Glove = std::dynamic_pointer_cast<FNova2GloveType>(ToSGDevicePtr());
ensureAlwaysMsgf(Nova2Glove.get(), TEXT("Invalid Nova2Glove"));
return Nova2Glove;
}
ESGDeviceType FSGNova2GloveImpl::GetDeviceType() const
{
return FSGCoreEnumCast::ToESGDeviceType(ToNova2GlovePtrChecked()->GetDeviceType());
}
FString FSGNova2GloveImpl::GetDeviceId() const
{
const FString Id{UTF8_TO_TCHAR(ToNova2GlovePtrChecked()->GetDeviceId().c_str())};
return Id;
}
FString FSGNova2GloveImpl::GetHardwareVersion() const
{
const FString Version{UTF8_TO_TCHAR(ToNova2GlovePtrChecked()->GetHardwareVersion().c_str())};
return Version;
}
int32 FSGNova2GloveImpl::GetFirmwareVersion() const
{
return ToNova2GlovePtrChecked()->GetFirmwareVersion();
}
int32 FSGNova2GloveImpl::GetSubFirmwareVersion() const
{
return ToNova2GlovePtrChecked()->GetSubFirmwareVersion();
}
bool FSGNova2GloveImpl::HasBattery() const
{
return ToNova2GlovePtrChecked()->HasBattery();
}
bool FSGNova2GloveImpl::IsCharging()
{
return ToNova2GlovePtrChecked()->IsCharging();
}
bool FSGNova2GloveImpl::GetBatteryLevel(float& OutBatteryLevel)
{
return ToNova2GlovePtrChecked()->GetBatteryLevel(OutBatteryLevel);
}
bool FSGNova2GloveImpl::IsRight() const
{
return ToNova2GlovePtrChecked()->IsRight();
}
bool FSGNova2GloveImpl::GetImuRotation(FQuat& OutImu)
{
FSGQuatImpl::FQuatType Imu;
const bool bResult = ToNova2GlovePtrChecked()->GetImuRotation(Imu);
OutImu = FSGQuatImpl{MoveTemp(Imu)}.ToFQuat();
return bResult;
}
bool FSGNova2GloveImpl::GetHandPose(const FSGBasicHandModelImpl& HandGeometry, FSGHandPoseImpl& OutHandPose) const
{
FSGHandPoseImpl::FHandPoseType HandPose;
const bool bResult = ToNova2GlovePtrChecked()->GetHandPose(HandGeometry.ToBasicHandModel(), HandPose);
OutHandPose = FSGHandPoseImpl{MoveTemp(HandPose)};
return bResult;
}
bool FSGNova2GloveImpl::GetHandAngles(TArray<TArray<FVector>>& OutHandAngles) const
{
std::vector<std::vector<FSGVect3DImpl::FVect3DType>> HandAngles;
const bool bResult = ToNova2GlovePtrChecked()->GetHandAngles(HandAngles);
OutHandAngles = FSGArrayUtils::FromSenseGloveAngles<FSGVect3DImpl::FVect3DType>(MoveTemp(HandAngles));
return bResult;
}
ESGHapticGloveCalibrationState FSGNova2GloveImpl::GetCalibrationState() const
{
return FSGCoreEnumCast::ToESGHapticGloveCalibrationState(ToNova2GlovePtrChecked()->GetCalibrationState());
}
void FSGNova2GloveImpl::EndCalibration()
{
ToNova2GlovePtrChecked()->EndCalibration();
}
void FSGNova2GloveImpl::ResetCalibration()
{
ToNova2GlovePtrChecked()->ResetCalibration();
}
void FSGNova2GloveImpl::StopHaptics()
{
ToNova2GlovePtrChecked()->StopHaptics();
}
void FSGNova2GloveImpl::StopVibrations()
{
ToNova2GlovePtrChecked()->StopVibrations();
}
bool FSGNova2GloveImpl::SendHaptics()
{
return ToNova2GlovePtrChecked()->SendHaptics();
}
bool FSGNova2GloveImpl::SupportsCustomWaveform(const ESGHapticLocation AtLocation) const
{
return ToNova2GlovePtrChecked()->SupportsCustomWaveform(FSGCoreEnumCast::ToEHapticLocation(AtLocation));
}
bool FSGNova2GloveImpl::SendCustomWaveform(FSGCustomWaveformImpl& OutWaveform, const ESGHapticLocation Location)
{
return ToNova2GlovePtrChecked()->SendCustomWaveform(OutWaveform.ToCustomWaveform(),
FSGCoreEnumCast::ToEHapticLocation(Location));
}
bool FSGNova2GloveImpl::QueueForceFeedbackLevels(const TArray<float>& Levels01)
{
std::vector<float> Levels01Vector{FSGArrayUtils::ToStdVector(Levels01)};
return ToNova2GlovePtrChecked()->QueueForceFeedbackLevels(MoveTemp(Levels01Vector));
}
bool FSGNova2GloveImpl::QueueForceFeedbackLevel(const int32 Finger, const float Level01)
{
return ToNova2GlovePtrChecked()->QueueForceFeedbackLevel(Finger, Level01);
}
bool FSGNova2GloveImpl::QueueVibroLevels(const TArray<float>& Amplitudes)
{
std::vector<float> AmplitudesVector{FSGArrayUtils::ToStdVector(Amplitudes)};
return ToNova2GlovePtrChecked()->QueueVibroLevels(MoveTemp(AmplitudesVector));
}
bool FSGNova2GloveImpl::QueueVibroLevel(const ESGHapticLocation Location, const float Level01)
{
return ToNova2GlovePtrChecked()->QueueVibroLevel(FSGCoreEnumCast::ToEHapticLocation(Location), Level01);
}
bool FSGNova2GloveImpl::QueueSqueezeLevel(const float SqueezeLevel01)
{
return ToNova2GlovePtrChecked()->QueueSqueezeLevel(SqueezeLevel01);
}
void FSGNova2GloveImpl::GetGloveLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
const ESGPositionalTrackingHardware TrackingHardware,
FVector& OutGlovePosition, FQuat& OutGloveRotation) const
{
FSGVect3DImpl::FVect3DType GlovePosition;
FSGQuatImpl::FQuatType GloveRotation;
ToNova2GlovePtrChecked()->GetGloveLocation(
FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
FSGQuatImpl{ReferenceRotation}.ToQuat(),
FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
GlovePosition, GloveRotation);
OutGlovePosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
MoveTemp(GlovePosition));
OutGloveRotation = FSGQuatImpl{MoveTemp(GloveRotation)}.ToFQuat();
}
void FSGNova2GloveImpl::GetWristLocation(const FVector& ReferencePosition, const FQuat& ReferenceRotation,
const ESGPositionalTrackingHardware TrackingHardware,
FVector& OutWristPosition, FQuat& OutWristRotation) const
{
FSGVect3DImpl::FVect3DType WristPosition;
FSGQuatImpl::FQuatType WristRotation;
ToNova2GlovePtrChecked()->GetWristLocation(
FSGUnits::CentimetersToMillimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(ReferencePosition),
FSGQuatImpl{ReferenceRotation}.ToQuat(),
FSGCoreEnumCast::ToEPositionalTrackingHardware(TrackingHardware),
WristPosition, WristRotation);
OutWristPosition = FSGUnits::MillimetersToCentimeters<FSGVect3DImpl::FVect3DType, FSGVect3DImpl>(
MoveTemp(WristPosition));
OutWristRotation = FSGQuatImpl{MoveTemp(WristRotation)}.ToFQuat();
}
FSGNovaGloveInfoImpl FSGNova2GloveImpl::GetGloveInfo() const
{
const FSGNovaGloveInfoImpl GloveInfo{ToNova2GlovePtrChecked()->GetGloveInfo()};
return GloveInfo;
}
bool FSGNova2GloveImpl::DoesIndexInfluenceOthers() const
{
return ToNova2GlovePtrChecked()->DoesIndexInfluenceOthers();
}
void FSGNova2GloveImpl::SetIndexInfluencesOthers(const bool bInfluenceOthers)
{
ToNova2GlovePtrChecked()->SetIndexInfluencesOthers(bInfluenceOthers);
}
bool FSGNova2GloveImpl::SupportsOnBoardNormalization() const
{
return ToNova2GlovePtrChecked()->SupportsOnBoardNormalization();
}
bool FSGNova2GloveImpl::GetSensorData(FSGNova2GloveSensorDataImpl& OutSensorData) const
{
FSGNova2GloveSensorDataImpl::FNova2GloveSensorDataType SensorData;
const bool bResult = ToNova2GlovePtrChecked()->GetSensorData(SensorData);
OutSensorData = FSGNova2GloveSensorDataImpl{MoveTemp(SensorData)};
return bResult;
}
bool FSGNova2GloveImpl::GetNormalizedInput(TArray<float>& OutNormalizedValues) const
{
std::vector<float> NormalizedValues;
const bool bResult = ToNova2GlovePtrChecked()->GetNormalizedInput(NormalizedValues);
OutNormalizedValues = FSGArrayUtils::FromStdVector(MoveTemp(NormalizedValues));
return bResult;
}
bool FSGNova2GloveImpl::SendRawData()
{
return ToNova2GlovePtrChecked()->SendRawData();
}
bool FSGNova2GloveImpl::SendNormalizedData()
{
return ToNova2GlovePtrChecked()->SendNormalizedData();
}
ESGNova2VibroMotor FSGNova2GloveImpl::ToNovaMotor(const ESGHapticLocation Location) const
{
return FSGCoreEnumCast::ToESGNova2VibroMotor(
ToNova2GlovePtrChecked()->ToNova2Motor(FSGCoreEnumCast::ToEHapticLocation(Location)));
}
int32 FSGNova2GloveImpl::ChannelIndex(const ESGNova2VibroMotor Motor) const
{
return ToNova2GlovePtrChecked()->ChannelIndex(FSGCoreEnumCast::ToENova2VibroMotor(Motor));
}
void FSGNova2GloveImpl::ValidateWaveform(FSGCustomWaveformImpl& OutWaveform, const ESGNova2VibroMotor Motor) const
{
return ToNova2GlovePtrChecked()->ValidateWaveform(OutWaveform.ToCustomWaveform(),
FSGCoreEnumCast::ToENova2VibroMotor(Motor));
}
bool FSGNova2GloveImpl::SendCustomWaveform_Nova2(FSGCustomWaveformImpl& OutWaveform,
const ESGNova2VibroMotor Motor, const bool bValidateWaveform)
{
return ToNova2GlovePtrChecked()->SendCustomWaveform_Nova2(
OutWaveform.ToCustomWaveform(), FSGCoreEnumCast::ToENova2VibroMotor(Motor), bValidateWaveform);
}
FString FSGNova2GloveImpl::ToString() const
{
const FString String{UTF8_TO_TCHAR(ToNova2GlovePtrChecked()->ToString().c_str())};
return String;
}
FSGNova2GloveImpl::FImpl::FImpl(FSGNova2GloveImpl* InOwner)
: Owner(InOwner)
{
}
FSGNova2GloveImpl::FImpl::~FImpl() = default;
void FSGNova2GloveImpl::FImplDeleter::operator()(const FImpl* P) const
{
delete P;
}