348 lines
13 KiB
C++
348 lines
13 KiB
C++
////======================================================================================================
|
|
//// Copyright 2023, NaturalPoint Inc.
|
|
////======================================================================================================
|
|
/**
|
|
* ExampleGloveAdapterSingleton class is an adapter class provided to demonstrate how communication between plugin device and
|
|
* the glove SDK can be managed. A singleton instance of this class manages interaction between plugin device and the glove
|
|
* device SDK. The adapter instance also stores a keeps the latest data and device info map that stores the currently detected device serials
|
|
* and the latest frame data so that corresponding glove device can poll from it. Please note that this is provided only as an example, and
|
|
* there could be other ways to set this up.
|
|
*/
|
|
|
|
|
|
#pragma once
|
|
#include <list>
|
|
#include <mutex>
|
|
#include <unordered_set>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
#include <functional>
|
|
|
|
// OptiTrack Peripheral Device API
|
|
#include "AnalogChannelDescriptor.h"
|
|
#include "GloveDataFormat.h"
|
|
#include "HardwareSimulator.h"
|
|
|
|
// Rokoko Integration
|
|
#include "RokokoData.h"
|
|
#include "RokokoUDPReceiver.h"
|
|
#include "RokokoDataParser.h"
|
|
#include "RokokoDataConverter.h"
|
|
|
|
namespace OptiTrackPluginDevices
|
|
{
|
|
namespace ExampleDevice
|
|
{
|
|
class ExampleGloveAdapterSingleton;
|
|
|
|
static ExampleGloveAdapterSingleton* s_Instance = nullptr;
|
|
static std::unique_ptr<ExampleGloveAdapterSingleton> gGloveAdapter;
|
|
|
|
|
|
/**
|
|
* This is an example glove adapter (singleton) class provided to demonstrate how glove data can be populated.
|
|
* This adapter class is reponsible for all interaction with the device SDK, and populating the glove data.
|
|
* Glove data for multiple glove devices should get aggregated in here.
|
|
*/
|
|
class ExampleGloveAdapterSingleton
|
|
{
|
|
friend class ExampleGloveDevice;
|
|
|
|
public:
|
|
ExampleGloveAdapterSingleton(AnalogSystem::IDeviceManager* pDeviceManager);
|
|
~ExampleGloveAdapterSingleton();
|
|
|
|
// Singleton design pattern
|
|
ExampleGloveAdapterSingleton(const ExampleGloveAdapterSingleton&) = delete; // Should not be cloneable
|
|
ExampleGloveAdapterSingleton& operator=(const ExampleGloveAdapterSingleton& other) = delete; // Shounot be assignable
|
|
|
|
|
|
/**
|
|
* Should call the shutdown from the plugin SDK. In this case, shutsdown the simulator
|
|
*/
|
|
bool ClientShutdown();
|
|
|
|
/**
|
|
* Detection thread for connecting to the glove server.
|
|
*/
|
|
void DoDetectionThread();
|
|
std::thread mDetectionThread;
|
|
unsigned int mConnectionAttemptCount = 0;
|
|
const unsigned int kMaxConnectionAttempt = 10;
|
|
|
|
|
|
/**
|
|
* Connect to the glove host.
|
|
*/
|
|
bool ConnectToHost();
|
|
|
|
/**
|
|
* Connect to Rokoko Studio via UDP.
|
|
*/
|
|
bool ConnectToRokoko();
|
|
|
|
/**
|
|
* Disconnect from Rokoko Studio.
|
|
*/
|
|
void DisconnectFromRokoko();
|
|
|
|
private:
|
|
/**
|
|
* Returns whether detector is connected to Glove Host.
|
|
*/
|
|
bool IsConnected();
|
|
|
|
/**
|
|
* Saves the latest data to the map
|
|
*/
|
|
void SetLatestData(const sGloveDeviceData& gloveFingerData);
|
|
|
|
/**
|
|
* Pointer to device manager for additional operations
|
|
*/
|
|
void SetDeviceManager(AnalogSystem::IDeviceManager* pDeviceManager);
|
|
|
|
/**
|
|
* Saves the latest glove data to the map
|
|
*/
|
|
void SetLatestDeviceInfo(const sGloveDeviceBaseInfo& deviceInfo);
|
|
|
|
|
|
/**
|
|
* Gets the latest data for device with given unique serial
|
|
*/
|
|
bool GetLatestData(const std::uint64_t mDeviceSerial, sGloveDeviceData& gloveFingerData);
|
|
|
|
/**
|
|
* Creates new device by instantiating the factory and transferring it to Motive
|
|
*/
|
|
void CreateNewGloveDevice(sGloveDeviceBaseInfo& deviceInfo);
|
|
|
|
/**
|
|
* Prints error into Motive.
|
|
*/
|
|
void NotifyConnectionFail();
|
|
|
|
bool bIsConnected = false;
|
|
bool bIsDetecting = true;
|
|
int mCurrentDeviceIndex = 0;
|
|
std::string mServerAddress = "";
|
|
|
|
|
|
/**
|
|
* [Glove SDK Placeholder]
|
|
* Example data map for storing aggregated device data.
|
|
*/
|
|
uint16_t mDeviceCount = 0;
|
|
std::vector<uint64_t> mDetectedDevices;
|
|
std::unordered_map<uint64_t, sGloveDeviceData> mLatestGloveData;
|
|
std::unordered_map<uint64_t, sGloveDeviceBaseInfo> mLatestDeviceInfo;
|
|
|
|
// T-포즈 캘리브레이션 데이터 (로컬 로테이션 계산을 위해 필요)
|
|
std::unordered_map<uint64_t, std::vector<sFingerNode>> mTPoseReferences; // 장치별 T-포즈 기준값
|
|
std::unordered_map<uint64_t, bool> mTPoseCalibrated; // 장치별 캘리브레이션 상태
|
|
|
|
/**
|
|
* Rokoko integration members
|
|
*/
|
|
std::unique_ptr<RokokoIntegration::RokokoUDPReceiver> mRokokoReceiver;
|
|
bool bIsRokokoConnected = false;
|
|
RokokoData::LiveFrame_v4 mLastRokokoFrame;
|
|
|
|
/**
|
|
* [Legacy Glove SDK Placeholder - Commented Out]
|
|
* The following methods were sample callback functions for simulated hardware.
|
|
* Now replaced by Rokoko UDP communication.
|
|
*/
|
|
// bool bIsSimulating;
|
|
void StartSimulatedHardware(int deviceCount); // Legacy support
|
|
static void RegisterSDKCallbacks(); // Legacy support
|
|
static void OnDataCallback(std::vector<SimulatedPluginDevices::SimulatedGloveFrameData>& gloveFingerData); // Legacy support
|
|
static void OnDeviceInfoCallback(std::vector<SimulatedPluginDevices::SimulatedDeviceInfo>& newGloveInfo); // Legacy support
|
|
static sGloveDeviceBaseInfo ConvertDeviceInfoFormat(SimulatedPluginDevices::SimulatedDeviceInfo& glove); // Legacy support
|
|
static sGloveDeviceData ConvertDataFormat(const SimulatedPluginDevices::SimulatedGloveFrameData& glove); // Legacy support
|
|
|
|
/**
|
|
* Rokoko UDP data callback function.
|
|
*/
|
|
void OnRokokoDataReceived(const std::vector<uint8_t>& data, const std::string& senderIP);
|
|
|
|
/**
|
|
* Process received Rokoko data and convert to OptiTrack format.
|
|
*/
|
|
void ProcessRokokoData(const std::vector<uint8_t>& data);
|
|
|
|
/**
|
|
* Update device information (battery, signal strength, etc.)
|
|
*/
|
|
void UpdateDeviceInfo(uint64_t deviceId);
|
|
|
|
// Dynamic Device Detection and Management
|
|
void DetectAndCreateRokokoDevices();
|
|
void DetectActorsFromRokokoData(const RokokoData::LiveFrame_v4& rokokoFrame);
|
|
void ProcessMultipleDeviceData(const RokokoData::LiveFrame_v4& rokokoFrame);
|
|
|
|
/**
|
|
* Process hand data for a specific device
|
|
*/
|
|
bool ProcessHandData(const RokokoData::Body& body, uint64_t deviceId, eGloveHandSide handSide);
|
|
|
|
/**
|
|
* Map left hand joints from Rokoko data
|
|
*/
|
|
void MapLeftHandJoints(const RokokoData::Body& body, std::vector<sFingerNode>& nodes);
|
|
|
|
/**
|
|
* Map right hand joints from Rokoko data
|
|
*/
|
|
void MapRightHandJoints(const RokokoData::Body& body, std::vector<sFingerNode>& nodes);
|
|
|
|
/**
|
|
* Map individual joint data
|
|
*/
|
|
void MapJoint(const RokokoData::ActorJointFrame& rokokoJoint, sFingerNode& optiTrackNode);
|
|
|
|
// 로컬 로테이션 변환 함수들
|
|
void ConvertToLocalRotations(std::vector<sFingerNode>& nodes, eGloveHandSide handSide);
|
|
void ApplyLocalRotation(sFingerNode& childNode, const sFingerNode& parentNode);
|
|
|
|
// 쿼터니언 연산 헬퍼 함수들
|
|
void MultiplyQuaternions(const sFingerNode& q1, const sFingerNode& q2, sFingerNode& result);
|
|
void InverseQuaternion(const sFingerNode& q, sFingerNode& result);
|
|
void NormalizeQuaternion(sFingerNode& q);
|
|
|
|
// T-포즈 캘리브레이션 시스템
|
|
void CalibrateTPose(uint64_t deviceId, const std::vector<sFingerNode>& tPoseData);
|
|
void ApplyTPoseOffset(std::vector<sFingerNode>& nodes, uint64_t deviceId);
|
|
bool IsTPoseCalibrated(uint64_t deviceId) const;
|
|
void ResetTPoseCalibration(uint64_t deviceId);
|
|
|
|
// 손목 기준 로컬 변환 시스템 (기존)
|
|
void ConvertToWristLocalRotations(std::vector<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
bool GetWristRotation(const RokokoData::Body& body, eGloveHandSide handSide, sFingerNode& wristRotation);
|
|
|
|
// 진짜 로컬 로테이션 변환 시스템 (계층적)
|
|
void ConvertToTrueLocalRotations(std::vector<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
void CalculateFingerLocalRotations(const std::vector<RokokoData::ActorJointFrame*>& fingerJoints, std::vector<sFingerNode>& outputNodes, int startIndex, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
bool GetFingerJointRotations(const RokokoData::Body& body, eGloveHandSide handSide, std::vector<std::vector<RokokoData::ActorJointFrame*>>& allFingers);
|
|
|
|
// 좌표계 변환 및 디버깅
|
|
void ConvertRokokoToOptiTrackCoordinates(sFingerNode& node, eGloveHandSide handSide);
|
|
void LogRotationData(const std::string& label, const sFingerNode& rotation);
|
|
void DebugCoordinateSystem(const RokokoData::Body& body, eGloveHandSide handSide);
|
|
|
|
// 절대 로컬 로테이션 시스템 (손목에 완전히 독립적)
|
|
void ConvertToAbsoluteLocalRotations(std::vector<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
void CalculateAbsoluteLocalForFinger(const std::vector<RokokoData::ActorJointFrame*>& fingerJoints, std::vector<sFingerNode>& outputNodes, int startIndex, eGloveHandSide handSide);
|
|
sFingerNode CalculateRelativeRotation(const sFingerNode& parent, const sFingerNode& child);
|
|
|
|
// Unity 방식 Rokoko 데이터 처리 (원시 데이터 + T-포즈 오프셋)
|
|
void ConvertToUnityRokokoMethod(std::vector<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
sFingerNode GetUnityTPoseOffset(int fingerIndex, int jointIndex, eGloveHandSide handSide);
|
|
void ApplyUnityRokokoRotation(sFingerNode& node, const RokokoData::ActorJointFrame* jointFrame, int fingerIndex, int jointIndex, eGloveHandSide handSide);
|
|
|
|
// 가상 Unity 아바타 시뮬레이션 시스템
|
|
void ConvertToVirtualUnityAvatar(std::vector<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
|
|
// Unity Transform 시뮬레이션 구조
|
|
struct VirtualTransform {
|
|
sFingerNode localRotation; // Unity localRotation
|
|
sFingerNode worldRotation; // Unity rotation (world)
|
|
VirtualTransform* parent; // Unity parent transform
|
|
std::vector<VirtualTransform*> children; // Unity children
|
|
std::string name; // 디버깅용
|
|
};
|
|
|
|
struct VirtualUnityAvatar {
|
|
// 전체 스켈레톤 구조 (Unity Humanoid와 동일한 계층)
|
|
VirtualTransform root;
|
|
|
|
// 몸통 구조
|
|
VirtualTransform* hips; // 허리 (Root)
|
|
VirtualTransform* spine; // 스파인
|
|
VirtualTransform* chest; // 가슴
|
|
|
|
// 어깨 구조
|
|
VirtualTransform* leftShoulder; // 왼쪽 어깨
|
|
VirtualTransform* rightShoulder; // 오른쪽 어깨
|
|
|
|
// 팔 구조
|
|
VirtualTransform* leftUpperArm; // 왼쪽 상완
|
|
VirtualTransform* leftLowerArm; // 왼쪽 하완
|
|
VirtualTransform* leftHand; // 왼쪽 손
|
|
|
|
VirtualTransform* rightUpperArm; // 오른쪽 상완
|
|
VirtualTransform* rightLowerArm; // 오른쪽 하완
|
|
VirtualTransform* rightHand; // 오른쪽 손
|
|
|
|
// 손가락 구조 (Unity 계층과 동일)
|
|
VirtualTransform* leftThumb[3]; // Proximal, Intermediate, Distal
|
|
VirtualTransform* leftIndex[3];
|
|
VirtualTransform* leftMiddle[3];
|
|
VirtualTransform* leftRing[3];
|
|
VirtualTransform* leftLittle[3];
|
|
|
|
VirtualTransform* rightThumb[3];
|
|
VirtualTransform* rightIndex[3];
|
|
VirtualTransform* rightMiddle[3];
|
|
VirtualTransform* rightRing[3];
|
|
VirtualTransform* rightLittle[3];
|
|
};
|
|
|
|
// 가상 Unity 아바타 헬퍼 함수들
|
|
void InitializeVirtualUnityAvatar(VirtualUnityAvatar& avatar);
|
|
void ApplyRokokoDataToVirtualAvatar(VirtualUnityAvatar& avatar, const RokokoData::Body& body);
|
|
void CalculateLocalRotationsFromWorldRotations(VirtualUnityAvatar& avatar);
|
|
void ExtractLocalRotationsFromVirtualAvatar(const VirtualUnityAvatar& avatar, std::vector<sFingerNode>& nodes, eGloveHandSide handSide);
|
|
void UpdateWorldRotationFromParent(VirtualTransform* transform);
|
|
|
|
// 가상 손 노드 구조
|
|
struct VirtualJoint {
|
|
sFingerNode worldRotation;
|
|
sFingerNode localRotation;
|
|
VirtualJoint* parent;
|
|
};
|
|
|
|
struct VirtualFinger {
|
|
VirtualJoint mp; // Metacarpophalangeal
|
|
VirtualJoint pip; // Proximal Interphalangeal
|
|
VirtualJoint dip; // Distal Interphalangeal
|
|
};
|
|
|
|
struct VirtualHand {
|
|
sFingerNode wristRotation;
|
|
VirtualFinger thumb;
|
|
VirtualFinger index;
|
|
VirtualFinger middle;
|
|
VirtualFinger ring;
|
|
VirtualFinger little;
|
|
};
|
|
|
|
// 가상 손 구조 헬퍼 함수들
|
|
void BuildVirtualHandFromRokoko(VirtualHand& virtualHand, const RokokoData::Body& body, eGloveHandSide handSide);
|
|
void CalculateHierarchicalLocalRotations(VirtualHand& virtualHand);
|
|
void ExtractLocalRotationsToNodes(const VirtualHand& virtualHand, std::vector<sFingerNode>& nodes);
|
|
sFingerNode CalculateLocalRotationFromParent(const VirtualJoint& parent, const VirtualJoint& child);
|
|
|
|
protected:
|
|
|
|
/**
|
|
* [Legacy Glove SDK Simulator - Commented Out]
|
|
* Instance of simulator (now replaced by Rokoko integration)
|
|
*/
|
|
// SimulatedPluginDevices::HardwareSimulator* mGloveSimulator;
|
|
|
|
/**
|
|
* [Glove SDK Placeholder]
|
|
* Example glove data mutex
|
|
*/
|
|
std::recursive_mutex* mGloveDataMutex;
|
|
|
|
/**
|
|
* Pointer to device manager in Motive for reporting error messages.
|
|
*/
|
|
AnalogSystem::IDeviceManager* mDeviceManager;
|
|
};
|
|
}
|
|
|
|
} |