2281 lines
85 KiB
C++
2281 lines
85 KiB
C++
//======================================================================================================
|
||
// Copyright 2023, NaturalPoint Inc.
|
||
// Modified 2025 for Rokoko Glove Integration
|
||
//======================================================================================================
|
||
|
||
#include <functional>
|
||
#include <chrono>
|
||
#include <sstream> // stringstream 사용을 위해 추가
|
||
#include <cmath> // sqrt 함수 사용을 위해 추가
|
||
#include <iomanip> // setprecision 사용을 위해 추가
|
||
#include <algorithm> // std::max, std::min 사용을 위해 추가
|
||
#define WIN32_LEAN_AND_MEAN
|
||
#define NOMINMAX // Windows min/max 매크로 비활성화
|
||
#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(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<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;
|
||
|
||
// 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<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;
|
||
|
||
// 🚀 실시간성 최우선: 예외 처리 및 로그 최소화
|
||
s_Instance->ProcessRokokoData(data);
|
||
}
|
||
|
||
void OptiTrackPluginDevices::ExampleDevice::ExampleGloveAdapterSingleton::ProcessRokokoData(const std::vector<uint8_t>& data)
|
||
{
|
||
try {
|
||
// 🚀 실시간성을 위해 정적 버퍼 사용 (메모리 할당 최소화)
|
||
static std::vector<uint8_t> decompressedData;
|
||
decompressedData.clear();
|
||
decompressedData.reserve(data.size() * 4); // 미리 메모리 예약
|
||
|
||
// 🚀 Unity 방식: LZ4 압축 해제 (모든 로그 제거)
|
||
decompressedData = RokokoIntegration::LZ4Wrapper::Decompress(data.data(), static_cast<int>(data.size()));
|
||
|
||
if (decompressedData.empty()) {
|
||
// 대안: LZ4 프레임 헤더 건너뛰고 압축 해제 시도
|
||
if (data.size() > 8) {
|
||
// LZ4 프레임 헤더 건너뛰기 (일반적으로 7바이트)
|
||
std::vector<uint8_t> dataWithoutHeader(data.begin() + 7, data.end());
|
||
decompressedData = RokokoIntegration::LZ4Wrapper::Decompress(dataWithoutHeader.data(), static_cast<int>(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<unsigned char>(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<uint64_t> 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<uint32_t>(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<uint32_t>(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<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)
|
||
{
|
||
// 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<sFingerNode>& 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<sFingerNode>& 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<sFingerNode>& 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<sFingerNode>& 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가 있음)
|
||
std::shared_ptr<RokokoData::ActorJointFrame> wristJoint;
|
||
|
||
if (handSide == eGloveHandSide::Left) {
|
||
// 왼손 실제 손목 데이터 사용
|
||
wristJoint = body.leftHand;
|
||
} else {
|
||
// 오른손 실제 손목 데이터 사용
|
||
wristJoint = body.rightHand;
|
||
}
|
||
|
||
if (wristJoint) {
|
||
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<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide)
|
||
{
|
||
try {
|
||
// 모든 손가락의 관절 데이터 가져오기
|
||
std::vector<std::vector<RokokoData::ActorJointFrame*>> 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<RokokoData::ActorJointFrame*>& fingerJoints, std::vector<sFingerNode>& 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_z; // X ← Z
|
||
outputNodes[startIndex + i].quat_y = -finalRotation.quat_y; // 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_z; // X ← -Z (미러링)
|
||
outputNodes[startIndex + i].quat_y = finalRotation.quat_y; // 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<std::vector<RokokoData::ActorJointFrame*>>& allFingers)
|
||
{
|
||
try {
|
||
allFingers.clear();
|
||
|
||
if (handSide == eGloveHandSide::Left) {
|
||
// 왼손 손가락들 (5개 손가락)
|
||
allFingers.resize(5);
|
||
|
||
// 엄지
|
||
allFingers[0].push_back(body.leftThumbProximal.get());
|
||
allFingers[0].push_back(body.leftThumbMedial.get());
|
||
allFingers[0].push_back(body.leftThumbDistal.get());
|
||
|
||
// 검지
|
||
allFingers[1].push_back(body.leftIndexProximal.get());
|
||
allFingers[1].push_back(body.leftIndexMedial.get());
|
||
allFingers[1].push_back(body.leftIndexDistal.get());
|
||
|
||
// 중지
|
||
allFingers[2].push_back(body.leftMiddleProximal.get());
|
||
allFingers[2].push_back(body.leftMiddleMedial.get());
|
||
allFingers[2].push_back(body.leftMiddleDistal.get());
|
||
|
||
// 약지
|
||
allFingers[3].push_back(body.leftRingProximal.get());
|
||
allFingers[3].push_back(body.leftRingMedial.get());
|
||
allFingers[3].push_back(body.leftRingDistal.get());
|
||
|
||
// 새끼
|
||
allFingers[4].push_back(body.leftLittleProximal.get());
|
||
allFingers[4].push_back(body.leftLittleMedial.get());
|
||
allFingers[4].push_back(body.leftLittleDistal.get());
|
||
|
||
} else {
|
||
// 오른손 손가락들 (5개 손가락)
|
||
allFingers.resize(5);
|
||
|
||
// 엄지
|
||
allFingers[0].push_back(body.rightThumbProximal.get());
|
||
allFingers[0].push_back(body.rightThumbMedial.get());
|
||
allFingers[0].push_back(body.rightThumbDistal.get());
|
||
|
||
// 검지
|
||
allFingers[1].push_back(body.rightIndexProximal.get());
|
||
allFingers[1].push_back(body.rightIndexMedial.get());
|
||
allFingers[1].push_back(body.rightIndexDistal.get());
|
||
|
||
// 중지
|
||
allFingers[2].push_back(body.rightMiddleProximal.get());
|
||
allFingers[2].push_back(body.rightMiddleMedial.get());
|
||
allFingers[2].push_back(body.rightMiddleDistal.get());
|
||
|
||
// 약지
|
||
allFingers[3].push_back(body.rightRingProximal.get());
|
||
allFingers[3].push_back(body.rightRingMedial.get());
|
||
allFingers[3].push_back(body.rightRingDistal.get());
|
||
|
||
// 새끼
|
||
allFingers[4].push_back(body.rightLittleProximal.get());
|
||
allFingers[4].push_back(body.rightLittleMedial.get());
|
||
allFingers[4].push_back(body.rightLittleDistal.get());
|
||
}
|
||
|
||
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<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide)
|
||
{
|
||
try {
|
||
// 모든 손가락의 관절 데이터 가져오기
|
||
std::vector<std::vector<RokokoData::ActorJointFrame*>> 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<RokokoData::ActorJointFrame*>& fingerJoints, std::vector<sFingerNode>& 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<sFingerNode>& nodes, const RokokoData::Body& body, eGloveHandSide handSide)
|
||
{
|
||
try {
|
||
// Unity 방식: 원시 Rokoko 데이터에 T-포즈 오프셋 적용
|
||
std::vector<std::vector<RokokoData::ActorJointFrame*>> 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<sFingerNode>& 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<sFingerNode>& 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<sFingerNode>& 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 (...) {
|
||
// 에러 시 기본값 유지
|
||
}
|
||
} |