//====================================================================================================== // Copyright 2023, NaturalPoint Inc. // Modified 2025 for Rokoko Glove Integration //====================================================================================================== #include #include #include // stringstream 사용을 위해 추가 #include // sqrt 함수 사용을 위해 추가 #include // setprecision 사용을 위해 추가 #include // std::max, std::min 사용을 위해 추가 #define WIN32_LEAN_AND_MEAN #define NOMINMAX // Windows min/max 매크로 비활성화 #include // 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(1)); // 🚀 실시간성 최우선 - 1ms로 최소화 } } /////////////////////////////////////////////////////////////////////////////// // // 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(); // 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& 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; // FIX: Always initialize result and use lock_guard for exception safety bool res = false; // FIX: Use lock_guard instead of try_lock + manual unlock std::lock_guard lock(*s_Instance->mGloveDataMutex); // 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; } return res; } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CreateNewGloveDevice(sGloveDeviceBaseInfo& deviceInfo) { uint64_t gloveId = deviceInfo.gloveId; // Actor 이름을 포함한 장치 이름 생성 std::string deviceName; if (!deviceInfo.actorName.empty()) { deviceName = deviceInfo.actorName + "_" + (deviceInfo.handSide == eGloveHandSide::Left ? "Left" : "Right") + "Hand"; } else { deviceName = "RokokoGlove_" + std::to_string(deviceInfo.gloveId); } // Create device factory using the name/id/serial/client. std::unique_ptr pDF = std::make_unique(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& 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& 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(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& data, const std::string& senderIP) { if (s_Instance == nullptr) return; // 🚀 실시간성 최우선: 예외 처리 및 로그 최소화 s_Instance->ProcessRokokoData(data); } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ProcessRokokoData(const std::vector& data) { try { // 🚀 실시간성을 위해 정적 버퍼 사용 (메모리 할당 최소화) static std::vector decompressedData; decompressedData.clear(); decompressedData.reserve(data.size() * 4); // 미리 메모리 예약 // 🚀 Unity 방식: LZ4 압축 해제 (모든 로그 제거) decompressedData = RokokoIntegration::LZ4Wrapper::Decompress(data.data(), static_cast(data.size())); if (decompressedData.empty()) { // 대안: LZ4 프레임 헤더 건너뛰고 압축 해제 시도 if (data.size() > 8) { // LZ4 프레임 헤더 건너뛰기 (일반적으로 7바이트) std::vector dataWithoutHeader(data.begin() + 7, data.end()); decompressedData = RokokoIntegration::LZ4Wrapper::Decompress(dataWithoutHeader.data(), static_cast(dataWithoutHeader.size())); if (decompressedData.empty()) { // Unity에서는 실패 시 에러 로그만 출력하고 null 반환 // 우리는 원본 데이터로 시도해봄 decompressedData = data; } } else { decompressedData = data; } } // 2단계: JSON 문자열로 변환 (유니티 방식 정확히 구현) // Unity 방식: string text = System.Text.Encoding.UTF8.GetString(uncompressed); std::string jsonString(decompressedData.begin(), decompressedData.end()); // 🚀 성능 최적화: 데이터 미리보기 로그 제거 // JSON 데이터 유효성 검사 if (jsonString.empty() || jsonString.length() < 10) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Invalid JSON data (too short)", MessageType_StatusInfo); } return; } // JSON 시작 문자 확인 if (jsonString[0] != '{') { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Invalid JSON data (does not start with '{')", MessageType_StatusInfo); } // 바이너리 데이터 분석 (첫 20바이트) std::stringstream hexStream; hexStream << "[RokokoGlove] Binary data detected: "; for (size_t i = 0; i < 20 && i < decompressedData.size(); i++) { char buf[8]; snprintf(buf, sizeof(buf), "%02X ", static_cast(decompressedData[i])); hexStream << buf; } mDeviceManager->MessageToHost(hexStream.str().c_str(), MessageType_StatusInfo); return; } // 3단계: JSON 파싱하여 Rokoko 데이터 구조로 변환 RokokoData::LiveFrame_v4 rokokoFrame; // 🚀 실시간성 최우선 - 모든 로그 제거, 즉시 처리 if (!RokokoIntegration::RokokoDataParser::ParseLiveFrame(jsonString, rokokoFrame)) { return; // 실패 시 즉시 종료 } // 🚀 Actor 기반 동적 장치 감지 (로그 제거) DetectActorsFromRokokoData(rokokoFrame); // 🚀 첫 번째 데이터 수신 시 기본 장치도 생성 (백업) if (mDetectedDevices.empty()) { DetectAndCreateRokokoDevices(); } // 🚀 다중 장치 데이터 처리 (왼손, 오른손) - 즉시 처리 ProcessMultipleDeviceData(rokokoFrame); // 🚀 최신 프레임 업데이트 - 큐 없이 즉시 업데이트 mLastRokokoFrame = rokokoFrame; } catch (...) { // 🚀 실시간성 최우선 - 예외 발생 시 즉시 종료 (로그 제거) return; } } /////////////////////////////////////////////////////////////////////////////// // // 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(); // 현재는 기본 2개 장치만 생성 (나중에 동적 감지로 변경) // TODO: 실제 Rokoko 데이터에서 Actor 정보를 기반으로 동적 감지 // 왼손 장치 생성 sGloveDeviceBaseInfo leftHandInfo; leftHandInfo.gloveId = 1; leftHandInfo.handSide = eGloveHandSide::Left; leftHandInfo.battery = 100; leftHandInfo.signalStrength = 100; leftHandInfo.actorName = "DefaultActor"; // Actor 이름 추가 CreateNewGloveDevice(leftHandInfo); SetLatestDeviceInfo(leftHandInfo); mDetectedDevices.push_back(1); // 오른손 장치 생성 sGloveDeviceBaseInfo rightHandInfo; rightHandInfo.gloveId = 2; rightHandInfo.handSide = eGloveHandSide::Right; rightHandInfo.battery = 100; rightHandInfo.signalStrength = 100; rightHandInfo.actorName = "DefaultActor"; // Actor 이름 추가 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); } } } // 새로운 함수: 동적 Actor 기반 장치 감지 void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DetectActorsFromRokokoData(const RokokoData::LiveFrame_v4& rokokoFrame) { try { // 기존 장치 정보 백업 std::vector oldDevices = mDetectedDevices; mDetectedDevices.clear(); // Actor별로 장치 생성 for (size_t actorIndex = 0; actorIndex < rokokoFrame.scene.actors.size(); actorIndex++) { const auto& actor = rokokoFrame.scene.actors[actorIndex]; std::string actorName = actor.name; // Actor 이름이 비어있으면 기본값 사용 if (actorName.empty()) { actorName = "Actor" + std::to_string(actorIndex + 1); } // 왼손 장치 생성 (Actor별로 고유 ID 생성) uint64_t leftHandId = (actorIndex * 2) + 1; sGloveDeviceBaseInfo leftHandInfo; leftHandInfo.gloveId = static_cast(leftHandId); leftHandInfo.handSide = eGloveHandSide::Left; leftHandInfo.battery = 100; leftHandInfo.signalStrength = 100; leftHandInfo.actorName = actorName; // 새 장치인지 확인하고 생성 if (std::find(oldDevices.begin(), oldDevices.end(), leftHandId) == oldDevices.end()) { CreateNewGloveDevice(leftHandInfo); if (mDeviceManager) { std::string msg = "[RokokoGlove] Created new left hand device for " + actorName + " (ID: " + std::to_string(leftHandId) + ")"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } SetLatestDeviceInfo(leftHandInfo); mDetectedDevices.push_back(leftHandId); // 오른손 장치 생성 uint64_t rightHandId = (actorIndex * 2) + 2; sGloveDeviceBaseInfo rightHandInfo; rightHandInfo.gloveId = static_cast(rightHandId); rightHandInfo.handSide = eGloveHandSide::Right; rightHandInfo.battery = 100; rightHandInfo.signalStrength = 100; rightHandInfo.actorName = actorName; // 새 장치인지 확인하고 생성 if (std::find(oldDevices.begin(), oldDevices.end(), rightHandId) == oldDevices.end()) { CreateNewGloveDevice(rightHandInfo); if (mDeviceManager) { std::string msg = "[RokokoGlove] Created new right hand device for " + actorName + " (ID: " + std::to_string(rightHandId) + ")"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } SetLatestDeviceInfo(rightHandInfo); mDetectedDevices.push_back(rightHandId); } // 🚀 성능 최적화: 장치 감지 완료 로그 제거 } catch (const std::exception& e) { if (mDeviceManager) { std::string errorMsg = "[RokokoGlove] Exception in actor detection: " + std::string(e.what()); mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Unknown exception in actor 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; } // 🚀 모든 Actor의 데이터를 처리 (다중 장갑 지원) for (size_t actorIndex = 0; actorIndex < rokokoFrame.scene.actors.size(); actorIndex++) { const auto& actor = rokokoFrame.scene.actors[actorIndex]; // Actor별 고유 ID 계산 uint64_t leftHandId = (actorIndex * 2) + 1; uint64_t rightHandId = (actorIndex * 2) + 2; // 왼손 데이터 처리 if (ProcessHandData(actor.body, leftHandId, eGloveHandSide::Left)) { UpdateDeviceInfo(leftHandId); } // 오른손 데이터 처리 if (ProcessHandData(actor.body, rightHandId, eGloveHandSide::Right)) { UpdateDeviceInfo(rightHandId); } } } 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; } // 🚀 실시간성 최우선 - 모든 디버깅 제거 // 🎯 계층적 로컬 로테이션 계산 (Rokoko 월드 데이터 → 로컬 변환) ConvertToTrueLocalRotations(optiTrackData.nodes, body, handSide); // 🚀 실시간성 최우선 - 데이터 유효성 검사 (로그 제거) if (!RokokoIntegration::RokokoDataConverter::ValidateOptiTrackData(optiTrackData)) { return false; } // OptiTrack 데이터 저장 mGloveDataMutex->lock(); SetLatestData(optiTrackData); mGloveDataMutex->unlock(); return true; } catch (...) { // 🚀 실시간성 최우선 - 예외 발생 시 즉시 종료 (로그 제거) return false; } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::MapLeftHandJoints(const RokokoData::Body& body, std::vector& 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& 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) { // Rokoko 로우 데이터를 직접 OptiTrack에 적용 optiTrackNode.quat_w = rokokoJoint.rotation.w; optiTrackNode.quat_x = rokokoJoint.rotation.x; optiTrackNode.quat_y = rokokoJoint.rotation.y; optiTrackNode.quat_z = rokokoJoint.rotation.z; } /////////////////////////////////////////////////////////////////////////////// // // 로컬 로테이션 변환 함수들 // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToLocalRotations(std::vector& nodes, eGloveHandSide handSide) { // OptiTrack 손가락 배치: [Thumb(0-2), Index(3-5), Middle(6-8), Ring(9-11), Little(12-14)] // 각 손가락의 계층 구조: Proximal/MP → Medial/PIP → Distal/DIP // 손가락별로 로컬 로테이션 계산 for (int fingerIndex = 0; fingerIndex < 5; fingerIndex++) { int baseIndex = fingerIndex * 3; // MP 관절 (루트) - 월드 로테이션 유지 // 이미 설정된 값 사용 // PIP 관절 - MP 관절 기준 로컬 로테이션 if (baseIndex + 1 < nodes.size()) { ApplyLocalRotation(nodes[baseIndex + 1], nodes[baseIndex]); } // DIP 관절 - PIP 관절 기준 로컬 로테이션 if (baseIndex + 2 < nodes.size()) { ApplyLocalRotation(nodes[baseIndex + 2], nodes[baseIndex + 1]); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ApplyLocalRotation(sFingerNode& childNode, const sFingerNode& parentNode) { // 로컬 로테이션 = parent^-1 * child sFingerNode parentInverse; InverseQuaternion(parentNode, parentInverse); sFingerNode localRotation; MultiplyQuaternions(parentInverse, childNode, localRotation); // 결과를 자식 노드에 적용 childNode.quat_w = localRotation.quat_w; childNode.quat_x = localRotation.quat_x; childNode.quat_y = localRotation.quat_y; childNode.quat_z = localRotation.quat_z; // 정규화 NormalizeQuaternion(childNode); } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::MultiplyQuaternions(const sFingerNode& q1, const sFingerNode& q2, sFingerNode& result) { // 쿼터니언 곱셈: q1 * q2 result.quat_w = q1.quat_w * q2.quat_w - q1.quat_x * q2.quat_x - q1.quat_y * q2.quat_y - q1.quat_z * q2.quat_z; result.quat_x = q1.quat_w * q2.quat_x + q1.quat_x * q2.quat_w + q1.quat_y * q2.quat_z - q1.quat_z * q2.quat_y; result.quat_y = q1.quat_w * q2.quat_y - q1.quat_x * q2.quat_z + q1.quat_y * q2.quat_w + q1.quat_z * q2.quat_x; result.quat_z = q1.quat_w * q2.quat_z + q1.quat_x * q2.quat_y - q1.quat_y * q2.quat_x + q1.quat_z * q2.quat_w; } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::InverseQuaternion(const sFingerNode& q, sFingerNode& result) { // 쿼터니언 역원: (w, -x, -y, -z) / |q|^2 float norm = q.quat_w * q.quat_w + q.quat_x * q.quat_x + q.quat_y * q.quat_y + q.quat_z * q.quat_z; if (norm > 0.0001f) { // 0으로 나누기 방지 result.quat_w = q.quat_w / norm; result.quat_x = -q.quat_x / norm; result.quat_y = -q.quat_y / norm; result.quat_z = -q.quat_z / norm; } else { // 단위 쿼터니언으로 설정 result.quat_w = 1.0f; result.quat_x = 0.0f; result.quat_y = 0.0f; result.quat_z = 0.0f; } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::NormalizeQuaternion(sFingerNode& q) { float norm = sqrt(q.quat_w * q.quat_w + q.quat_x * q.quat_x + q.quat_y * q.quat_y + q.quat_z * q.quat_z); if (norm > 0.0001f) { // 0으로 나누기 방지 q.quat_w /= norm; q.quat_x /= norm; q.quat_y /= norm; q.quat_z /= norm; } else { // 단위 쿼터니언으로 설정 q.quat_w = 1.0f; q.quat_x = 0.0f; q.quat_y = 0.0f; q.quat_z = 0.0f; } } /////////////////////////////////////////////////////////////////////////////// // // T-포즈 캘리브레이션 시스템 // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalibrateTPose(uint64_t deviceId, const std::vector& tPoseData) { try { // T-포즈 기준값 저장 mTPoseReferences[deviceId] = tPoseData; mTPoseCalibrated[deviceId] = true; if (mDeviceManager) { std::string msg = "[RokokoGlove] T-pose calibration completed for device " + std::to_string(deviceId); mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { std::string errorMsg = "[RokokoGlove] Error during T-pose calibration for device " + std::to_string(deviceId); mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ApplyTPoseOffset(std::vector& nodes, uint64_t deviceId) { if (!IsTPoseCalibrated(deviceId)) { // T-포즈 캘리브레이션이 없으면 현재 데이터를 T-포즈로 사용 CalibrateTPose(deviceId, nodes); // 첫 프레임은 중립 상태로 설정 for (auto& node : nodes) { node.quat_w = 1.0f; node.quat_x = 0.0f; node.quat_y = 0.0f; node.quat_z = 0.0f; } return; } try { const auto& tPoseRef = mTPoseReferences[deviceId]; if (tPoseRef.size() != nodes.size()) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] T-pose reference size mismatch", MessageType_StatusInfo); } return; } // 각 관절에 대해 T-포즈 기준 오프셋 계산 for (size_t i = 0; i < nodes.size(); i++) { // 오프셋 = T-pose^-1 * current sFingerNode tPoseInverse; InverseQuaternion(tPoseRef[i], tPoseInverse); sFingerNode offsetRotation; MultiplyQuaternions(tPoseInverse, nodes[i], offsetRotation); // 결과를 노드에 적용 nodes[i].quat_w = offsetRotation.quat_w; nodes[i].quat_x = offsetRotation.quat_x; nodes[i].quat_y = offsetRotation.quat_y; nodes[i].quat_z = offsetRotation.quat_z; // 정규화 NormalizeQuaternion(nodes[i]); } } catch (...) { if (mDeviceManager) { std::string errorMsg = "[RokokoGlove] Error applying T-pose offset for device " + std::to_string(deviceId); mDeviceManager->MessageToHost(errorMsg.c_str(), MessageType_StatusInfo); } } } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::IsTPoseCalibrated(uint64_t deviceId) const { auto it = mTPoseCalibrated.find(deviceId); return (it != mTPoseCalibrated.end() && it->second); } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ResetTPoseCalibration(uint64_t deviceId) { mTPoseReferences.erase(deviceId); mTPoseCalibrated[deviceId] = false; if (mDeviceManager) { std::string msg = "[RokokoGlove] T-pose calibration reset for device " + std::to_string(deviceId); mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } /////////////////////////////////////////////////////////////////////////////// // // 손목 기준 로컬 변환 시스템 // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToWristLocalRotations(std::vector& nodes, const RokokoData::Body& body, eGloveHandSide handSide) { try { // 손목 로테이션 가져오기 sFingerNode wristRotation; if (!GetWristRotation(body, handSide, wristRotation)) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Failed to get wrist rotation", MessageType_StatusInfo); } return; } // 손목 로테이션의 역원 계산 sFingerNode wristInverse; InverseQuaternion(wristRotation, wristInverse); // 모든 손가락에 대해 손목 기준 로컬 로테이션 계산 for (auto& node : nodes) { // 로컬 로테이션 = 손목^-1 * 손가락 sFingerNode localRotation; MultiplyQuaternions(wristInverse, node, localRotation); // 결과 적용 node.quat_w = localRotation.quat_w; node.quat_x = localRotation.quat_x; node.quat_y = localRotation.quat_y; node.quat_z = localRotation.quat_z; // 정규화 NormalizeQuaternion(node); } if (mDeviceManager) { std::string handStr = (handSide == eGloveHandSide::Left) ? "Left" : "Right"; std::string msg = "[RokokoGlove] Applied wrist-relative rotations for " + handStr + " hand"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Error in wrist-relative rotation conversion", MessageType_StatusInfo); } } } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::GetWristRotation(const RokokoData::Body& body, eGloveHandSide handSide, sFingerNode& wristRotation) { try { // 🎯 실제 손목 데이터 사용! (이제 body.leftHand, body.rightHand가 있음) // ✅ OPTIMIZATION: Use optional reference instead of shared_ptr const std::optional& wristJoint = (handSide == eGloveHandSide::Left) ? body.leftHand : body.rightHand; if (wristJoint.has_value()) { wristRotation.quat_w = wristJoint->rotation.w; wristRotation.quat_x = wristJoint->rotation.x; wristRotation.quat_y = wristJoint->rotation.y; wristRotation.quat_z = wristJoint->rotation.z; return true; } // 데이터가 없으면 단위 쿼터니언 사용 wristRotation.quat_w = 1.0f; wristRotation.quat_x = 0.0f; wristRotation.quat_y = 0.0f; wristRotation.quat_z = 0.0f; if (mDeviceManager) { std::string handStr = (handSide == eGloveHandSide::Left) ? "Left" : "Right"; std::string msg = "[RokokoGlove] " + handStr + " reference joint not available, using identity rotation"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } return true; } catch (...) { // 에러 시 단위 쿼터니언 wristRotation.quat_w = 1.0f; wristRotation.quat_x = 0.0f; wristRotation.quat_y = 0.0f; wristRotation.quat_z = 0.0f; return false; } } /////////////////////////////////////////////////////////////////////////////// // // 진짜 로컬 로테이션 변환 시스템 (계층적) // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToTrueLocalRotations(std::vector& nodes, const RokokoData::Body& body, eGloveHandSide handSide) { try { // 모든 손가락의 관절 데이터 가져오기 std::vector> allFingers; if (!GetFingerJointRotations(body, handSide, allFingers)) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Failed to get finger joint rotations", MessageType_StatusInfo); } return; } // 각 손가락별로 계층적 로컬 로테이션 계산 int nodeIndex = 0; for (const auto& fingerJoints : allFingers) { if (fingerJoints.size() >= 3) { // MP, PIP, DIP 최소 3개 CalculateFingerLocalRotations(fingerJoints, nodes, nodeIndex, body, handSide); nodeIndex += 3; // OptiTrack은 손가락당 3개 노드 (MP, PIP, DIP) } } // 🚀 실시간성을 위해 로그 메시지 제거 } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Error in true local rotation conversion", MessageType_StatusInfo); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateFingerLocalRotations(const std::vector& fingerJoints, std::vector& outputNodes, int startIndex, const RokokoData::Body& body, eGloveHandSide handSide) { try { if (fingerJoints.size() < 3 || startIndex + 2 >= outputNodes.size()) { return; } // 🎯 핵심: 실제 손목 로테이션을 부모로 사용! sFingerNode wristRotation; GetWristRotation(body, handSide, wristRotation); // body 파라미터 사용 sFingerNode parentRotation = wristRotation; // 첫 번째 손가락 관절의 부모는 손목 // 각 관절별로 부모 기준 로컬 로테이션 계산 for (int i = 0; i < 3 && (startIndex + i) < outputNodes.size(); i++) { if (i < fingerJoints.size() && fingerJoints[i]) { // 현재 관절의 월드 로테이션 sFingerNode currentWorld; currentWorld.quat_w = fingerJoints[i]->rotation.w; currentWorld.quat_x = fingerJoints[i]->rotation.x; currentWorld.quat_y = fingerJoints[i]->rotation.y; currentWorld.quat_z = fingerJoints[i]->rotation.z; // 부모 로테이션의 역원 sFingerNode parentInverse; InverseQuaternion(parentRotation, parentInverse); // 로컬 로테이션 = 부모^-1 × 현재 sFingerNode localRotation; MultiplyQuaternions(parentInverse, currentWorld, localRotation); // 🔧 좌표계 변환 적용 (Rokoko → OptiTrack) - 손별 대칭성 보정 ConvertRokokoToOptiTrackCoordinates(localRotation, handSide); // 🎯 손가락별 축 순서 변경 적용 (엄지와 일반 손가락 분리) sFingerNode finalRotation = localRotation; // 엄지손가락인지 확인 (startIndex == 0이면 엄지) bool isThumb = (startIndex == 0); if (isThumb) { // 🖐️ 엄지손가락 전용 축 변환 if (handSide == eGloveHandSide::Left) { // 🤚 왼손 엄지: 특별한 축 변환 outputNodes[startIndex + i].quat_w = finalRotation.quat_w; // W는 그대로 outputNodes[startIndex + i].quat_x = finalRotation.quat_y; // X ← Z outputNodes[startIndex + i].quat_y = -finalRotation.quat_z; // Y ← Y (그대로) outputNodes[startIndex + i].quat_z = -finalRotation.quat_x; // Z ← X (엄지 전용) } else { // 🖐️ 오른손 엄지: 특별한 축 변환 outputNodes[startIndex + i].quat_w = finalRotation.quat_w; // W는 그대로 outputNodes[startIndex + i].quat_x = -finalRotation.quat_y; // X ← Z (엄지 전용) outputNodes[startIndex + i].quat_y = -finalRotation.quat_z; // Y ← Y (그대로) outputNodes[startIndex + i].quat_z = finalRotation.quat_x; // Z ← -X (엄지 전용) } } else { // 🤚 일반 손가락 축 변환 if (handSide == eGloveHandSide::Left) { // 🤚 왼손 일반 손가락: x y z → z y x (기존 방식) outputNodes[startIndex + i].quat_w = finalRotation.quat_w; // W는 그대로 outputNodes[startIndex + i].quat_x = finalRotation.quat_y; // X ← Z outputNodes[startIndex + i].quat_y = -finalRotation.quat_z; // Y ← -Y outputNodes[startIndex + i].quat_z = -finalRotation.quat_x; // Z ← -X } else { // 🖐️ 오른손 일반 손가락: 대칭적 축 매핑 (미러링 보정) outputNodes[startIndex + i].quat_w = finalRotation.quat_w; // W는 그대로 outputNodes[startIndex + i].quat_x = -finalRotation.quat_y; // X ← -Z (미러링) outputNodes[startIndex + i].quat_y = -finalRotation.quat_z; // Y ← Y (그대로) outputNodes[startIndex + i].quat_z = finalRotation.quat_x; // Z ← X (미러링) } } // 정규화 NormalizeQuaternion(outputNodes[startIndex + i]); // 다음 계산을 위해 현재 관절을 부모로 설정 parentRotation = currentWorld; } } } catch (...) { // 에러 시 기본값 설정 for (int i = 0; i < 3 && (startIndex + i) < outputNodes.size(); i++) { outputNodes[startIndex + i].quat_w = 1.0f; outputNodes[startIndex + i].quat_x = 0.0f; outputNodes[startIndex + i].quat_y = 0.0f; outputNodes[startIndex + i].quat_z = 0.0f; } } } bool OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::GetFingerJointRotations(const RokokoData::Body& body, eGloveHandSide handSide, std::vector>& allFingers) { try { allFingers.clear(); // ✅ OPTIMIZATION: Use const_cast to get mutable pointer from const optional (no heap allocation) if (handSide == eGloveHandSide::Left) { // 왼손 손가락들 (5개 손가락) allFingers.resize(5); // 엄지 allFingers[0].push_back(const_cast(&*body.leftThumbProximal)); allFingers[0].push_back(const_cast(&*body.leftThumbMedial)); allFingers[0].push_back(const_cast(&*body.leftThumbDistal)); // 검지 allFingers[1].push_back(const_cast(&*body.leftIndexProximal)); allFingers[1].push_back(const_cast(&*body.leftIndexMedial)); allFingers[1].push_back(const_cast(&*body.leftIndexDistal)); // 중지 allFingers[2].push_back(const_cast(&*body.leftMiddleProximal)); allFingers[2].push_back(const_cast(&*body.leftMiddleMedial)); allFingers[2].push_back(const_cast(&*body.leftMiddleDistal)); // 약지 allFingers[3].push_back(const_cast(&*body.leftRingProximal)); allFingers[3].push_back(const_cast(&*body.leftRingMedial)); allFingers[3].push_back(const_cast(&*body.leftRingDistal)); // 새끼 allFingers[4].push_back(const_cast(&*body.leftLittleProximal)); allFingers[4].push_back(const_cast(&*body.leftLittleMedial)); allFingers[4].push_back(const_cast(&*body.leftLittleDistal)); } else { // 오른손 손가락들 (5개 손가락) allFingers.resize(5); // 엄지 allFingers[0].push_back(const_cast(&*body.rightThumbProximal)); allFingers[0].push_back(const_cast(&*body.rightThumbMedial)); allFingers[0].push_back(const_cast(&*body.rightThumbDistal)); // 검지 allFingers[1].push_back(const_cast(&*body.rightIndexProximal)); allFingers[1].push_back(const_cast(&*body.rightIndexMedial)); allFingers[1].push_back(const_cast(&*body.rightIndexDistal)); // 중지 allFingers[2].push_back(const_cast(&*body.rightMiddleProximal)); allFingers[2].push_back(const_cast(&*body.rightMiddleMedial)); allFingers[2].push_back(const_cast(&*body.rightMiddleDistal)); // 약지 allFingers[3].push_back(const_cast(&*body.rightRingProximal)); allFingers[3].push_back(const_cast(&*body.rightRingMedial)); allFingers[3].push_back(const_cast(&*body.rightRingDistal)); // 새끼 allFingers[4].push_back(const_cast(&*body.rightLittleProximal)); allFingers[4].push_back(const_cast(&*body.rightLittleMedial)); allFingers[4].push_back(const_cast(&*body.rightLittleDistal)); } return true; } catch (...) { allFingers.clear(); return false; } } /////////////////////////////////////////////////////////////////////////////// // // 좌표계 변환 및 디버깅 시스템 // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertRokokoToOptiTrackCoordinates(sFingerNode& node, eGloveHandSide handSide) { // 🎯 손별 좌표계 변환 (왼손/오른손 대칭성 보정) // // 로코코: Unity 좌표계 (왼손 좌표계, Y-up, Z-forward) // OptiTrack: 오른손 좌표계 (Y-up, Z-backward) float original_x = node.quat_x; float original_z = node.quat_z; if (handSide == eGloveHandSide::Left) { // 🤚 왼손: 미러링 변환 필요 (좌표계 차이로 인한 대칭성 보정) node.quat_x = -original_x; // X축 반전 (좌우 미러링) node.quat_z = -original_z; // Z축 반전 (앞뒤 미러링) // Y, W는 그대로 유지 } else { // 🖐️ 오른손: 직접 매핑 (OptiTrack과 좌표계 일치) // X, Z 축 반전하지 않음 node.quat_x = original_x; // X축 그대로 node.quat_z = original_z; // Z축 그대로 // Y, W는 그대로 유지 } } // 🚀 ApplyThumbOffset 함수 제거됨 - 인라인으로 처리 void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::LogRotationData(const std::string& label, const sFingerNode& rotation) { if (mDeviceManager) { std::stringstream ss; ss << "[RokokoGlove] " << label << " - W:" << std::fixed << std::setprecision(3) << rotation.quat_w << " X:" << rotation.quat_x << " Y:" << rotation.quat_y << " Z:" << rotation.quat_z; mDeviceManager->MessageToHost(ss.str().c_str(), MessageType_StatusInfo); } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::DebugCoordinateSystem(const RokokoData::Body& body, eGloveHandSide handSide) { // 🚀 실시간성 최우선 - 모든 디버깅 제거 (함수 비활성화) return; } /////////////////////////////////////////////////////////////////////////////// // // 절대 로컬 로테이션 시스템 (손목에 완전히 독립적) // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToAbsoluteLocalRotations(std::vector& nodes, const RokokoData::Body& body, eGloveHandSide handSide) { try { // 모든 손가락의 관절 데이터 가져오기 std::vector> allFingers; if (!GetFingerJointRotations(body, handSide, allFingers)) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Failed to get finger joint rotations for absolute local", MessageType_StatusInfo); } return; } // 각 손가락별로 절대 로컬 로테이션 계산 int nodeIndex = 0; for (const auto& fingerJoints : allFingers) { if (fingerJoints.size() >= 3) { // MP, PIP, DIP 최소 3개 CalculateAbsoluteLocalForFinger(fingerJoints, nodes, nodeIndex, handSide); nodeIndex += 3; // OptiTrack은 손가락당 3개 노드 (MP, PIP, DIP) } } if (mDeviceManager) { std::string handStr = (handSide == eGloveHandSide::Left) ? "Left" : "Right"; std::string msg = "[RokokoGlove] Applied absolute local rotations for " + handStr + " hand"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Error in absolute local rotation conversion", MessageType_StatusInfo); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateAbsoluteLocalForFinger(const std::vector& fingerJoints, std::vector& outputNodes, int startIndex, eGloveHandSide handSide) { try { if (fingerJoints.size() < 3 || startIndex + 2 >= outputNodes.size()) { return; } // 각 관절별로 절대 로컬 로테이션 계산 for (int i = 0; i < 3 && (startIndex + i) < outputNodes.size(); i++) { if (i < fingerJoints.size() && fingerJoints[i]) { // 현재 관절의 월드 로테이션을 가져옴 sFingerNode worldRotation; worldRotation.quat_w = fingerJoints[i]->rotation.w; worldRotation.quat_x = fingerJoints[i]->rotation.x; worldRotation.quat_y = fingerJoints[i]->rotation.y; worldRotation.quat_z = fingerJoints[i]->rotation.z; // 좌표계 변환 적용 ConvertRokokoToOptiTrackCoordinates(worldRotation, handSide); // 절대적인 로컬 로테이션 계산 sFingerNode localRotation; if (i == 0) { // 첫 번째 관절 (MP): 기본 T-포즈 기준으로 로컬 변환 // T-포즈 = (0, 0, 0, 1) 즉, 단위 쿼터니언 localRotation = worldRotation; // 첫 번째는 월드 그대로 사용 } else { // 나머지 관절들: 이전 관절 기준으로 상대 로테이션 계산 sFingerNode parentWorld; parentWorld.quat_w = fingerJoints[i-1]->rotation.w; parentWorld.quat_x = fingerJoints[i-1]->rotation.x; parentWorld.quat_y = fingerJoints[i-1]->rotation.y; parentWorld.quat_z = fingerJoints[i-1]->rotation.z; // 부모도 좌표계 변환 ConvertRokokoToOptiTrackCoordinates(parentWorld, handSide); // 상대 로테이션 = parent^-1 * child localRotation = CalculateRelativeRotation(parentWorld, worldRotation); } // 결과를 출력 노드에 저장 outputNodes[startIndex + i].quat_w = localRotation.quat_w; outputNodes[startIndex + i].quat_x = localRotation.quat_x; outputNodes[startIndex + i].quat_y = localRotation.quat_y; outputNodes[startIndex + i].quat_z = localRotation.quat_z; // 정규화 NormalizeQuaternion(outputNodes[startIndex + i]); } } } catch (...) { // 에러 시 기본값 설정 for (int i = 0; i < 3 && (startIndex + i) < outputNodes.size(); i++) { outputNodes[startIndex + i].quat_w = 1.0f; outputNodes[startIndex + i].quat_x = 0.0f; outputNodes[startIndex + i].quat_y = 0.0f; outputNodes[startIndex + i].quat_z = 0.0f; } } } sFingerNode OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateRelativeRotation(const sFingerNode& parent, const sFingerNode& child) { try { // 상대 로테이션 = parent^-1 * child sFingerNode parentInverse; InverseQuaternion(parent, parentInverse); sFingerNode result; MultiplyQuaternions(parentInverse, child, result); // 정규화 NormalizeQuaternion(result); return result; } catch (...) { // 에러 시 단위 쿼터니언 반환 sFingerNode identity; identity.quat_w = 1.0f; identity.quat_x = 0.0f; identity.quat_y = 0.0f; identity.quat_z = 0.0f; return identity; } } /////////////////////////////////////////////////////////////////////////////// // // 가상 손 구조 시스템 (정확한 계층적 로컬 로테이션) // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToUnityRokokoMethod(std::vector& nodes, const RokokoData::Body& body, eGloveHandSide handSide) { try { // Unity 방식: 원시 Rokoko 데이터에 T-포즈 오프셋 적용 std::vector> allFingers; if (!GetFingerJointRotations(body, handSide, allFingers)) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Failed to get finger joint rotations for Unity method", MessageType_StatusInfo); } return; } // 각 손가락별로 Unity 방식 적용 int nodeIndex = 0; for (int fingerIndex = 0; fingerIndex < allFingers.size(); fingerIndex++) { const auto& fingerJoints = allFingers[fingerIndex]; if (fingerJoints.size() >= 3) { // MP, PIP, DIP 최소 3개 for (int jointIndex = 0; jointIndex < 3 && (nodeIndex + jointIndex) < nodes.size(); jointIndex++) { if (jointIndex < fingerJoints.size() && fingerJoints[jointIndex]) { // Unity 방식 로테이션 적용 ApplyUnityRokokoRotation(nodes[nodeIndex + jointIndex], fingerJoints[jointIndex], fingerIndex, jointIndex, handSide); } else { // 데이터가 없으면 단위 쿼터니언 nodes[nodeIndex + jointIndex].quat_w = 1.0f; nodes[nodeIndex + jointIndex].quat_x = 0.0f; nodes[nodeIndex + jointIndex].quat_y = 0.0f; nodes[nodeIndex + jointIndex].quat_z = 0.0f; } // 노드 ID 설정 nodes[nodeIndex + jointIndex].node_id = nodeIndex + jointIndex; } nodeIndex += 3; // OptiTrack은 손가락당 3개 노드 } } if (mDeviceManager) { std::string handStr = (handSide == eGloveHandSide::Left) ? "Left" : "Right"; std::string msg = "[RokokoGlove] Applied Unity Rokoko method for " + handStr + " hand"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Error in Unity Rokoko method conversion", MessageType_StatusInfo); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::BuildVirtualHandFromRokoko(VirtualHand& virtualHand, const RokokoData::Body& body, eGloveHandSide handSide) { try { // 손목 로테이션 설정 (기준점) virtualHand.wristRotation.quat_w = 1.0f; virtualHand.wristRotation.quat_x = 0.0f; virtualHand.wristRotation.quat_y = 0.0f; virtualHand.wristRotation.quat_z = 0.0f; // 각 손가락의 월드 로테이션 설정 if (handSide == eGloveHandSide::Left) { // 왼손 엄지 if (body.leftThumbProximal) { virtualHand.thumb.mp.worldRotation.quat_w = body.leftThumbProximal->rotation.w; virtualHand.thumb.mp.worldRotation.quat_x = body.leftThumbProximal->rotation.x; virtualHand.thumb.mp.worldRotation.quat_y = body.leftThumbProximal->rotation.y; virtualHand.thumb.mp.worldRotation.quat_z = body.leftThumbProximal->rotation.z; virtualHand.thumb.mp.parent = nullptr; // 손목이 부모 } if (body.leftThumbMedial) { virtualHand.thumb.pip.worldRotation.quat_w = body.leftThumbMedial->rotation.w; virtualHand.thumb.pip.worldRotation.quat_x = body.leftThumbMedial->rotation.x; virtualHand.thumb.pip.worldRotation.quat_y = body.leftThumbMedial->rotation.y; virtualHand.thumb.pip.worldRotation.quat_z = body.leftThumbMedial->rotation.z; virtualHand.thumb.pip.parent = &virtualHand.thumb.mp; } if (body.leftThumbDistal) { virtualHand.thumb.dip.worldRotation.quat_w = body.leftThumbDistal->rotation.w; virtualHand.thumb.dip.worldRotation.quat_x = body.leftThumbDistal->rotation.x; virtualHand.thumb.dip.worldRotation.quat_y = body.leftThumbDistal->rotation.y; virtualHand.thumb.dip.worldRotation.quat_z = body.leftThumbDistal->rotation.z; virtualHand.thumb.dip.parent = &virtualHand.thumb.pip; } // 왼손 검지 if (body.leftIndexProximal) { virtualHand.index.mp.worldRotation.quat_w = body.leftIndexProximal->rotation.w; virtualHand.index.mp.worldRotation.quat_x = body.leftIndexProximal->rotation.x; virtualHand.index.mp.worldRotation.quat_y = body.leftIndexProximal->rotation.y; virtualHand.index.mp.worldRotation.quat_z = body.leftIndexProximal->rotation.z; virtualHand.index.mp.parent = nullptr; } if (body.leftIndexMedial) { virtualHand.index.pip.worldRotation.quat_w = body.leftIndexMedial->rotation.w; virtualHand.index.pip.worldRotation.quat_x = body.leftIndexMedial->rotation.x; virtualHand.index.pip.worldRotation.quat_y = body.leftIndexMedial->rotation.y; virtualHand.index.pip.worldRotation.quat_z = body.leftIndexMedial->rotation.z; virtualHand.index.pip.parent = &virtualHand.index.mp; } if (body.leftIndexDistal) { virtualHand.index.dip.worldRotation.quat_w = body.leftIndexDistal->rotation.w; virtualHand.index.dip.worldRotation.quat_x = body.leftIndexDistal->rotation.x; virtualHand.index.dip.worldRotation.quat_y = body.leftIndexDistal->rotation.y; virtualHand.index.dip.worldRotation.quat_z = body.leftIndexDistal->rotation.z; virtualHand.index.dip.parent = &virtualHand.index.pip; } // 같은 방식으로 중지, 약지, 새끼 (여기서는 간략화) // TODO: 나머지 손가락들도 동일하게 구현 } else { // 오른손 - 같은 구조, 다른 데이터 // TODO: 오른손 구현 } } catch (...) { // 에러 시 기본값 설정 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateHierarchicalLocalRotations(VirtualHand& virtualHand) { try { // 각 손가락의 계층적 로컬 로테이션 계산 // 엄지 손가락 virtualHand.thumb.mp.localRotation = CalculateLocalRotationFromParent(virtualHand.thumb.mp, virtualHand.thumb.mp); // 첫 번째는 손목 기준 virtualHand.thumb.pip.localRotation = CalculateLocalRotationFromParent(virtualHand.thumb.mp, virtualHand.thumb.pip); virtualHand.thumb.dip.localRotation = CalculateLocalRotationFromParent(virtualHand.thumb.pip, virtualHand.thumb.dip); // 검지 손가락 virtualHand.index.mp.localRotation = CalculateLocalRotationFromParent(virtualHand.index.mp, virtualHand.index.mp); virtualHand.index.pip.localRotation = CalculateLocalRotationFromParent(virtualHand.index.mp, virtualHand.index.pip); virtualHand.index.dip.localRotation = CalculateLocalRotationFromParent(virtualHand.index.pip, virtualHand.index.dip); // TODO: 나머지 손가락들 } catch (...) { // 에러 시 기본값 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ExtractLocalRotationsToNodes(const VirtualHand& virtualHand, std::vector& nodes) { try { if (nodes.size() < 15) return; // OptiTrack 노드 순서에 맞게 추출 // 엄지: 0, 1, 2 nodes[0] = virtualHand.thumb.mp.localRotation; nodes[1] = virtualHand.thumb.pip.localRotation; nodes[2] = virtualHand.thumb.dip.localRotation; // 검지: 3, 4, 5 nodes[3] = virtualHand.index.mp.localRotation; nodes[4] = virtualHand.index.pip.localRotation; nodes[5] = virtualHand.index.dip.localRotation; // TODO: 나머지 손가락들 (6~14) // 노드 ID 설정 for (int i = 0; i < 15; i++) { nodes[i].node_id = i; NormalizeQuaternion(nodes[i]); } } catch (...) { // 에러 시 기본값 } } sFingerNode OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateLocalRotationFromParent(const VirtualJoint& parent, const VirtualJoint& child) { try { if (parent.parent == nullptr) { // 첫 번째 관절은 월드 로테이션 그대로 (손목 기준) return child.worldRotation; } else { // 부모 기준 로컬 로테이션 = parent^-1 * child sFingerNode parentInverse; InverseQuaternion(parent.worldRotation, parentInverse); sFingerNode localRotation; MultiplyQuaternions(parentInverse, child.worldRotation, localRotation); return localRotation; } } catch (...) { // 에러 시 단위 쿼터니언 sFingerNode identity; identity.quat_w = 1.0f; identity.quat_x = 0.0f; identity.quat_y = 0.0f; identity.quat_z = 0.0f; return identity; } } /////////////////////////////////////////////////////////////////////////////// // // Unity 방식 Rokoko 데이터 처리 (원시 데이터 + T-포즈 오프셋) // sFingerNode OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::GetUnityTPoseOffset(int fingerIndex, int jointIndex, eGloveHandSide handSide) { // Unity의 SmartsuitTPose 데이터 (정확히 동일) sFingerNode offset; if (handSide == eGloveHandSide::Left) { // 왼손 T-포즈 오프셋 (Unity Actor.cs에서 복사) if (fingerIndex == 0) { // 엄지 if (jointIndex == 0) { // Proximal offset.quat_w = -0.092f; offset.quat_x = -0.561f; offset.quat_y = -0.701f; offset.quat_z = 0.430f; } else { // Intermediate, Distal offset.quat_w = -0.271f; offset.quat_x = -0.653f; offset.quat_y = -0.653f; offset.quat_z = 0.271f; } } else { // 검지, 중지, 약지, 새끼 offset.quat_w = -0.500f; offset.quat_x = -0.500f; offset.quat_y = -0.500f; offset.quat_z = 0.500f; } } else { // 오른손 T-포즈 오프셋 (Unity Actor.cs에서 복사) if (fingerIndex == 0) { // 엄지 if (jointIndex == 0) { // Proximal offset.quat_w = 0.092f; offset.quat_x = 0.561f; offset.quat_y = -0.701f; offset.quat_z = 0.430f; } else { // Intermediate, Distal offset.quat_w = 0.271f; offset.quat_x = 0.653f; offset.quat_y = -0.653f; offset.quat_z = 0.271f; } } else { // 검지, 중지, 약지, 새끼 offset.quat_w = 0.500f; offset.quat_x = 0.500f; offset.quat_y = -0.500f; offset.quat_z = 0.500f; } } return offset; } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ApplyUnityRokokoRotation(sFingerNode& node, const RokokoData::ActorJointFrame* jointFrame, int fingerIndex, int jointIndex, eGloveHandSide handSide) { try { // 1. Rokoko 원시 월드 로테이션 sFingerNode worldRotation; worldRotation.quat_w = jointFrame->rotation.w; worldRotation.quat_x = jointFrame->rotation.x; worldRotation.quat_y = jointFrame->rotation.y; worldRotation.quat_z = jointFrame->rotation.z; // 2. Unity T-포즈 오프셋 sFingerNode tposeOffset = GetUnityTPoseOffset(fingerIndex, jointIndex, handSide); // 3. Unity Actor.cs 공식 정확히 따라하기 // Unity: boneTransform.rotation = parentRotation * worldRotation * offset // OptiTrack: 각 손가락은 독립 센서이므로 worldRotation * offset만 적용 // 🎯 핵심: Unity는 worldRotation을 그대로 사용 (로컬 변환 안함!) // 손가락이 손목에 따라 움직이는 것은 Unity Transform 계층에서 자동 처리됨 sFingerNode finalRotation; MultiplyQuaternions(worldRotation, tposeOffset, finalRotation); // 4. 결과 적용 node.quat_w = finalRotation.quat_w; node.quat_x = finalRotation.quat_x; node.quat_y = finalRotation.quat_y; node.quat_z = finalRotation.quat_z; // 5. 정규화 NormalizeQuaternion(node); } catch (...) { // 에러 시 단위 쿼터니언 node.quat_w = 1.0f; node.quat_x = 0.0f; node.quat_y = 0.0f; node.quat_z = 0.0f; } } /////////////////////////////////////////////////////////////////////////////// // // 가상 Unity 아바타 시뮬레이션 시스템 // void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ConvertToVirtualUnityAvatar(std::vector& nodes, const RokokoData::Body& body, eGloveHandSide handSide) { try { // 1. 가상 Unity 아바타 초기화 VirtualUnityAvatar avatar; InitializeVirtualUnityAvatar(avatar); // 2. Rokoko 데이터를 가상 아바타에 적용 (월드 로테이션) ApplyRokokoDataToVirtualAvatar(avatar, body); // 3. Unity 방식으로 로컬 로테이션 계산 CalculateLocalRotationsFromWorldRotations(avatar); // 4. 로컬 로테이션을 OptiTrack 노드로 추출 ExtractLocalRotationsFromVirtualAvatar(avatar, nodes, handSide); if (mDeviceManager) { std::string handStr = (handSide == eGloveHandSide::Left) ? "Left" : "Right"; std::string msg = "[RokokoGlove] Applied Virtual Unity Avatar for " + handStr + " hand"; mDeviceManager->MessageToHost(msg.c_str(), MessageType_StatusInfo); } } catch (...) { if (mDeviceManager) { mDeviceManager->MessageToHost("[RokokoGlove] Error in Virtual Unity Avatar conversion", MessageType_StatusInfo); } } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::InitializeVirtualUnityAvatar(VirtualUnityAvatar& avatar) { try { // Root Transform 초기화 avatar.root.name = "Root"; avatar.root.parent = nullptr; avatar.root.localRotation.quat_w = 1.0f; // Identity avatar.root.localRotation.quat_x = 0.0f; avatar.root.localRotation.quat_y = 0.0f; avatar.root.localRotation.quat_z = 0.0f; avatar.root.worldRotation.quat_w = 1.0f; avatar.root.worldRotation.quat_x = 0.0f; avatar.root.worldRotation.quat_y = 0.0f; avatar.root.worldRotation.quat_z = 0.0f; // === 전체 스켈레톤 계층 구조 생성 (Unity Humanoid와 동일) === // 1. 허리 (최상위 부모) avatar.hips = new VirtualTransform(); avatar.hips->name = "Hips"; avatar.hips->parent = &avatar.root; avatar.hips->localRotation.quat_w = 1.0f; avatar.hips->localRotation.quat_x = 0.0f; avatar.hips->localRotation.quat_y = 0.0f; avatar.hips->localRotation.quat_z = 0.0f; // 2. 스파인 (허리의 자식) avatar.spine = new VirtualTransform(); avatar.spine->name = "Spine"; avatar.spine->parent = avatar.hips; avatar.spine->localRotation.quat_w = 1.0f; avatar.spine->localRotation.quat_x = 0.0f; avatar.spine->localRotation.quat_y = 0.0f; avatar.spine->localRotation.quat_z = 0.0f; // 3. 가슴 (스파인의 자식) avatar.chest = new VirtualTransform(); avatar.chest->name = "Chest"; avatar.chest->parent = avatar.spine; avatar.chest->localRotation.quat_w = 1.0f; avatar.chest->localRotation.quat_x = 0.0f; avatar.chest->localRotation.quat_y = 0.0f; avatar.chest->localRotation.quat_z = 0.0f; // 4. 어깨들 (가슴의 자식) avatar.leftShoulder = new VirtualTransform(); avatar.leftShoulder->name = "LeftShoulder"; avatar.leftShoulder->parent = avatar.chest; avatar.leftShoulder->localRotation.quat_w = 1.0f; avatar.leftShoulder->localRotation.quat_x = 0.0f; avatar.leftShoulder->localRotation.quat_y = 0.0f; avatar.leftShoulder->localRotation.quat_z = 0.0f; avatar.rightShoulder = new VirtualTransform(); avatar.rightShoulder->name = "RightShoulder"; avatar.rightShoulder->parent = avatar.chest; avatar.rightShoulder->localRotation.quat_w = 1.0f; avatar.rightShoulder->localRotation.quat_x = 0.0f; avatar.rightShoulder->localRotation.quat_y = 0.0f; avatar.rightShoulder->localRotation.quat_z = 0.0f; // 5. 팔 계층 (어깨 → 상완 → 하완 → 손) avatar.leftUpperArm = new VirtualTransform(); avatar.leftUpperArm->name = "LeftUpperArm"; avatar.leftUpperArm->parent = avatar.leftShoulder; avatar.leftUpperArm->localRotation.quat_w = 1.0f; avatar.leftUpperArm->localRotation.quat_x = 0.0f; avatar.leftUpperArm->localRotation.quat_y = 0.0f; avatar.leftUpperArm->localRotation.quat_z = 0.0f; avatar.leftLowerArm = new VirtualTransform(); avatar.leftLowerArm->name = "LeftLowerArm"; avatar.leftLowerArm->parent = avatar.leftUpperArm; avatar.leftLowerArm->localRotation.quat_w = 1.0f; avatar.leftLowerArm->localRotation.quat_x = 0.0f; avatar.leftLowerArm->localRotation.quat_y = 0.0f; avatar.leftLowerArm->localRotation.quat_z = 0.0f; avatar.leftHand = new VirtualTransform(); avatar.leftHand->name = "LeftHand"; avatar.leftHand->parent = avatar.leftLowerArm; avatar.leftHand->localRotation.quat_w = 1.0f; avatar.leftHand->localRotation.quat_x = 0.0f; avatar.leftHand->localRotation.quat_y = 0.0f; avatar.leftHand->localRotation.quat_z = 0.0f; // 오른쪽 팔도 동일 avatar.rightUpperArm = new VirtualTransform(); avatar.rightUpperArm->name = "RightUpperArm"; avatar.rightUpperArm->parent = avatar.rightShoulder; avatar.rightUpperArm->localRotation.quat_w = 1.0f; avatar.rightUpperArm->localRotation.quat_x = 0.0f; avatar.rightUpperArm->localRotation.quat_y = 0.0f; avatar.rightUpperArm->localRotation.quat_z = 0.0f; avatar.rightLowerArm = new VirtualTransform(); avatar.rightLowerArm->name = "RightLowerArm"; avatar.rightLowerArm->parent = avatar.rightUpperArm; avatar.rightLowerArm->localRotation.quat_w = 1.0f; avatar.rightLowerArm->localRotation.quat_x = 0.0f; avatar.rightLowerArm->localRotation.quat_y = 0.0f; avatar.rightLowerArm->localRotation.quat_z = 0.0f; avatar.rightHand = new VirtualTransform(); avatar.rightHand->name = "RightHand"; avatar.rightHand->parent = avatar.rightLowerArm; avatar.rightHand->localRotation.quat_w = 1.0f; avatar.rightHand->localRotation.quat_x = 0.0f; avatar.rightHand->localRotation.quat_y = 0.0f; avatar.rightHand->localRotation.quat_z = 0.0f; // 왼손 손가락 계층 구조 생성 for (int i = 0; i < 3; i++) { // 엄지 avatar.leftThumb[i] = new VirtualTransform(); avatar.leftThumb[i]->name = "LeftThumb" + std::to_string(i); avatar.leftThumb[i]->parent = (i == 0) ? avatar.leftHand : avatar.leftThumb[i-1]; avatar.leftThumb[i]->localRotation.quat_w = 1.0f; avatar.leftThumb[i]->localRotation.quat_x = 0.0f; avatar.leftThumb[i]->localRotation.quat_y = 0.0f; avatar.leftThumb[i]->localRotation.quat_z = 0.0f; // 검지 avatar.leftIndex[i] = new VirtualTransform(); avatar.leftIndex[i]->name = "LeftIndex" + std::to_string(i); avatar.leftIndex[i]->parent = (i == 0) ? avatar.leftHand : avatar.leftIndex[i-1]; avatar.leftIndex[i]->localRotation.quat_w = 1.0f; avatar.leftIndex[i]->localRotation.quat_x = 0.0f; avatar.leftIndex[i]->localRotation.quat_y = 0.0f; avatar.leftIndex[i]->localRotation.quat_z = 0.0f; // 중지 avatar.leftMiddle[i] = new VirtualTransform(); avatar.leftMiddle[i]->name = "LeftMiddle" + std::to_string(i); avatar.leftMiddle[i]->parent = (i == 0) ? avatar.leftHand : avatar.leftMiddle[i-1]; avatar.leftMiddle[i]->localRotation.quat_w = 1.0f; avatar.leftMiddle[i]->localRotation.quat_x = 0.0f; avatar.leftMiddle[i]->localRotation.quat_y = 0.0f; avatar.leftMiddle[i]->localRotation.quat_z = 0.0f; // 약지 avatar.leftRing[i] = new VirtualTransform(); avatar.leftRing[i]->name = "LeftRing" + std::to_string(i); avatar.leftRing[i]->parent = (i == 0) ? avatar.leftHand : avatar.leftRing[i-1]; avatar.leftRing[i]->localRotation.quat_w = 1.0f; avatar.leftRing[i]->localRotation.quat_x = 0.0f; avatar.leftRing[i]->localRotation.quat_y = 0.0f; avatar.leftRing[i]->localRotation.quat_z = 0.0f; // 새끼 avatar.leftLittle[i] = new VirtualTransform(); avatar.leftLittle[i]->name = "LeftLittle" + std::to_string(i); avatar.leftLittle[i]->parent = (i == 0) ? avatar.leftHand : avatar.leftLittle[i-1]; avatar.leftLittle[i]->localRotation.quat_w = 1.0f; avatar.leftLittle[i]->localRotation.quat_x = 0.0f; avatar.leftLittle[i]->localRotation.quat_y = 0.0f; avatar.leftLittle[i]->localRotation.quat_z = 0.0f; } // 오른손도 동일하게 (간략화) for (int i = 0; i < 3; i++) { avatar.rightThumb[i] = new VirtualTransform(); avatar.rightThumb[i]->name = "RightThumb" + std::to_string(i); avatar.rightThumb[i]->parent = (i == 0) ? avatar.rightHand : avatar.rightThumb[i-1]; avatar.rightThumb[i]->localRotation.quat_w = 1.0f; avatar.rightThumb[i]->localRotation.quat_x = 0.0f; avatar.rightThumb[i]->localRotation.quat_y = 0.0f; avatar.rightThumb[i]->localRotation.quat_z = 0.0f; avatar.rightIndex[i] = new VirtualTransform(); avatar.rightIndex[i]->name = "RightIndex" + std::to_string(i); avatar.rightIndex[i]->parent = (i == 0) ? avatar.rightHand : avatar.rightIndex[i-1]; avatar.rightIndex[i]->localRotation.quat_w = 1.0f; avatar.rightIndex[i]->localRotation.quat_x = 0.0f; avatar.rightIndex[i]->localRotation.quat_y = 0.0f; avatar.rightIndex[i]->localRotation.quat_z = 0.0f; // 나머지 손가락들도 필요시 추가... } } catch (...) { // 에러 시 기본값 설정 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ApplyRokokoDataToVirtualAvatar(VirtualUnityAvatar& avatar, const RokokoData::Body& body) { try { // === Rokoko 전신 + 손가락 데이터를 가상 아바타에 적용 === // Unity BodyFrame과 동일한 구조로 전신 스켈레톤 데이터 적용 // 1. 전신 스켈레톤 데이터 적용 if (body.hip) { avatar.hips->worldRotation.quat_w = body.hip->rotation.w; avatar.hips->worldRotation.quat_x = body.hip->rotation.x; avatar.hips->worldRotation.quat_y = body.hip->rotation.y; avatar.hips->worldRotation.quat_z = body.hip->rotation.z; } if (body.spine) { avatar.spine->worldRotation.quat_w = body.spine->rotation.w; avatar.spine->worldRotation.quat_x = body.spine->rotation.x; avatar.spine->worldRotation.quat_y = body.spine->rotation.y; avatar.spine->worldRotation.quat_z = body.spine->rotation.z; } if (body.chest) { avatar.chest->worldRotation.quat_w = body.chest->rotation.w; avatar.chest->worldRotation.quat_x = body.chest->rotation.x; avatar.chest->worldRotation.quat_y = body.chest->rotation.y; avatar.chest->worldRotation.quat_z = body.chest->rotation.z; } // 2. 어깨 데이터 적용 if (body.leftShoulder) { avatar.leftShoulder->worldRotation.quat_w = body.leftShoulder->rotation.w; avatar.leftShoulder->worldRotation.quat_x = body.leftShoulder->rotation.x; avatar.leftShoulder->worldRotation.quat_y = body.leftShoulder->rotation.y; avatar.leftShoulder->worldRotation.quat_z = body.leftShoulder->rotation.z; } if (body.rightShoulder) { avatar.rightShoulder->worldRotation.quat_w = body.rightShoulder->rotation.w; avatar.rightShoulder->worldRotation.quat_x = body.rightShoulder->rotation.x; avatar.rightShoulder->worldRotation.quat_y = body.rightShoulder->rotation.y; avatar.rightShoulder->worldRotation.quat_z = body.rightShoulder->rotation.z; } // 3. 팔 데이터 적용 if (body.leftUpperArm) { avatar.leftUpperArm->worldRotation.quat_w = body.leftUpperArm->rotation.w; avatar.leftUpperArm->worldRotation.quat_x = body.leftUpperArm->rotation.x; avatar.leftUpperArm->worldRotation.quat_y = body.leftUpperArm->rotation.y; avatar.leftUpperArm->worldRotation.quat_z = body.leftUpperArm->rotation.z; } if (body.leftLowerArm) { avatar.leftLowerArm->worldRotation.quat_w = body.leftLowerArm->rotation.w; avatar.leftLowerArm->worldRotation.quat_x = body.leftLowerArm->rotation.x; avatar.leftLowerArm->worldRotation.quat_y = body.leftLowerArm->rotation.y; avatar.leftLowerArm->worldRotation.quat_z = body.leftLowerArm->rotation.z; } if (body.leftHand) { avatar.leftHand->worldRotation.quat_w = body.leftHand->rotation.w; avatar.leftHand->worldRotation.quat_x = body.leftHand->rotation.x; avatar.leftHand->worldRotation.quat_y = body.leftHand->rotation.y; avatar.leftHand->worldRotation.quat_z = body.leftHand->rotation.z; } // 오른쪽 팔도 동일 if (body.rightUpperArm) { avatar.rightUpperArm->worldRotation.quat_w = body.rightUpperArm->rotation.w; avatar.rightUpperArm->worldRotation.quat_x = body.rightUpperArm->rotation.x; avatar.rightUpperArm->worldRotation.quat_y = body.rightUpperArm->rotation.y; avatar.rightUpperArm->worldRotation.quat_z = body.rightUpperArm->rotation.z; } if (body.rightLowerArm) { avatar.rightLowerArm->worldRotation.quat_w = body.rightLowerArm->rotation.w; avatar.rightLowerArm->worldRotation.quat_x = body.rightLowerArm->rotation.x; avatar.rightLowerArm->worldRotation.quat_y = body.rightLowerArm->rotation.y; avatar.rightLowerArm->worldRotation.quat_z = body.rightLowerArm->rotation.z; } if (body.rightHand) { avatar.rightHand->worldRotation.quat_w = body.rightHand->rotation.w; avatar.rightHand->worldRotation.quat_x = body.rightHand->rotation.x; avatar.rightHand->worldRotation.quat_y = body.rightHand->rotation.y; avatar.rightHand->worldRotation.quat_z = body.rightHand->rotation.z; } // 4. 손가락 데이터 적용 // 왼손 엄지 if (body.leftThumbProximal) { avatar.leftThumb[0]->worldRotation.quat_w = body.leftThumbProximal->rotation.w; avatar.leftThumb[0]->worldRotation.quat_x = body.leftThumbProximal->rotation.x; avatar.leftThumb[0]->worldRotation.quat_y = body.leftThumbProximal->rotation.y; avatar.leftThumb[0]->worldRotation.quat_z = body.leftThumbProximal->rotation.z; } if (body.leftThumbMedial) { avatar.leftThumb[1]->worldRotation.quat_w = body.leftThumbMedial->rotation.w; avatar.leftThumb[1]->worldRotation.quat_x = body.leftThumbMedial->rotation.x; avatar.leftThumb[1]->worldRotation.quat_y = body.leftThumbMedial->rotation.y; avatar.leftThumb[1]->worldRotation.quat_z = body.leftThumbMedial->rotation.z; } if (body.leftThumbDistal) { avatar.leftThumb[2]->worldRotation.quat_w = body.leftThumbDistal->rotation.w; avatar.leftThumb[2]->worldRotation.quat_x = body.leftThumbDistal->rotation.x; avatar.leftThumb[2]->worldRotation.quat_y = body.leftThumbDistal->rotation.y; avatar.leftThumb[2]->worldRotation.quat_z = body.leftThumbDistal->rotation.z; } // 왼손 검지 if (body.leftIndexProximal) { avatar.leftIndex[0]->worldRotation.quat_w = body.leftIndexProximal->rotation.w; avatar.leftIndex[0]->worldRotation.quat_x = body.leftIndexProximal->rotation.x; avatar.leftIndex[0]->worldRotation.quat_y = body.leftIndexProximal->rotation.y; avatar.leftIndex[0]->worldRotation.quat_z = body.leftIndexProximal->rotation.z; } if (body.leftIndexMedial) { avatar.leftIndex[1]->worldRotation.quat_w = body.leftIndexMedial->rotation.w; avatar.leftIndex[1]->worldRotation.quat_x = body.leftIndexMedial->rotation.x; avatar.leftIndex[1]->worldRotation.quat_y = body.leftIndexMedial->rotation.y; avatar.leftIndex[1]->worldRotation.quat_z = body.leftIndexMedial->rotation.z; } if (body.leftIndexDistal) { avatar.leftIndex[2]->worldRotation.quat_w = body.leftIndexDistal->rotation.w; avatar.leftIndex[2]->worldRotation.quat_x = body.leftIndexDistal->rotation.x; avatar.leftIndex[2]->worldRotation.quat_y = body.leftIndexDistal->rotation.y; avatar.leftIndex[2]->worldRotation.quat_z = body.leftIndexDistal->rotation.z; } // TODO: 나머지 손가락들과 오른손도 추가 } catch (...) { // 에러 시 기본값 유지 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::CalculateLocalRotationsFromWorldRotations(VirtualUnityAvatar& avatar) { try { // Unity 공식: localRotation = Quaternion.Inverse(parent.rotation) * child.rotation // === 전체 스켈레톤 계층적 로컬 로테이션 계산 === // 1. 몸통 계층 계산 if (avatar.hips && avatar.hips->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.hips->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.hips->worldRotation, avatar.hips->localRotation); } if (avatar.spine && avatar.spine->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.spine->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.spine->worldRotation, avatar.spine->localRotation); } if (avatar.chest && avatar.chest->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.chest->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.chest->worldRotation, avatar.chest->localRotation); } // 2. 어깨 계층 계산 if (avatar.leftShoulder && avatar.leftShoulder->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftShoulder->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftShoulder->worldRotation, avatar.leftShoulder->localRotation); } if (avatar.rightShoulder && avatar.rightShoulder->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.rightShoulder->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.rightShoulder->worldRotation, avatar.rightShoulder->localRotation); } // 3. 팔 계층 계산 (상완 → 하완 → 손) if (avatar.leftUpperArm && avatar.leftUpperArm->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftUpperArm->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftUpperArm->worldRotation, avatar.leftUpperArm->localRotation); } if (avatar.leftLowerArm && avatar.leftLowerArm->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftLowerArm->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftLowerArm->worldRotation, avatar.leftLowerArm->localRotation); } if (avatar.leftHand && avatar.leftHand->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftHand->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftHand->worldRotation, avatar.leftHand->localRotation); } // 오른쪽 팔도 동일 if (avatar.rightUpperArm && avatar.rightUpperArm->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.rightUpperArm->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.rightUpperArm->worldRotation, avatar.rightUpperArm->localRotation); } if (avatar.rightLowerArm && avatar.rightLowerArm->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.rightLowerArm->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.rightLowerArm->worldRotation, avatar.rightLowerArm->localRotation); } if (avatar.rightHand && avatar.rightHand->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.rightHand->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.rightHand->worldRotation, avatar.rightHand->localRotation); } // 4. 손가락 계층 계산 (이제 손을 올바른 부모로 사용) for (int i = 0; i < 3; i++) { if (avatar.leftThumb[i] && avatar.leftThumb[i]->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftThumb[i]->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftThumb[i]->worldRotation, avatar.leftThumb[i]->localRotation); } if (avatar.leftIndex[i] && avatar.leftIndex[i]->parent) { sFingerNode parentInverse; InverseQuaternion(avatar.leftIndex[i]->parent->worldRotation, parentInverse); MultiplyQuaternions(parentInverse, avatar.leftIndex[i]->worldRotation, avatar.leftIndex[i]->localRotation); } // TODO: 나머지 손가락들과 오른손도 추가 } } catch (...) { // 에러 시 기본값 유지 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ExtractLocalRotationsFromVirtualAvatar(const VirtualUnityAvatar& avatar, std::vector& nodes, eGloveHandSide handSide) { try { if (nodes.size() < 15) return; if (handSide == eGloveHandSide::Left) { // 왼손 엄지: 0, 1, 2 for (int i = 0; i < 3; i++) { if (avatar.leftThumb[i]) { nodes[i] = avatar.leftThumb[i]->localRotation; nodes[i].node_id = i; NormalizeQuaternion(nodes[i]); } } // 왼손 검지: 3, 4, 5 for (int i = 0; i < 3; i++) { if (avatar.leftIndex[i]) { nodes[3 + i] = avatar.leftIndex[i]->localRotation; nodes[3 + i].node_id = 3 + i; NormalizeQuaternion(nodes[3 + i]); } } // TODO: 나머지 손가락들 (6~14) } else { // 오른손 처리 // TODO: 오른손 구현 } } catch (...) { // 에러 시 기본값 } } void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::UpdateWorldRotationFromParent(VirtualTransform* transform) { // Unity 공식: rotation = parent.rotation * localRotation try { if (transform && transform->parent) { MultiplyQuaternions(transform->parent->worldRotation, transform->localRotation, transform->worldRotation); } } catch (...) { // 에러 시 기본값 유지 } }