Humanoid본 위치를 확인할 수 있도록 Gizmos를 그림(노란선)

실제 Xsens Suit 위치에 해당하는 부분에 놓을 수 있도록 Offset 설정 기능(cyan색 구)

Json파일로 저장 / 불러오기 기능도 추가

#if UNITY_EDITOR
using UnityEngine;
using UnityEditor;
using System.IO;
using System.Collections.Generic;

public class SensorEditorWindow : EditorWindow
{
    SensorOffsetData data;
    Vector2 scroll;

    [MenuItem("Xsens/Sensor Editor")]
    static void Init()
    {
        GetWindow<SensorEditorWindow>("Sensor Editor");
    }

    void OnGUI()
    {
        EditorGUILayout.Space();
        if (GUILayout.Button("Export to JSON"))
        {
            string path = EditorUtility.SaveFilePanel("Export Sensor Data", "Assets", "XsensSensorData", "json");
            if (!string.IsNullOrEmpty(path))
            {
                SensorJsonUtility.ExportToJson(data, path);
            }
        }

        if (GUILayout.Button("Import from JSON"))
        {
            string path = EditorUtility.OpenFilePanel("Import Sensor Data", "Assets", "json");
            if (!string.IsNullOrEmpty(path))
            {
                SensorJsonUtility.ImportFromJson(data, path);
            }
        }

        data = (SensorOffsetData)EditorGUILayout.ObjectField("Sensor Data", data, typeof(SensorOffsetData), false);
        if (data == null) return;

        scroll = EditorGUILayout.BeginScrollView(scroll);

        foreach (var sensor in data.sensors)
        {
            EditorGUILayout.Space();
            EditorGUILayout.LabelField(string.Format($"Joint Sensor Name : {sensor.jointPoint}"), EditorStyles.boldLabel);
            sensor.jointPoint = (XsensJointManager.eXSensSuitJointPoint)EditorGUILayout.EnumPopup("Joint List", sensor.jointPoint);
            sensor.bone = (HumanBodyBones)EditorGUILayout.EnumPopup("Bone", sensor.bone);
            sensor.localPositionOffset = EditorGUILayout.Vector3Field("Position Offset", sensor.localPositionOffset);
            sensor.forward = EditorGUILayout.Vector3Field("Forward", sensor.forward);
            sensor.up = EditorGUILayout.Vector3Field("Up", sensor.up);
        }

        EditorGUILayout.EndScrollView();

        if (GUI.changed)
        {
            EditorUtility.SetDirty(data);
        }
    }
}
public static class SensorJsonUtility
{
    [System.Serializable]
    private class SensorEntryJson
    {
        public string name;
        public string bone;
        public Vector3 localPositionOffset;
        public Vector3 forward;
        public Vector3 up;
    }

    [System.Serializable]
    private class SensorEntryListJson
    {
        public List<SensorEntryJson> sensors;
    }

    public static void ExportToJson(SensorOffsetData data, string path)
    {
        var jsonData = new SensorEntryListJson { sensors = new List<SensorEntryJson>() };

        foreach (var sensor in data.sensors)
        {
            jsonData.sensors.Add(new SensorEntryJson
            {
                name = sensor.jointPoint.ToString(),
                bone = sensor.bone.ToString(),
                localPositionOffset = sensor.localPositionOffset,
                forward = sensor.forward,
                up = sensor.up
            });
        }

        string json = JsonUtility.ToJson(jsonData, true);
        File.WriteAllText(path, json);
        AssetDatabase.Refresh();
        Debug.Log("Sensor data exported to: " + path);
    }

    public static void ImportFromJson(SensorOffsetData data, string path)
    {
        if (!File.Exists(path))
        {
            Debug.LogError("File not found: " + path);
            return;
        }

        string json = File.ReadAllText(path);
        var jsonData = JsonUtility.FromJson<SensorEntryListJson>(json);

        data.sensors.Clear();

        foreach (var entry in jsonData.sensors)
        {
            if (!System.Enum.TryParse(entry.bone, out HumanBodyBones bone))
            {
                Debug.LogWarning($"Unknown bone: {entry.bone}");
                continue;
            }

            XsensJointManager.eXSensSuitJointPoint joint;

            if (!System.Enum.TryParse(entry.name, out joint))
            {
                Debug.LogWarning($"Unknown Joint : {entry.name}");
                continue;
            }

            data.sensors.Add(new SensorOffsetData.SensorEntry
            {
                jointPoint = joint,
                bone = bone,
                localPositionOffset = entry.localPositionOffset,
                forward = entry.forward,
                up = entry.up
            });
        }

        EditorUtility.SetDirty(data);
        Debug.Log("Sensor data imported from: " + path);
    }
}

#endif

using UnityEngine;
using System.Collections.Generic;

public class JointSpawner : GenericSingleton<JointSpawner>
{
    [SerializeField] private GameObject jointPrefab; // Empty + Gizmo용 프리팹
    [SerializeField] private Animator sourceAnimator; // 애니메이션 재생용 모델

    [SerializeField] private SensorOffsetData sensorOffsetData;

    //휴머노이드 리깅 계층구조
    Dictionary<int, int> sensorParentMap = new Dictionary<int, int>
    {
        { 1, 0 },   // Sternum → Pelvis
        { 2, 1 },   // Head → Sternum
    
        { 3, 1 },   // L Shoulder → Sternum
        { 4, 3 },   // L Upper Arm → L Shoulder
        { 5, 4 },   // L Lower Arm → L Upper Arm
        { 6, 5 },   // L Hand → L Lower Arm
    
        { 7, 0 },   // L Upper Leg → Pelvis
        { 8, 7 },   // L Lower Leg → L Upper Leg
        { 9, 8 },   // L Foot → L Lower Leg
    
        { 10, 1 },  // R Shoulder → Sternum
        { 11, 10 },
        { 12, 11 },
        { 13, 12 },

        { 14, 0 },
        { 15, 14 },
        { 16, 15 }
    };

    //계층 연결된 parent의 index를 반환
    public int GetSensorParentIndex(int currentSensorIndex)
    {
        if (sensorParentMap.TryGetValue(currentSensorIndex, out int parentIndex))
        {
            return parentIndex;
        }
        else
        {
            return -1;
        }
    }

    public List<GameObject> CreateJoint()
    {
        List<GameObject> jointObjects = new List<GameObject>();
        int count = Devcat.ValueCastTo<int>.From(XsensJointManager.eXSensSuitJointPoint.Count);

        for (int i = 0; i < count; i++)
        {
            var sensor = sensorOffsetData.sensors[i];

            //센서에 해당하는 본의 Transform을 받음
            Transform bone = sourceAnimator.GetBoneTransform(sensor.bone);

            if (bone == null)
            {
                Debug.LogWarning($"Bone not found: {sensor.bone}");

                continue;
            }

            //센서의 로컬 오프셋 값을 기준 본에 더한 위치에 생성
            GameObject joint = Instantiate(jointPrefab, bone.transform.position + sensor.localPositionOffset, Quaternion.identity);

            joint.name = $"{i:D2}_{sensor.jointPoint}";
            joint.transform.localScale = Vector3.one * 0.02f;
            jointObjects[i] = joint;

            // 회전 적용 (Z+: forward, Y+: up)
            joint.transform.rotation = Quaternion.LookRotation(
                bone.TransformDirection(sensor.forward.normalized),
                bone.TransformDirection(sensor.up.normalized)
            );
        }

        //실제 생성된 관절 포인트들을 하이라키 계층구로로 바꿔줌
        for (int i = 0; i < count; i++)
        {
            int parentIndex = GetSensorParentIndex(i);

            if (parentIndex != -1)
            {
                jointObjects[i].transform.SetParent(jointObjects[parentIndex].transform, true);
            }
            else
            {
                jointObjects[i].transform.SetParent(jointObjects[Devcat.ValueCastTo<int>.From(XsensJointManager.eXSensSuitJointPoint.Pelvis)].transform);
            }
        }

        return jointObjects;
    }
}

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Unity.Mathematics;

public class XsensJointManager : MonoBehaviour
{
    public enum eXSensSuitJointPoint
    {
        Head, Sternum, Pelvis,

        L_Shoulder, L_UpperArm, L_ForeArm, L_Hand,
        L_UpperLeg, L_LowerLeg, L_Foot,

        R_Shoulder, R_UpperArm, R_Forearm, R_Hand,
        R_UpperLeg, R_LowerLeg, R_Foot,
        Count
    }

    private List<GameObject> jointPoints;

    private Quaternion[] jointCalibrations = new Quaternion[Devcat.ValueCastTo<int>.From(eXSensSuitJointPoint.Count)];

    [Header("Target Model Animator")][SerializeField] private Animator animator;

    void Start()
    {
        jointPoints = JointSpawner.Instance.CreateJoint();
    }

    private void Calibration()
    {
        //T포즈 캘리브레이션

        SetCalibrationData();
    }

    //캘리브레이션 데이터 저장
    private void SetCalibrationData()
    {
        int index = 0;

        foreach (var joint in jointPoints)
        {
            jointCalibrations[index++] = joint.transform.rotation;
        }
    }

    //매개변수로 넘어온 관절 포인트의 회전값 반환
    private Quaternion GetSensorRotation(eXSensSuitJointPoint eXSensSuitJointPoint)
    {
        return jointCalibrations[Devcat.ValueCastTo<int>.From(eXSensSuitJointPoint)];
    }

    //매개변수로 넘어온 관절 포인트의 센서 회전값을 통해 본의 회전값을 역산하여 추정 (offset vector 회전수치 사용)
    private Quaternion GetBoneRotation(eXSensSuitJointPoint eXSensSuitJointPoint)
    {
        Transform boneTransform = animator.GetBoneTransform(HumanBodyBones.RightLowerLeg);
        Quaternion initialBoneRotation = boneTransform.rotation; // Unity 기준 회전

        Quaternion initialSensorRotation = GetSensorRotation(eXSensSuitJointPoint); // 센서가 처음 측정한 회전
        Quaternion R_offset = initialSensorRotation * Quaternion.Inverse(initialBoneRotation);
        Quaternion calibratedBoneRotation = jointPoints[Devcat.ValueCastTo<int>.From(eXSensSuitJointPoint)].transform.rotation * Quaternion.Inverse(R_offset);

        return calibratedBoneRotation;
    }

    Vector3 ConvertXsensToUnity(Vector3 xsensVector)
    {
        return new Vector3(
            xsensVector.x,    // X는 동일
            xsensVector.z,    // Z (Xsens up) → Unity Y
            xsensVector.y     // Y (Xsens forward) → Unity Z
        );
    }

    Quaternion ConvertXsensToUnity(Quaternion xsensRotation)
    {
        // Xsens은 오른손 좌표계이므로, Z축 반전을 통해 Unity 좌표계에 맞춤
        return new Quaternion(xsensRotation.x, xsensRotation.y, -xsensRotation.z, -xsensRotation.w);
    }

    private void ApplyBoneRotate(Quaternion xsensSensorRotation, SensorOffsetData.SensorEntry sensor)
    {
        var bone = animator.GetBoneTransform(sensor.bone);

        bone.rotation = ConvertXsensToUnity(xsensSensorRotation) * Quaternion.LookRotation(sensor.forward, sensor.up);
        var sensorRotation = Quaternion.LookRotation(sensor.forward, sensor.up);
        bone.rotation = ConvertXsensToUnity(xsensSensorRotation) * sensorRotation;

    }

    // List<JointInfo> sensorJointInfos = new List<JointInfo>
    // {
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.Pelvis),       bone = HumanBodyBones.Hips,             forward = Vector3.forward,  up = Vector3.up },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.Sternum),      bone = HumanBodyBones.Chest,            forward = Vector3.forward,  up = Vector3.up },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.Head),         bone = HumanBodyBones.Head,             forward = Vector3.forward,  up = Vector3.up },

    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_Shoulder),   bone = HumanBodyBones.LeftShoulder,     forward = Vector3.right,    up = Vector3.down },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_UpperArm),   bone = HumanBodyBones.LeftUpperArm,     forward = Vector3.down,     up = Vector3.back },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_ForeArm),    bone = HumanBodyBones.LeftLowerArm,     forward = Vector3.down,     up = Vector3.back },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_Hand),       bone = HumanBodyBones.LeftHand,         forward = Vector3.forward,  up = Vector3.up },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_UpperLeg),   bone = HumanBodyBones.LeftUpperLeg,     forward = Vector3.down,     up = Vector3.forward },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_LowerLeg),   bone = HumanBodyBones.LeftLowerLeg,     forward = Vector3.down,     up = Vector3.forward },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.L_Foot),       bone = HumanBodyBones.LeftFoot,         forward = Vector3.forward,  up = Vector3.up },

    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_Shoulder),   bone = HumanBodyBones.RightShoulder,    forward = Vector3.left,     up = Vector3.down },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_UpperArm),   bone = HumanBodyBones.RightUpperArm,    forward = Vector3.down,     up = Vector3.back },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_Forearm),    bone = HumanBodyBones.RightLowerArm,    forward = Vector3.down,     up = Vector3.back },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_Hand),       bone = HumanBodyBones.RightHand,        forward = Vector3.forward,  up = Vector3.up },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_UpperLeg),   bone = HumanBodyBones.RightUpperLeg,    forward = Vector3.down,     up = Vector3.forward },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_LowerLeg),   bone = HumanBodyBones.RightLowerLeg,    forward = Vector3.down,     up = Vector3.forward },
    //     new JointInfo { name = GetJointName(eXSensSuitJointPoint.R_Foot),       bone = HumanBodyBones.RightFoot,        forward = Vector3.forward,  up = Vector3.up }
    // };

    // private static string GetJointName(eXSensSuitJointPoint eXSensSuitJointPoint)
    // {
    //     string str = string.Empty;

    //     switch (eXSensSuitJointPoint)
    //     {
    //         case eXSensSuitJointPoint.Head:
    //             str = eXSensSuitJointPoint.Head.ToString();
    //             break;
    //         case eXSensSuitJointPoint.Sternum:
    //             str = eXSensSuitJointPoint.Sternum.ToString();
    //             break;
    //         case eXSensSuitJointPoint.Pelvis:
    //             str = eXSensSuitJointPoint.Pelvis.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_Shoulder:
    //             str = eXSensSuitJointPoint.L_Shoulder.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_UpperArm:
    //             str = eXSensSuitJointPoint.L_UpperArm.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_ForeArm:
    //             str = eXSensSuitJointPoint.L_ForeArm.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_Hand:
    //             str = eXSensSuitJointPoint.L_Hand.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_UpperLeg:
    //             str = eXSensSuitJointPoint.L_UpperLeg.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_LowerLeg:
    //             str = eXSensSuitJointPoint.L_LowerLeg.ToString();
    //             break;
    //         case eXSensSuitJointPoint.L_Foot:
    //             str = eXSensSuitJointPoint.L_Foot.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_Shoulder:
    //             str = eXSensSuitJointPoint.R_Shoulder.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_UpperArm:
    //             str = eXSensSuitJointPoint.R_UpperArm.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_Forearm:
    //             str = eXSensSuitJointPoint.R_Forearm.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_Hand:
    //             str = eXSensSuitJointPoint.R_Hand.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_UpperLeg:
    //             str = eXSensSuitJointPoint.R_UpperLeg.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_LowerLeg:
    //             str = eXSensSuitJointPoint.R_LowerLeg.ToString();
    //             break;
    //         case eXSensSuitJointPoint.R_Foot:
    //             str = eXSensSuitJointPoint.R_Foot.ToString();
    //             break;
    //     }

    //     return str;
    // }

}

using System.Collections.Generic;
using UnityEngine;

[CreateAssetMenu(fileName = "SensorOffsetData", menuName = "Xsens/SensorOffsetData")]
public class SensorOffsetData : ScriptableObject
{
    [System.Serializable]
    public class SensorEntry
    {
        public XsensJointManager.eXSensSuitJointPoint jointPoint;   //관절 포인트
        public HumanBodyBones bone;                                 //관절 포인트에 해당하는 휴머노이드 리깅 본
        public Vector3 localPositionOffset;                         //본에서 관절 포인트가 얼마나 떨어져있는지
        public Vector3 forward = Vector3.forward;
        public Vector3 up = Vector3.up;
    }

    public List<SensorEntry> sensors;
}

using UnityEngine;
using System.Collections.Generic;

public class SensorDebugger : MonoBehaviour
{
    public SensorOffsetData sensorData;
    public Animator animator;
    public float gizmoSize = 0.02f;

    void OnDrawGizmos()
    {
        if (sensorData == null || animator == null) return;

        int index = 0;

        foreach (var sensor in sensorData.sensors)
        {
            var bone = animator.GetBoneTransform(sensor.bone);
            if (bone == null)
            {
                continue;
            }

            Vector3 basePos = bone.position;
            Vector3 sensorPos = bone.TransformPoint(sensor.localPositionOffset);

            // Draw line
            //Gizmos.color = Color.yellow;
            //Gizmos.DrawLine(basePos, sensorPos);

            // Draw sensor position
            Gizmos.color = Color.cyan;
            Gizmos.DrawSphere(sensorPos, gizmoSize * 0.1f);

            // Draw forward/up
            Vector3 forward = bone.TransformDirection(sensor.forward.normalized);
            Vector3 up = bone.TransformDirection(sensor.up.normalized);

            Gizmos.color = Color.red;
            Gizmos.DrawLine(sensorPos, sensorPos + forward * gizmoSize);

            Gizmos.color = Color.green;
            Gizmos.DrawLine(sensorPos, sensorPos + up * gizmoSize);

            //모든 센서를 휴머노이드 계층 구조에 따라 연결시켜줌

            int parentIndex = JointSpawner.Instance.GetSensorParentIndex(index);

            if (parentIndex != -1)
            {
                var parentSensor = sensorData.sensors[parentIndex];
                // Draw line
                Gizmos.color = Color.yellow;
                //bone base
                Gizmos.DrawLine(basePos, animator.GetBoneTransform(parentSensor.bone).position);
                //sensor 
                //Gizmos.DrawLine(bone.TransformPoint(sensorData.sensors[index].localPositionOffset), 
                //                  animator.GetBoneTransform(parentSensor.bone).TransformPoint(parentSensor.localPositionOffset));
            }

            index++;
        }
    }
}