334 lines
13 KiB
C++
334 lines
13 KiB
C++
//======================================================================================================
|
|
// Copyright 2025, Rokoko Glove OptiTrack Integration
|
|
//======================================================================================================
|
|
|
|
#include "RokokoDataParser.h"
|
|
#include "RokokoData.h"
|
|
#include <sstream>
|
|
#include <iostream>
|
|
#include <cstring>
|
|
#include <cmath>
|
|
|
|
namespace RokokoIntegration
|
|
{
|
|
bool RokokoDataParser::ParseLiveFrame(const std::string& jsonString, RokokoData::LiveFrame_v4& frame)
|
|
{
|
|
try {
|
|
// nlohmann/json을 사용하여 JSON 파싱
|
|
nlohmann::json j = nlohmann::json::parse(jsonString);
|
|
|
|
// Unity JsonLiveSerializerV3.cs와 동일한 구조로 파싱
|
|
|
|
// Scene 데이터 파싱
|
|
if (j.contains("scene")) {
|
|
auto& scene = j["scene"];
|
|
|
|
// 타임스탬프
|
|
if (scene.contains("timestamp")) {
|
|
frame.timestamp = scene["timestamp"];
|
|
}
|
|
|
|
// Actors 파싱
|
|
if (scene.contains("actors") && scene["actors"].is_array()) {
|
|
auto& actors = scene["actors"];
|
|
for (const auto& actorJson : actors) {
|
|
RokokoData::ActorData actor;
|
|
|
|
// Actor 기본 정보
|
|
if (actorJson.contains("name")) {
|
|
actor.name = actorJson["name"];
|
|
}
|
|
|
|
// Body 데이터 파싱
|
|
if (actorJson.contains("body")) {
|
|
ParseBodyFrame(actorJson["body"], actor.body);
|
|
}
|
|
|
|
frame.scene.actors.push_back(actor);
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
|
|
} catch (const nlohmann::json::exception& e) {
|
|
// JSON 파싱 에러 처리
|
|
std::cerr << "JSON parsing error: " << e.what() << std::endl;
|
|
return false;
|
|
} catch (...) {
|
|
// 기타 예외 처리
|
|
std::cerr << "Unknown error during JSON parsing" << std::endl;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void RokokoDataParser::ParseBodyFrame(const nlohmann::json& bodyJson, RokokoData::Body& body)
|
|
{
|
|
// Unity JsonLiveSerializerV3.cs의 BodyFrame 구조와 동일하게 파싱
|
|
|
|
// === 전신 스켈레톤 데이터 파싱 (Unity BodyFrame과 동일) ===
|
|
|
|
// 몸통 파싱
|
|
ParseFingerJoint(bodyJson, "hip", body.hip);
|
|
ParseFingerJoint(bodyJson, "spine", body.spine);
|
|
ParseFingerJoint(bodyJson, "chest", body.chest);
|
|
ParseFingerJoint(bodyJson, "neck", body.neck);
|
|
ParseFingerJoint(bodyJson, "head", body.head);
|
|
|
|
// 왼쪽 팔 파싱
|
|
ParseFingerJoint(bodyJson, "leftShoulder", body.leftShoulder);
|
|
ParseFingerJoint(bodyJson, "leftUpperArm", body.leftUpperArm);
|
|
ParseFingerJoint(bodyJson, "leftLowerArm", body.leftLowerArm);
|
|
ParseFingerJoint(bodyJson, "leftHand", body.leftHand);
|
|
|
|
// 오른쪽 팔 파싱
|
|
ParseFingerJoint(bodyJson, "rightShoulder", body.rightShoulder);
|
|
ParseFingerJoint(bodyJson, "rightUpperArm", body.rightUpperArm);
|
|
ParseFingerJoint(bodyJson, "rightLowerArm", body.rightLowerArm);
|
|
ParseFingerJoint(bodyJson, "rightHand", body.rightHand);
|
|
|
|
// 다리 파싱 (필요시)
|
|
ParseFingerJoint(bodyJson, "leftUpLeg", body.leftUpLeg);
|
|
ParseFingerJoint(bodyJson, "leftLeg", body.leftLeg);
|
|
ParseFingerJoint(bodyJson, "leftFoot", body.leftFoot);
|
|
ParseFingerJoint(bodyJson, "leftToe", body.leftToe);
|
|
ParseFingerJoint(bodyJson, "leftToeEnd", body.leftToeEnd);
|
|
|
|
ParseFingerJoint(bodyJson, "rightUpLeg", body.rightUpLeg);
|
|
ParseFingerJoint(bodyJson, "rightLeg", body.rightLeg);
|
|
ParseFingerJoint(bodyJson, "rightFoot", body.rightFoot);
|
|
ParseFingerJoint(bodyJson, "rightToe", body.rightToe);
|
|
ParseFingerJoint(bodyJson, "rightToeEnd", body.rightToeEnd);
|
|
|
|
// === 손가락 데이터 파싱 ===
|
|
|
|
// 왼손 손가락 데이터 파싱
|
|
ParseFingerJoint(bodyJson, "leftThumbProximal", body.leftThumbProximal);
|
|
ParseFingerJoint(bodyJson, "leftThumbMedial", body.leftThumbMedial);
|
|
ParseFingerJoint(bodyJson, "leftThumbDistal", body.leftThumbDistal);
|
|
ParseFingerJoint(bodyJson, "leftThumbTip", body.leftThumbTip);
|
|
|
|
ParseFingerJoint(bodyJson, "leftIndexProximal", body.leftIndexProximal);
|
|
ParseFingerJoint(bodyJson, "leftIndexMedial", body.leftIndexMedial);
|
|
ParseFingerJoint(bodyJson, "leftIndexDistal", body.leftIndexDistal);
|
|
ParseFingerJoint(bodyJson, "leftIndexTip", body.leftIndexTip);
|
|
|
|
ParseFingerJoint(bodyJson, "leftMiddleProximal", body.leftMiddleProximal);
|
|
ParseFingerJoint(bodyJson, "leftMiddleMedial", body.leftMiddleMedial);
|
|
ParseFingerJoint(bodyJson, "leftMiddleDistal", body.leftMiddleDistal);
|
|
ParseFingerJoint(bodyJson, "leftMiddleTip", body.leftMiddleTip);
|
|
|
|
ParseFingerJoint(bodyJson, "leftRingProximal", body.leftRingProximal);
|
|
ParseFingerJoint(bodyJson, "leftRingMedial", body.leftRingMedial);
|
|
ParseFingerJoint(bodyJson, "leftRingDistal", body.leftRingDistal);
|
|
ParseFingerJoint(bodyJson, "leftRingTip", body.leftRingTip);
|
|
|
|
ParseFingerJoint(bodyJson, "leftLittleProximal", body.leftLittleProximal);
|
|
ParseFingerJoint(bodyJson, "leftLittleMedial", body.leftLittleMedial);
|
|
ParseFingerJoint(bodyJson, "leftLittleDistal", body.leftLittleDistal);
|
|
ParseFingerJoint(bodyJson, "leftLittleTip", body.leftLittleTip);
|
|
|
|
// 오른손 손가락 데이터 파싱
|
|
ParseFingerJoint(bodyJson, "rightThumbProximal", body.rightThumbProximal);
|
|
ParseFingerJoint(bodyJson, "rightThumbMedial", body.rightThumbMedial);
|
|
ParseFingerJoint(bodyJson, "rightThumbDistal", body.rightThumbDistal);
|
|
ParseFingerJoint(bodyJson, "rightThumbTip", body.rightThumbTip);
|
|
|
|
ParseFingerJoint(bodyJson, "rightIndexProximal", body.rightIndexProximal);
|
|
ParseFingerJoint(bodyJson, "rightIndexMedial", body.rightIndexMedial);
|
|
ParseFingerJoint(bodyJson, "rightIndexDistal", body.rightIndexDistal);
|
|
ParseFingerJoint(bodyJson, "rightIndexTip", body.rightIndexTip);
|
|
|
|
ParseFingerJoint(bodyJson, "rightMiddleProximal", body.rightMiddleProximal);
|
|
ParseFingerJoint(bodyJson, "rightMiddleMedial", body.rightMiddleMedial);
|
|
ParseFingerJoint(bodyJson, "rightMiddleDistal", body.rightMiddleDistal);
|
|
ParseFingerJoint(bodyJson, "rightMiddleTip", body.rightMiddleTip);
|
|
|
|
ParseFingerJoint(bodyJson, "rightRingProximal", body.rightRingProximal);
|
|
ParseFingerJoint(bodyJson, "rightRingMedial", body.rightRingMedial);
|
|
ParseFingerJoint(bodyJson, "rightRingDistal", body.rightRingDistal);
|
|
ParseFingerJoint(bodyJson, "rightRingTip", body.rightRingTip);
|
|
|
|
ParseFingerJoint(bodyJson, "rightLittleProximal", body.rightLittleProximal);
|
|
ParseFingerJoint(bodyJson, "rightLittleMedial", body.rightLittleMedial);
|
|
ParseFingerJoint(bodyJson, "rightLittleDistal", body.rightLittleDistal);
|
|
ParseFingerJoint(bodyJson, "rightLittleTip", body.rightLittleTip);
|
|
}
|
|
|
|
void RokokoDataParser::ParseFingerJoint(const nlohmann::json& bodyJson, const std::string& jointName, std::shared_ptr<RokokoData::ActorJointFrame>& joint)
|
|
{
|
|
if (bodyJson.contains(jointName)) {
|
|
auto& jointJson = bodyJson[jointName];
|
|
|
|
joint = std::make_shared<RokokoData::ActorJointFrame>();
|
|
|
|
// Position 파싱
|
|
if (jointJson.contains("position")) {
|
|
auto& pos = jointJson["position"];
|
|
joint->position.x = pos.value("x", 0.0f);
|
|
joint->position.y = pos.value("y", 0.0f);
|
|
joint->position.z = pos.value("z", 0.0f);
|
|
}
|
|
|
|
// Rotation 파싱
|
|
if (jointJson.contains("rotation")) {
|
|
auto& rot = jointJson["rotation"];
|
|
joint->rotation.x = rot.value("x", 0.0f);
|
|
joint->rotation.y = rot.value("y", 0.0f);
|
|
joint->rotation.z = rot.value("z", 0.0f);
|
|
joint->rotation.w = rot.value("w", 1.0f);
|
|
|
|
// 쿼터니언 정규화
|
|
NormalizeQuaternion(joint->rotation.x, joint->rotation.y, joint->rotation.z, joint->rotation.w);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool RokokoDataParser::ValidateFrameData(const RokokoData::LiveFrame_v4& frame)
|
|
{
|
|
try {
|
|
// 기본 검증
|
|
if (frame.scene.actors.empty()) {
|
|
return false;
|
|
}
|
|
|
|
const auto& actor = frame.scene.actors[0];
|
|
|
|
// 손가락 데이터 검증 (왼손 엄지손가락만 체크)
|
|
if (!actor.body.leftThumbMedial || !actor.body.leftThumbDistal || !actor.body.leftThumbTip) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool RokokoDataParser::ExtractFingerData(const RokokoData::LiveFrame_v4& frame,
|
|
std::vector<RokokoData::ActorJointFrame>& leftHandFingers,
|
|
std::vector<RokokoData::ActorJointFrame>& rightHandFingers)
|
|
{
|
|
try {
|
|
leftHandFingers.clear();
|
|
rightHandFingers.clear();
|
|
|
|
if (frame.scene.actors.empty()) {
|
|
return false;
|
|
}
|
|
|
|
const auto& actor = frame.scene.actors[0];
|
|
|
|
// 왼손 데이터 추출 (20개 관절)
|
|
if (actor.body.leftThumbProximal) leftHandFingers.push_back(*actor.body.leftThumbProximal);
|
|
if (actor.body.leftThumbMedial) leftHandFingers.push_back(*actor.body.leftThumbMedial);
|
|
if (actor.body.leftThumbDistal) leftHandFingers.push_back(*actor.body.leftThumbDistal);
|
|
if (actor.body.leftThumbTip) leftHandFingers.push_back(*actor.body.leftThumbTip);
|
|
|
|
if (actor.body.leftIndexProximal) leftHandFingers.push_back(*actor.body.leftIndexProximal);
|
|
if (actor.body.leftIndexMedial) leftHandFingers.push_back(*actor.body.leftIndexMedial);
|
|
if (actor.body.leftIndexDistal) leftHandFingers.push_back(*actor.body.leftIndexDistal);
|
|
if (actor.body.leftIndexTip) leftHandFingers.push_back(*actor.body.leftIndexTip);
|
|
|
|
if (actor.body.leftMiddleProximal) leftHandFingers.push_back(*actor.body.leftMiddleProximal);
|
|
if (actor.body.leftMiddleMedial) leftHandFingers.push_back(*actor.body.leftMiddleMedial);
|
|
if (actor.body.leftMiddleDistal) leftHandFingers.push_back(*actor.body.leftMiddleDistal);
|
|
if (actor.body.leftMiddleTip) leftHandFingers.push_back(*actor.body.leftMiddleTip);
|
|
|
|
if (actor.body.leftRingProximal) leftHandFingers.push_back(*actor.body.leftRingProximal);
|
|
if (actor.body.leftRingMedial) leftHandFingers.push_back(*actor.body.leftRingMedial);
|
|
if (actor.body.leftRingDistal) leftHandFingers.push_back(*actor.body.leftRingDistal);
|
|
if (actor.body.leftRingTip) leftHandFingers.push_back(*actor.body.leftRingTip);
|
|
|
|
if (actor.body.leftLittleProximal) leftHandFingers.push_back(*actor.body.leftLittleProximal);
|
|
if (actor.body.leftLittleMedial) leftHandFingers.push_back(*actor.body.leftLittleMedial);
|
|
if (actor.body.leftLittleDistal) leftHandFingers.push_back(*actor.body.leftLittleDistal);
|
|
if (actor.body.leftLittleTip) leftHandFingers.push_back(*actor.body.leftLittleTip);
|
|
|
|
// 오른손 데이터 추출 (20개 관절)
|
|
if (actor.body.rightThumbProximal) rightHandFingers.push_back(*actor.body.rightThumbProximal);
|
|
if (actor.body.rightThumbMedial) rightHandFingers.push_back(*actor.body.rightThumbMedial);
|
|
if (actor.body.rightThumbDistal) rightHandFingers.push_back(*actor.body.rightThumbDistal);
|
|
if (actor.body.rightThumbTip) rightHandFingers.push_back(*actor.body.rightThumbTip);
|
|
|
|
if (actor.body.rightIndexProximal) rightHandFingers.push_back(*actor.body.rightIndexProximal);
|
|
if (actor.body.rightIndexMedial) rightHandFingers.push_back(*actor.body.rightIndexMedial);
|
|
if (actor.body.rightIndexDistal) rightHandFingers.push_back(*actor.body.rightIndexDistal);
|
|
if (actor.body.rightIndexTip) rightHandFingers.push_back(*actor.body.rightIndexTip);
|
|
|
|
if (actor.body.rightMiddleProximal) rightHandFingers.push_back(*actor.body.rightMiddleProximal);
|
|
if (actor.body.rightMiddleMedial) rightHandFingers.push_back(*actor.body.rightMiddleMedial);
|
|
if (actor.body.rightMiddleDistal) rightHandFingers.push_back(*actor.body.rightMiddleDistal);
|
|
if (actor.body.rightMiddleTip) rightHandFingers.push_back(*actor.body.rightMiddleTip);
|
|
|
|
if (actor.body.rightRingProximal) rightHandFingers.push_back(*actor.body.rightRingProximal);
|
|
if (actor.body.rightRingMedial) rightHandFingers.push_back(*actor.body.rightRingMedial);
|
|
if (actor.body.rightRingDistal) rightHandFingers.push_back(*actor.body.rightRingDistal);
|
|
if (actor.body.rightRingTip) rightHandFingers.push_back(*actor.body.rightRingTip);
|
|
|
|
if (actor.body.rightLittleProximal) rightHandFingers.push_back(*actor.body.rightLittleProximal);
|
|
if (actor.body.rightLittleMedial) rightHandFingers.push_back(*actor.body.rightLittleMedial);
|
|
if (actor.body.rightLittleDistal) rightHandFingers.push_back(*actor.body.rightLittleDistal);
|
|
if (actor.body.rightLittleTip) rightHandFingers.push_back(*actor.body.rightLittleTip);
|
|
|
|
return true;
|
|
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool RokokoDataParser::ParseActorData(const void* actorJson, RokokoData::ActorData& actor)
|
|
{
|
|
// 현재는 기본 구현만 제공
|
|
// 실제 JSON 파싱 라이브러리 사용 시 구현
|
|
return true;
|
|
}
|
|
|
|
bool RokokoDataParser::ParseFingerData(const void* fingerJson, RokokoData::ActorJointFrame& finger)
|
|
{
|
|
// 현재는 기본 구현만 제공
|
|
// 실제 JSON 파싱 라이브러리 사용 시 구현
|
|
return true;
|
|
}
|
|
|
|
bool RokokoDataParser::ValidateQuaternion(float x, float y, float z, float w)
|
|
{
|
|
// NaN 체크
|
|
if (std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w)) {
|
|
return false;
|
|
}
|
|
|
|
// 무한대 체크
|
|
if (std::isinf(x) || std::isinf(y) || std::isinf(z) || std::isinf(w)) {
|
|
return false;
|
|
}
|
|
|
|
// 쿼터니언 길이 체크 (정규화된 경우 1.0에 가까워야 함)
|
|
float length = std::sqrt(x * x + y * y + z * z + w * w);
|
|
if (length < 0.1f || length > 10.0f) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void RokokoDataParser::NormalizeQuaternion(float& x, float& y, float& z, float& w)
|
|
{
|
|
float length = std::sqrt(x * x + y * y + z * z + w * w);
|
|
if (length > 0.0001f) {
|
|
x /= length;
|
|
y /= length;
|
|
z /= length;
|
|
w /= length;
|
|
} else {
|
|
// 기본값 설정
|
|
x = 0.0f;
|
|
y = 0.0f;
|
|
z = 0.0f;
|
|
w = 1.0f;
|
|
}
|
|
}
|
|
}
|