733 lines
24 KiB
C++
733 lines
24 KiB
C++
//======================================================================================================
|
||
// Copyright 2023, NaturalPoint Inc.
|
||
// Modified 2025 for Rokoko Glove Integration
|
||
//======================================================================================================
|
||
|
||
#include <functional>
|
||
#include <chrono>
|
||
#define WIN32_LEAN_AND_MEAN
|
||
#include <windows.h>
|
||
|
||
// Peripheral Import
|
||
#include "ExampleGloveAdapterSingleton.h"
|
||
#include "IDeviceManager.h"
|
||
#include "ExampleGloveDevice.h"
|
||
#include "LZ4Wrapper.h"
|
||
using namespace AnalogSystem;
|
||
|
||
// Rokoko Integration - 기존 시뮬레이션 관련 정의를 주석 처리
|
||
// #define SIM_DEVICE_COUNT 2
|
||
// #define SIM_DEVICE_RATE 100
|
||
|
||
|
||
OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ExampleGloveAdapterSingleton(AnalogSystem::IDeviceManager* pDeviceManager)
|
||
{
|
||
s_Instance = this;
|
||
mDeviceManager = pDeviceManager;
|
||
|
||
// 기존 시뮬레이션 코드를 주석 처리 (로코코 통신으로 대체)
|
||
// mGloveSimulator = new SimulatedPluginDevices::HardwareSimulator();
|
||
|
||
mGloveDataMutex = new std::recursive_mutex();
|
||
mDetectedDevices.clear();
|
||
mLatestGloveData.clear();
|
||
|
||
// start detection thread
|
||
mDetectionThread = std::thread(&ExampleGloveAdapterSingleton::DoDetectionThread, this);
|
||
}
|
||
|
||
|
||
OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::~ExampleGloveAdapterSingleton()
|
||
{
|
||
mDetectedDevices.clear();
|
||
mLatestGloveData.clear();
|
||
delete mGloveDataMutex;
|
||
bIsConnected = false;
|
||
bIsDetecting = false;
|
||
s_Instance = nullptr;
|
||
|
||
if (mDetectionThread.joinable())
|
||
{
|
||
mDetectionThread.join();
|
||
}
|
||
|
||
// 기존 시뮬레이션 코드를 주석 처리
|
||
// mGloveSimulator->Shutdown();
|
||
// delete mGloveSimulator;
|
||
|
||
// 로코코 연결 해제
|
||
DisconnectFromRokoko();
|
||
}
|
||
|
||
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ClientShutdown()
|
||
{
|
||
bIsConnected = false;
|
||
bIsDetecting = false;
|
||
|
||
// 기존 시뮬레이션 코드를 주석 처리
|
||
// if (mGloveSimulator != nullptr)
|
||
// {
|
||
// mGloveSimulator->Shutdown();
|
||
// }
|
||
|
||
// 로코코 연결 해제
|
||
DisconnectFromRokoko();
|
||
|
||
return false;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Rokoko Glove Host Detection Thread
|
||
//
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DoDetectionThread()
|
||
{
|
||
while (!bIsConnected && bIsDetecting)
|
||
{
|
||
// Try reconnecting
|
||
if (s_Instance->mConnectionAttemptCount < s_Instance->kMaxConnectionAttempt) {
|
||
bIsConnected = ConnectToHost();
|
||
s_Instance->mConnectionAttemptCount++;
|
||
}
|
||
else {
|
||
bIsDetecting = false;
|
||
NotifyConnectionFail();
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(20000));
|
||
}
|
||
}
|
||
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Rokoko Integration - Connection Management
|
||
//
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConnectToHost()
|
||
{
|
||
// Check if glove server address is defined in Motive's settings.
|
||
if ((mServerAddress.empty())) {
|
||
char* szServerAddress = new char[MAX_PATH];
|
||
if (mDeviceManager->GetProperty("Glove Server Address", &szServerAddress))
|
||
{
|
||
std::string str(szServerAddress);
|
||
mServerAddress = str;
|
||
}
|
||
delete[] szServerAddress;
|
||
}
|
||
|
||
/*
|
||
* [Glove SDK Placeholder]
|
||
* SDK call to connect to the host at 'mServerAddress'.
|
||
*/
|
||
bIsConnected = true;
|
||
|
||
// 로코코 UDP 통신 시작
|
||
if (ConnectToRokoko()) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Successfully connected to Rokoko Studio", MessageType_StatusInfo);
|
||
}
|
||
return true;
|
||
} else {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Failed to connect to Rokoko Studio", MessageType_StatusInfo);
|
||
}
|
||
// 로코코 연결 실패 시 연결 실패로 처리
|
||
bIsConnected = false;
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConnectToRokoko()
|
||
{
|
||
try {
|
||
// 로코코 UDP 리시버 초기화
|
||
mRokokoReceiver = std::make_unique<RokokoIntegration::RokokoUDPReceiver>();
|
||
|
||
// UDP 리시버 초기화 (포트 14043)
|
||
if (!mRokokoReceiver->Initialize(14043)) {
|
||
// 에러 메시지 출력
|
||
std::string errorMsg = "Failed to initialize Rokoko UDP receiver: " + mRokokoReceiver->GetLastError();
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// 데이터 콜백 설정
|
||
mRokokoReceiver->SetDataCallback([this](const std::vector<uint8_t>& data, const std::string& senderIP) {
|
||
this->OnRokokoDataReceived(data, senderIP);
|
||
});
|
||
|
||
// UDP 수신 시작
|
||
if (!mRokokoReceiver->StartListening()) {
|
||
std::string errorMsg = "Failed to start Rokoko UDP listening: " + mRokokoReceiver->GetLastError();
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
return false;
|
||
}
|
||
|
||
bIsRokokoConnected = true;
|
||
|
||
// 로코코 장치 동적 감지 및 생성
|
||
DetectAndCreateRokokoDevices();
|
||
|
||
// 성공 메시지 출력
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("Successfully connected to Rokoko Studio via UDP on port 14043", MessageType_StatusInfo);
|
||
}
|
||
|
||
return true;
|
||
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("Exception occurred while connecting to Rokoko", MessageType_StatusInfo);
|
||
}
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DisconnectFromRokoko()
|
||
{
|
||
if (mRokokoReceiver) {
|
||
mRokokoReceiver->StopListening();
|
||
mRokokoReceiver.reset();
|
||
}
|
||
bIsRokokoConnected = false;
|
||
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("Disconnected from Rokoko Studio", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
|
||
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::IsConnected()
|
||
{
|
||
return bIsConnected;
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetLatestData(const sGloveDeviceData& gloveFingerData)
|
||
{
|
||
s_Instance->mLatestGloveData[gloveFingerData.gloveId] = gloveFingerData;
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetDeviceManager(AnalogSystem::IDeviceManager* pDeviceManager)
|
||
{
|
||
if (s_Instance == nullptr) return;
|
||
if (pDeviceManager != nullptr)
|
||
{
|
||
s_Instance->mDeviceManager = pDeviceManager;
|
||
}
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::SetLatestDeviceInfo(const sGloveDeviceBaseInfo& deviceInfo)
|
||
{
|
||
s_Instance->mLatestDeviceInfo[deviceInfo.gloveId] = deviceInfo;
|
||
}
|
||
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::GetLatestData(const uint64_t mDeviceSerial, sGloveDeviceData& gloveFingerData)
|
||
{
|
||
if (s_Instance == nullptr || !s_Instance -> bIsConnected) return false;
|
||
|
||
bool res = false;
|
||
if (s_Instance->mGloveDataMutex->try_lock()) {
|
||
// Iterate through the glove data table and update
|
||
auto iter = s_Instance->mLatestGloveData.find(mDeviceSerial);
|
||
if (iter != s_Instance->mLatestGloveData.end())
|
||
{
|
||
gloveFingerData = iter->second;
|
||
res = true;
|
||
}
|
||
s_Instance->mGloveDataMutex->unlock();
|
||
}
|
||
return res;
|
||
}
|
||
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CreateNewGloveDevice(sGloveDeviceBaseInfo& deviceInfo)
|
||
{
|
||
uint64_t gloveId = deviceInfo.gloveId;
|
||
std::string deviceName = "RokokoGlove_" + std::to_string(deviceInfo.gloveId); // 이름을 RokokoGlove로 변경
|
||
|
||
// Create device factory using the name/id/serial/client.
|
||
std::unique_ptr<ExampleGloveDeviceFactory> pDF =
|
||
std::make_unique<ExampleGloveDeviceFactory>(deviceName, deviceInfo);
|
||
pDF->mDeviceIndex = s_Instance->mCurrentDeviceIndex;
|
||
s_Instance->mDeviceManager->AddDevice(std::move(pDF));
|
||
s_Instance->mCurrentDeviceIndex++;
|
||
|
||
// 로코코 장치 생성 성공 메시지
|
||
if (s_Instance->mDeviceManager) {
|
||
std::string successMsg = "[RokokoGlove] Created new device: " + deviceName;
|
||
s_Instance->mDeviceManager->MessageToHost(successMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::NotifyConnectionFail()
|
||
{
|
||
char* szServerAddress = new char[MAX_PATH];
|
||
char szMessage[MAX_PATH];
|
||
if (mDeviceManager->GetProperty("Glove Server Address", &szServerAddress)) {
|
||
if (!(strlen(szServerAddress) == 0))
|
||
{
|
||
sprintf_s(szMessage, "[RokokoGlove] Failed to connect to Rokoko Studio on %s", szServerAddress);
|
||
mDeviceManager->MessageToHost(szMessage, MessageType_StatusInfo);
|
||
}
|
||
else
|
||
{
|
||
sprintf_s(szMessage, "[RokokoGlove] Failed to connect to Rokoko Studio on localhost");
|
||
mDeviceManager->MessageToHost(szMessage, MessageType_StatusInfo);
|
||
}
|
||
}
|
||
delete[] szServerAddress;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Legacy Simulation Support (Commented Out)
|
||
//
|
||
/*
|
||
* Methods in this section were used for simulated hardware.
|
||
* Now replaced by Rokoko UDP communication.
|
||
*/
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::StartSimulatedHardware(int deviceCount)
|
||
{
|
||
// mGloveSimulator->StartData(); // 기존 시뮬레이션 코드를 주석 처리
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::RegisterSDKCallbacks()
|
||
{
|
||
// s_Instance->mGloveSimulator->RegisterFrameDataCallback(OnDataCallback); // 기존 시뮬레이션 코드를 주석 처리
|
||
// s_Instance->mGloveSimulator->RegisterDeviceInfoCallback(OnDeviceInfoCallback); // 기존 시뮬레이션 코드를 주석 처리
|
||
|
||
// Add simulated glove devices
|
||
// s_Instance->mGloveSimulator->AddSimulatedGlove(1, 15, 1); // 기존 시뮬레이션 코드를 주석 처리
|
||
// s_Instance->mGloveSimulator->AddSimulatedGlove(2, 15, 2); // 기존 시뮬레이션 코드를 주석 처리
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::OnDataCallback(std::vector<SimulatedPluginDevices::SimulatedGloveFrameData>& gloveAllFingerData)
|
||
{
|
||
// 기존 시뮬레이션 콜백을 주석 처리 (로코코 UDP 데이터 처리로 대체)
|
||
/*
|
||
if (s_Instance == nullptr) return;
|
||
|
||
// [Glove SDK Placeholder]
|
||
// Data update callbacks to Keep the latest glove data map up-to-date.
|
||
s_Instance->mGloveDataMutex->lock();
|
||
for (auto gloveFingerData : gloveAllFingerData) {
|
||
// Convert simulated data into glove data format.
|
||
sGloveDeviceData newGloveData = ConvertDataFormat(gloveFingerData);
|
||
s_Instance->SetLatestData(newGloveData);
|
||
}
|
||
s_Instance->mGloveDataMutex->unlock();
|
||
*/
|
||
|
||
// 로코코 UDP 데이터는 OnRokokoDataReceived에서 처리됨
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::OnDeviceInfoCallback(std::vector<SimulatedPluginDevices::SimulatedDeviceInfo>& newGloveInfo)
|
||
{
|
||
// 기존 시뮬레이션 콜백을 주석 처리 (로코코 장치 정보로 대체)
|
||
/*
|
||
if (s_Instance == nullptr) return;
|
||
|
||
// Create new gloves
|
||
for (SimulatedPluginDevices::SimulatedDeviceInfo glove : newGloveInfo)
|
||
{
|
||
s_Instance->mDeviceManager->MessageToHost(" [ExampleGloveDevice] New device(s) detected.", MessageType_StatusInfo);
|
||
sGloveDeviceBaseInfo newGloveDevice = ConvertDeviceInfoFormat(glove);
|
||
s_Instance->CreateNewGloveDevice(newGloveDevice);
|
||
}
|
||
*/
|
||
|
||
// 로코코 장치 정보는 ConnectToRokoko에서 처리됨
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Legacy Data Conversion (Commented Out)
|
||
//
|
||
/*
|
||
* Methods in this section were used for converting simulated data.
|
||
* Now replaced by Rokoko data conversion.
|
||
*/
|
||
sGloveDeviceBaseInfo OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertDeviceInfoFormat(SimulatedPluginDevices::SimulatedDeviceInfo& glove)
|
||
{
|
||
// 기존 시뮬레이션 데이터 변환을 주석 처리 (로코코 데이터로 대체)
|
||
/*
|
||
sGloveDeviceBaseInfo ret;
|
||
|
||
ret.battery = glove.mBattery;
|
||
ret.gloveId = glove.mDeviceSerial;
|
||
ret.signalStrength = glove.mSignalStrength;
|
||
ret.handSide = (glove.mHandSide == 1) ? eGloveHandSide::Left :
|
||
(glove.mHandSide == 2) ? eGloveHandSide::Right :
|
||
eGloveHandSide::Unknown;
|
||
|
||
return ret;
|
||
*/
|
||
|
||
// 로코코 장치 정보는 기본값으로 설정
|
||
sGloveDeviceBaseInfo ret;
|
||
ret.battery = 100; // 기본 배터리 100%
|
||
ret.gloveId = 1; // 기본 장갑 ID
|
||
ret.signalStrength = 100; // 기본 신호 강도 100%
|
||
ret.handSide = eGloveHandSide::Left; // 기본 왼손
|
||
|
||
return ret;
|
||
}
|
||
|
||
sGloveDeviceData OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertDataFormat(const SimulatedPluginDevices::SimulatedGloveFrameData& gloveFrameData)
|
||
{
|
||
// 기존 시뮬레이션 데이터 변환을 주석 처리 (로코코 데이터로 대체)
|
||
/*
|
||
sGloveDeviceData ret;
|
||
ret.gloveId = gloveFrameData.mDeviceSerial;
|
||
ret.timestamp = 0;
|
||
|
||
sFingerNode defaultNodes = { 0, 0, 0, 1 };
|
||
ret.nodes = std::vector<sFingerNode>(15, defaultNodes);
|
||
|
||
int node_iter = 0;
|
||
for (auto& fingerNode : ret.nodes)
|
||
{
|
||
fingerNode.node_id = node_iter;
|
||
fingerNode.quat_w = gloveFrameData.gloveFingerData[node_iter].quat_w;
|
||
fingerNode.quat_x = gloveFrameData.gloveFingerData[node_iter].quat_x;
|
||
fingerNode.quat_y = gloveFrameData.gloveFingerData[node_iter].quat_y;
|
||
fingerNode.quat_z = gloveFrameData.gloveFingerData[node_iter].quat_z;
|
||
node_iter++;
|
||
}
|
||
|
||
return ret;
|
||
*/
|
||
|
||
// 로코코 데이터는 ProcessRokokoData에서 처리됨
|
||
// 기본 빈 데이터 반환
|
||
sGloveDeviceData ret;
|
||
ret.gloveId = 1;
|
||
ret.timestamp = 0;
|
||
ret.nodes.clear();
|
||
|
||
return ret;
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Rokoko Integration - UDP Data Processing
|
||
//
|
||
/*
|
||
* Methods in this section handle Rokoko UDP data reception and processing.
|
||
*/
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::OnRokokoDataReceived(const std::vector<uint8_t>& data, const std::string& senderIP)
|
||
{
|
||
if (s_Instance == nullptr) return;
|
||
|
||
try {
|
||
// 로코코 데이터 처리
|
||
s_Instance->ProcessRokokoData(data);
|
||
|
||
// 디버그 정보 출력 (OptiTrack에서 확인 가능)
|
||
if (s_Instance->mDeviceManager) {
|
||
std::string debugMsg = "[RokokoGlove] Received " + std::to_string(data.size()) + " bytes from " + senderIP;
|
||
s_Instance->mDeviceManager->MessageToHost(debugMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
|
||
} catch (...) {
|
||
if (s_Instance->mDeviceManager) {
|
||
s_Instance->mDeviceManager->MessageToHost("[RokokoGlove] Error processing received data", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ProcessRokokoData(const std::vector<uint8_t>& data)
|
||
{
|
||
try {
|
||
// 1단계: LZ4 압축 해제 (필요한 경우)
|
||
std::vector<uint8_t> decompressedData;
|
||
if (RokokoIntegration::LZ4Wrapper::IsValidLZ4Data(data.data(), static_cast<int>(data.size()))) {
|
||
decompressedData = RokokoIntegration::LZ4Wrapper::Decompress(data.data(), static_cast<int>(data.size()));
|
||
if (decompressedData.empty()) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] LZ4 decompression failed", MessageType_StatusInfo);
|
||
}
|
||
return;
|
||
}
|
||
} else {
|
||
// LZ4 압축이 아닌 경우 원본 데이터 사용
|
||
decompressedData = data;
|
||
}
|
||
|
||
// 2단계: JSON 문자열로 변환
|
||
std::string jsonString(decompressedData.begin(), decompressedData.end());
|
||
|
||
// 3단계: JSON 파싱하여 Rokoko 데이터 구조로 변환
|
||
RokokoData::LiveFrame_v4 rokokoFrame;
|
||
if (!RokokoIntegration::RokokoDataParser::ParseLiveFrame(jsonString, rokokoFrame)) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] JSON parsing failed", MessageType_StatusInfo);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 4단계: 다중 장치 데이터 처리 (왼손, 오른손)
|
||
ProcessMultipleDeviceData(rokokoFrame);
|
||
|
||
// 5단계: 최신 프레임 업데이트
|
||
mLastRokokoFrame = rokokoFrame;
|
||
|
||
// 성공 메시지 출력
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Successfully processed Rokoko data", MessageType_StatusInfo);
|
||
}
|
||
|
||
} catch (const std::exception& e) {
|
||
if (mDeviceManager) {
|
||
std::string errorMsg = "[RokokoGlove] Exception: " + std::string(e.what());
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Unknown exception occurred", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Device Information Management
|
||
//
|
||
/*
|
||
* Methods in this section handle device information updates and management.
|
||
*/
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::UpdateDeviceInfo(uint64_t deviceId)
|
||
{
|
||
try {
|
||
// 장치 정보 업데이트 (배터리, 신호 강도 등)
|
||
auto deviceInfoIter = mLatestDeviceInfo.find(deviceId);
|
||
if (deviceInfoIter != mLatestDeviceInfo.end()) {
|
||
// 기존 장치 정보 업데이트
|
||
sGloveDeviceBaseInfo& deviceInfo = deviceInfoIter->second;
|
||
|
||
// 배터리 상태 업데이트 (실제로는 Rokoko 데이터에서 가져와야 함)
|
||
deviceInfo.battery = 100; // 기본값
|
||
|
||
// 신호 강도 업데이트 (UDP 연결 상태 기반)
|
||
if (bIsRokokoConnected) {
|
||
deviceInfo.signalStrength = 100; // 연결됨
|
||
} else {
|
||
deviceInfo.signalStrength = 0; // 연결 안됨
|
||
}
|
||
|
||
// 장치 정보 저장
|
||
SetLatestDeviceInfo(deviceInfo);
|
||
}
|
||
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Error updating device info", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////////////////////
|
||
//
|
||
// Dynamic Device Detection and Management
|
||
//
|
||
/*
|
||
* Methods in this section handle dynamic device detection and multi-device data processing.
|
||
*/
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DetectAndCreateRokokoDevices()
|
||
{
|
||
try {
|
||
// 기존 장치 정보 초기화
|
||
mDetectedDevices.clear();
|
||
mLatestGloveData.clear();
|
||
mLatestDeviceInfo.clear();
|
||
|
||
// 왼손 장치 생성
|
||
sGloveDeviceBaseInfo leftHandInfo;
|
||
leftHandInfo.gloveId = 1;
|
||
leftHandInfo.handSide = eGloveHandSide::Left;
|
||
leftHandInfo.battery = 100;
|
||
leftHandInfo.signalStrength = 100;
|
||
CreateNewGloveDevice(leftHandInfo);
|
||
SetLatestDeviceInfo(leftHandInfo);
|
||
mDetectedDevices.push_back(1);
|
||
|
||
// 오른손 장치 생성
|
||
sGloveDeviceBaseInfo rightHandInfo;
|
||
rightHandInfo.gloveId = 2;
|
||
rightHandInfo.handSide = eGloveHandSide::Right;
|
||
rightHandInfo.battery = 100;
|
||
rightHandInfo.signalStrength = 100;
|
||
CreateNewGloveDevice(rightHandInfo);
|
||
SetLatestDeviceInfo(rightHandInfo);
|
||
mDetectedDevices.push_back(2);
|
||
|
||
// 장치 감지 완료 메시지
|
||
if (mDeviceManager) {
|
||
std::string msg = "[RokokoGlove] Detected " + std::to_string(mDetectedDevices.size()) + " devices (Left & Right Hand)";
|
||
mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Error during device detection", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ProcessMultipleDeviceData(const RokokoData::LiveFrame_v4& rokokoFrame)
|
||
{
|
||
try {
|
||
// 검증: actors가 존재하는지 확인
|
||
if (rokokoFrame.scene.actors.empty()) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] No actors found in frame", MessageType_StatusInfo);
|
||
}
|
||
return;
|
||
}
|
||
|
||
const auto& actor = rokokoFrame.scene.actors[0];
|
||
|
||
// 왼손 데이터 처리
|
||
if (ProcessHandData(actor.body, 1, eGloveHandSide::Left)) {
|
||
UpdateDeviceInfo(1);
|
||
}
|
||
|
||
// 오른손 데이터 처리
|
||
if (ProcessHandData(actor.body, 2, eGloveHandSide::Right)) {
|
||
UpdateDeviceInfo(2);
|
||
}
|
||
|
||
} catch (const std::exception& e) {
|
||
if (mDeviceManager) {
|
||
std::string errorMsg = "[RokokoGlove] Exception in multi-device processing: " + std::string(e.what());
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
mDeviceManager->MessageToHost("[RokokoGlove] Unknown exception in multi-device processing", MessageType_StatusInfo);
|
||
}
|
||
}
|
||
}
|
||
|
||
bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ProcessHandData(const RokokoData::Body& body, uint64_t deviceId, eGloveHandSide handSide)
|
||
{
|
||
try {
|
||
// 손가락 데이터를 OptiTrack 형식으로 변환
|
||
sGloveDeviceData optiTrackData;
|
||
optiTrackData.gloveId = deviceId;
|
||
optiTrackData.timestamp = 0.0;
|
||
optiTrackData.nodes.resize(15); // 5개 손가락 × 3개 관절
|
||
|
||
// 손가락별 데이터 매핑
|
||
if (handSide == eGloveHandSide::Left) {
|
||
// 왼손 데이터 매핑
|
||
MapLeftHandJoints(body, optiTrackData.nodes);
|
||
} else {
|
||
// 오른손 데이터 매핑
|
||
MapRightHandJoints(body, optiTrackData.nodes);
|
||
}
|
||
|
||
// 노드 ID 설정
|
||
for (int i = 0; i < 15; i++) {
|
||
optiTrackData.nodes[i].node_id = i;
|
||
}
|
||
|
||
// 데이터 유효성 검사
|
||
if (!RokokoIntegration::RokokoDataConverter::ValidateOptiTrackData(optiTrackData)) {
|
||
if (mDeviceManager) {
|
||
std::string errorMsg = "[RokokoGlove] Data validation failed for device " + std::to_string(deviceId);
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// OptiTrack 데이터 저장
|
||
mGloveDataMutex->lock();
|
||
SetLatestData(optiTrackData);
|
||
mGloveDataMutex->unlock();
|
||
|
||
return true;
|
||
|
||
} catch (...) {
|
||
if (mDeviceManager) {
|
||
std::string errorMsg = "[RokokoGlove] Error processing hand data for device " + std::to_string(deviceId);
|
||
mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo);
|
||
}
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::MapLeftHandJoints(const RokokoData::Body& body, std::vector<sFingerNode>& nodes)
|
||
{
|
||
// 엄지손가락 (Thumb)
|
||
if (body.leftThumbMedial) MapJoint(*body.leftThumbMedial, nodes[0]); // MP
|
||
if (body.leftThumbDistal) MapJoint(*body.leftThumbDistal, nodes[1]); // PIP
|
||
if (body.leftThumbTip) MapJoint(*body.leftThumbTip, nodes[2]); // DIP
|
||
|
||
// 검지손가락 (Index)
|
||
if (body.leftIndexMedial) MapJoint(*body.leftIndexMedial, nodes[3]); // MP
|
||
if (body.leftIndexDistal) MapJoint(*body.leftIndexDistal, nodes[4]); // PIP
|
||
if (body.leftIndexTip) MapJoint(*body.leftIndexTip, nodes[5]); // DIP
|
||
|
||
// 중지손가락 (Middle)
|
||
if (body.leftMiddleMedial) MapJoint(*body.leftMiddleMedial, nodes[6]); // MP
|
||
if (body.leftMiddleDistal) MapJoint(*body.leftMiddleDistal, nodes[7]); // PIP
|
||
if (body.leftMiddleTip) MapJoint(*body.leftMiddleTip, nodes[8]); // DIP
|
||
|
||
// 약지손가락 (Ring)
|
||
if (body.leftRingMedial) MapJoint(*body.leftRingMedial, nodes[9]); // MP
|
||
if (body.leftRingDistal) MapJoint(*body.leftRingDistal, nodes[10]); // PIP
|
||
if (body.leftRingTip) MapJoint(*body.leftRingTip, nodes[11]); // DIP
|
||
|
||
// 새끼손가락 (Little)
|
||
if (body.leftLittleMedial) MapJoint(*body.leftLittleMedial, nodes[12]); // MP
|
||
if (body.leftLittleDistal) MapJoint(*body.leftLittleDistal, nodes[13]); // PIP
|
||
if (body.leftLittleTip) MapJoint(*body.leftLittleTip, nodes[14]); // DIP
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::MapRightHandJoints(const RokokoData::Body& body, std::vector<sFingerNode>& nodes)
|
||
{
|
||
// 엄지손가락 (Thumb)
|
||
if (body.rightThumbMedial) MapJoint(*body.rightThumbMedial, nodes[0]); // MP
|
||
if (body.rightThumbDistal) MapJoint(*body.rightThumbDistal, nodes[1]); // PIP
|
||
if (body.rightThumbTip) MapJoint(*body.rightThumbTip, nodes[2]); // DIP
|
||
|
||
// 검지손가락 (Index)
|
||
if (body.rightIndexMedial) MapJoint(*body.rightIndexMedial, nodes[3]); // MP
|
||
if (body.rightIndexDistal) MapJoint(*body.rightIndexDistal, nodes[4]); // PIP
|
||
if (body.rightIndexTip) MapJoint(*body.rightIndexTip, nodes[5]); // DIP
|
||
|
||
// 중지손가락 (Middle)
|
||
if (body.rightMiddleMedial) MapJoint(*body.rightMiddleMedial, nodes[6]); // MP
|
||
if (body.rightMiddleDistal) MapJoint(*body.rightMiddleDistal, nodes[7]); // PIP
|
||
if (body.rightMiddleTip) MapJoint(*body.rightMiddleTip, nodes[8]); // DIP
|
||
|
||
// 약지손가락 (Ring)
|
||
if (body.rightRingMedial) MapJoint(*body.rightRingMedial, nodes[9]); // MP
|
||
if (body.rightRingDistal) MapJoint(*body.rightRingDistal, nodes[10]); // PIP
|
||
if (body.rightRingTip) MapJoint(*body.rightRingTip, nodes[11]); // DIP
|
||
|
||
// 새끼손가락 (Little)
|
||
if (body.rightLittleMedial) MapJoint(*body.rightLittleMedial, nodes[12]); // MP
|
||
if (body.rightLittleDistal) MapJoint(*body.rightLittleDistal, nodes[13]); // PIP
|
||
if (body.rightLittleTip) MapJoint(*body.rightLittleTip, nodes[14]); // DIP
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::MapJoint(const RokokoData::ActorJointFrame& rokokoJoint, sFingerNode& optiTrackNode)
|
||
{
|
||
// 쿼터니언 변환
|
||
optiTrackNode.quat_w = rokokoJoint.rotation.w;
|
||
optiTrackNode.quat_x = rokokoJoint.rotation.x;
|
||
optiTrackNode.quat_y = rokokoJoint.rotation.y;
|
||
optiTrackNode.quat_z = rokokoJoint.rotation.z;
|
||
} |