test/Assets/ModelRenderer/Scripts/SkinnedMesh/SkeletonBuilder.cs

using System.Collections.Generic;
using UnityEngine;

namespace BrewMonster.Scripts
{
    public class SkeletonBuilder : MonoBehaviour
    {
        public List<Transform> bones = new List<Transform>();
        public List<Matrix4x4> bindPoses = new List<Matrix4x4>();
        public Transform rootBone;

        

        /// <summary>
        /// This function retrieves an array of transforms for the specified bone names.
        /// It iterates through the input names, finds the matching transform in the global bones list,
        /// and then appends the root bone transform as the final element before returning the assembled array.
        /// </summary>
        /// <param name="boneNames"></param>
        /// <returns></returns>
        public Transform[] GetBones(string[] boneNames)
        {
            if (boneNames == null || boneNames.Length == 0)
            {
                // Return all bones if boneNames is empty or null
                Transform[] allBoneTransforms = new Transform[bones.Count + 1];
                for (int i = 0; i < bones.Count; i++)
                {
                    allBoneTransforms[i] = bones[i];
                }
                allBoneTransforms[bones.Count] = rootBone;
                return allBoneTransforms;
            }
            else
            {
                // Get only the bones with names in the boneNames array
                Transform[] boneTransforms = new Transform[boneNames.Length + 1];
                for (int i = 0; i < boneNames.Length; i++)
                {
                    boneTransforms[i] = bones.Find(bone => bone.name == boneNames[i]);
                }
                boneTransforms[boneNames.Length] = rootBone;
                return boneTransforms;
            }
        }
#if MODEL_RENDERER_PROJECT
        private A3DSkeleton _skeleton;
        private int _rootBoneIndex;

        public void BuildSkeleton(A3DSkeleton skeleton)
        {
            if (skeleton == null)
            {
                Debug.LogError("No skeleton data found.");
                return;
            }

            _skeleton = skeleton;


            Dictionary<int, GameObject> boneGameObjects = new Dictionary<int, GameObject>();
            GameObject boneGO = null;
            // Create GameObject for each bone
            for (int i = 0; i < skeleton.m_aBones.Count; i++)
            {
                boneGO = new GameObject(skeleton.m_aBones[i].m_strName);
                boneGameObjects[i] = boneGO;
            }

            A3DBone bone;


            // first find the root bone and set up the relative TM for each bone
            for (int i = 0; i < skeleton.m_aBones.Count; i++)
            {
                bone = skeleton.m_aBones[i];
                bone.m_relativeTM = CreateMatrixFromA3DMatrix4X4(bone.boneData.matRelative);
                Console.WriteLine($"Bone {bone.m_strName} relative\n{bone.m_relativeTM}");
                if (bone.boneData.iParent < 0)
                {
                    _rootBoneIndex = i;
                    rootBone = boneGameObjects[i].transform;
                }
            }

            // Build upToRootTM matrix for each bone. Start from the root bone.
            // for now it's not required
            BuildUpToRootMatrix(skeleton.m_aBones[_rootBoneIndex]);

            // setup parent-child relationship
            foreach (var kvp in boneGameObjects)
            {
                int boneIndex = kvp.Key;
                boneGO = kvp.Value;
                int parentIndex = skeleton.m_aBones[boneIndex].boneData.iParent;

                if (parentIndex >= 0)
                {
                    boneGO.transform.parent = boneGameObjects[parentIndex].transform;
                }
                else
                {
                    boneGO.transform.parent = this.transform; // Set root bones to be children of the skeleton GameObject
                }
            }
            
            // Set up position of each bone
            foreach (var kvp in boneGameObjects)
            {
                int boneIndex = kvp.Key;
                boneGO = kvp.Value;
                bone = skeleton.m_aBones[boneIndex];
                
                Matrix4x4 inverseInitMatrix = CreateMatrixFromA3DMatrix4X4(bone.boneData.matBoneInit).inverse;
                Vector3 boneWorldPosition = inverseInitMatrix.MultiplyPoint3x4(Vector3.zero);

                boneGO.transform.position = boneWorldPosition;
                boneGO.transform.rotation = Quaternion.LookRotation(inverseInitMatrix.GetColumn(2), inverseInitMatrix.GetColumn(1));
                boneGO.transform.localScale = inverseInitMatrix.lossyScale;
                
                // print out data as human readable
                PrintOutData(boneGO, bone.m_upToRootTM, inverseInitMatrix, inverseInitMatrix.rotation.eulerAngles, bone);
            }

            // Add bones to the list and build bind poses for each bone.
            for (int i = 0; i < skeleton.m_aBones.Count; i++)
            {
                boneGO = boneGameObjects[i];
                bones.Add(boneGO.transform);

                if (i != _rootBoneIndex)
                {
                    // Calculate bind pose using worldToLocalMatrix
                    bindPoses.Add(boneGO.transform.worldToLocalMatrix * rootBone.localToWorldMatrix);
                }
                else // Root bone
                {
                    bindPoses.Add(boneGO.transform.worldToLocalMatrix);
                }
            }

            // now we have to create the hooks for the skeleton
            for (int i = 0; i < skeleton.m_aHooks.Count; i++)
            {
                A3DSkeletonHook hook = skeleton.m_aHooks[i];
                GameObject hookGO = new GameObject(hook.m_strName);
                hookGO.transform.parent = hook.Data.iBone >= 0 ? boneGameObjects[hook.Data.iBone].transform : rootBone;
                hookGO.transform.localPosition = CreateMatrixFromA3DMatrix4X4(hook.Data.matHookTM).GetColumn(3);
                hookGO.transform.localRotation = Quaternion.LookRotation(CreateMatrixFromA3DMatrix4X4(hook.Data.matHookTM).GetColumn(2), CreateMatrixFromA3DMatrix4X4(hook.Data.matHookTM).GetColumn(1));
                hookGO.transform.localScale = CreateMatrixFromA3DMatrix4X4(hook.Data.matHookTM).lossyScale;
            }
        }

        public Matrix4x4[] GetBindPoses(string[] boneNames)
        {
            Matrix4x4[] boneBindPoses = null;
            if (boneNames?.Length > 0)
            {
                boneBindPoses = new Matrix4x4[boneNames.Length + 1];
                for (int i = 0; i < boneNames.Length; i++)
                {
                    boneBindPoses[i] = bindPoses[bones.FindIndex(bone => bone.name == boneNames[i])];
                }
                boneBindPoses[boneNames.Length] = bindPoses[_rootBoneIndex];
            }
            else
            {
                boneBindPoses = new Matrix4x4[bones.Count + 1];
                for (int i = 0; i < bones.Count; i++)
                {
                    boneBindPoses[i] = bindPoses[i];
                }
                boneBindPoses[bones.Count] = bindPoses[_rootBoneIndex];
            }

            return boneBindPoses;
        }

        private Matrix4x4 CreateMatrixFromA3DMatrix4X4(A3DMATRIX4 mat)
        {
            return new Matrix4x4(
                new Vector4(mat.m[0], mat.m[1], mat.m[2], mat.m[3]),
                new Vector4(mat.m[4], mat.m[5], mat.m[6], mat.m[7]),
                new Vector4(mat.m[8], mat.m[9], mat.m[10], mat.m[11]),
                new Vector4(mat.m[12], mat.m[13], mat.m[14], mat.m[15])
            );
        }

        /// <summary>
        /// Build the upToRootTM matrix for each bone in the skeleton. <br/>
        /// Just send in the root bone and this function will run recursively to build the upToRootTM matrix for each bone.
        /// </summary>
        /// <param name="rootBone"></param>
        private void BuildUpToRootMatrix(A3DBone rootBone)
        {
            if (_skeleton == null)
            {
                Debug.LogError("No skeleton data found.");
                return;
            }

            // Get the bone's parent index
            int parentIndex = rootBone.boneData.iParent;
            if (parentIndex < 0)
            {
                rootBone.m_upToRootTM = rootBone.m_relativeTM;
            }
            else
            {
                // Get the parent bone
                A3DBone parentBone = _skeleton.m_aBones[parentIndex];
                // Calculate the upToRootTM matrix
                rootBone.m_upToRootTM = parentBone.m_upToRootTM * rootBone.m_relativeTM;
            }

            for (int i = 0; i < rootBone.m_aChildren.Length; i++)
            {
                BuildUpToRootMatrix(_skeleton.m_aBones[rootBone.m_aChildren[i]]);
            }
        }


        private void PrintOutData(GameObject boneGO, Matrix4x4 relativeMatrix, Matrix4x4 initMatrix,
            Vector3 initRotation, A3DBone bone)
        {
            // debug log the relative matrix in a human readable format
            Console.WriteLine($"Bone {boneGO.name} relative\n{relativeMatrix}\ninit\n{initMatrix}");

            Console.WriteLine($"{boneGO.name} Flipped: {bone.boneData.byFlags}");

            // print out matBoneInit position
            Console.WriteLine($"Bone {boneGO.name} matBoneInit position: {initMatrix.GetColumn(3)}");
            // print out matBoneInit rotation
            Console.WriteLine($"Bone {boneGO.name} matBoneInit rotation: {initRotation}");
        }

#endif
    }
}