How to calculate nearest bones, weights and bindposes programmatically?

Hey guys,

I'm trying to implement mesh rigging algorithm inside the Unity.

For testing, I'm using Ethan's mesh. The algorithm looks something like:

1. For each bone, find all related bones (parents and children)

2. For each vertex in mesh, find nearest bone in hierarchy

3. Then, find nearest four (or less) related bones

4. Calculate weights inversely proportional to the distance from vertex

5. Calculate bindposes

This is how mesh looks like with 1 bone quality set in SkinnedMeshRenderer: And this is with 4 bone quality:

I was working on it for a quite while and don't know how to fix this problem.

Here's the code I am using:

 public class RiggingHelper : MonoBehaviour
 {
     private GameObject rootBoneObject;
     private Transform[] bones;
     private Mesh mesh;
     private float boneDistance;
     private int boneIndex;
     private int vertexCount;
     private BoneWeight[] boneWeights;
     private Matrix4x4[] bindposes;
     private int[] boneIndicies;
     private Vector3[] vertices;
     private Vector3[] bonesPositions;
 
     private bool finished = false;
     private bool calcWeightsFinished = false;
 
     private Vector3 weightBonesVertex;
     private Vector3[] weightBonesPositions;
     private float[] weightBonesWeights;
     private int[] weightBonesIndices;
 
     private List<Vector3>[] relatedBonesPositions;
     private List<int>[] relatedBonesIndices;
 
     public void SkinMesh(SkinnedMeshRenderer meshRenderer, GameObject skeleton)
     {
         rootBoneObject = skeleton;
         List<Transform> bonesList = new List<Transform>();
         foreach (var bone in skeleton.GetComponentsInChildren<Transform>())
         {
             if (bone.parent != null)
                 bonesList.Add(bone);
         }
         bones = bonesList.ToArray();
 
         mesh = meshRenderer.sharedMesh;
         vertexCount = mesh.vertexCount;
         meshRenderer.bones = bones;
         boneWeights = new BoneWeight[vertexCount];
         boneIndicies = new int[vertexCount];
         vertices = mesh.vertices;
         bonesPositions = new Vector3[bones.Length];
 
         for (int i = 0; i < bones.Length; i++)
         {
             bonesPositions[i] = bones[i].position;
         }
 
         StartCoroutine(SkinMeshCoroutine());
     }
 
     private IEnumerator SkinMeshCoroutine()
     {
         ThreadPool.QueueUserWorkItem(FindNearestBones);
         while (!finished)
             yield return null;
 
         FindRelatedBones();
 
         finished = false;
         ThreadPool.QueueUserWorkItem(CalcAdditionalWeights);
         while (!finished)
             yield return null;
 
         // Create bindposes
         bindposes = new Matrix4x4[bones.Length];
         for (int i = 0; i < bindposes.Length; i++)
         {
             if (bones[i].parent != null)
                 bindposes[i] = bones[i].worldToLocalMatrix * bones[i].parent.localToWorldMatrix;
             else
                 bindposes[i] = bones[i].worldToLocalMatrix;
         }
 
         mesh.boneWeights = boneWeights;
         mesh.bindposes = bindposes;
     }
 
     private void FindNearestBones(object token)
     {
         // For each of the vertices, find nearest bone
         for (int i = 0; i < vertexCount; i++)
         {
             Vector3 vertex = vertices[i];
             boneIndex = 0;
             boneDistance = Vector3.Distance(bonesPositions[0], vertex);
             for (int j = 1; j < bones.Length; j++)
             {
                 float newDistance = Vector3.Distance(bonesPositions[j], vertex);
                 if (newDistance < boneDistance)
                 {
                     boneIndex = j;
                     boneDistance = newDistance;
                 }
             }
             boneIndicies[i] = boneIndex;
         }
         finished = true;
     }
 
     private void FindRelatedBones()
     {
         relatedBonesPositions = new List<Vector3>[bones.Length];
         relatedBonesIndices = new List<int>[bones.Length];
 
         // Find all related bones, including children and parents
         for (int i = 0; i < bones.Length; i++)
         {
             Transform startBone = bones[i];
             relatedBonesPositions[i] = new List<Vector3>();
             relatedBonesIndices[i] = new List<int>();
 
             relatedBonesPositions[i].Add(startBone.position);
             relatedBonesIndices[i].Add(i);
 
             foreach (var child in startBone.GetComponentsInChildren<Transform>())
             {
                 if (child.gameObject.GetInstanceID() != startBone.gameObject.GetInstanceID())
                 {
                     relatedBonesPositions[i].Add(child.position);
                     for (int j = 0; j < bones.Length; j++)
                     {
                         if (bones[j].gameObject.GetInstanceID() == child.gameObject.GetInstanceID())
                         {
                             relatedBonesIndices[i].Add(j);
                             break;
                         }
                     }
                 }
             }
             foreach (var parent in startBone.GetComponentsInParent<Transform>())
             {
                 if (parent.gameObject.GetInstanceID() != startBone.gameObject.GetInstanceID() &&
                     parent.gameObject.GetInstanceID() != startBone.parent.gameObject.GetInstanceID())
                 {
                     for (int j = 0; j < bones.Length; j++)
                     {
                         if (bones[j].gameObject.GetInstanceID() == parent.gameObject.GetInstanceID())
                         {
                             relatedBonesPositions[i].Add(parent.position);
                             relatedBonesIndices[i].Add(j);
                             break;
                         }
                     }
                 }
             }
         }
     }
 
     private void CalcAdditionalWeights(object token)
     {
         for (int i = 0; i < boneIndicies.Length; i++)
         {
             weightBonesVertex = vertices[i];
             weightBonesPositions = relatedBonesPositions[boneIndicies[i]].ToArray();
             weightBonesIndices = relatedBonesIndices[boneIndicies[i]].ToArray();
             calcWeightsFinished = false;
             GetBonesWithWages();
 
             // Assign bone indicies and weights
             for (int j = 0; j < weightBonesWeights.Length; j++)
             {
                 switch (j)
                 {
                     case 0:
                         boneWeights[i].boneIndex0 = weightBonesIndices[j];
                         boneWeights[i].weight0 = weightBonesWeights[j];
                         break;
                     case 1:
                         boneWeights[i].boneIndex1 = weightBonesIndices[j];
                         boneWeights[i].weight1 = weightBonesWeights[j];
                         break;
                     case 2:
                         boneWeights[i].boneIndex2 = weightBonesIndices[j];
                         boneWeights[i].weight2 = weightBonesWeights[j];
                         break;
                     case 3:
                         boneWeights[i].boneIndex3 = weightBonesIndices[j];
                         boneWeights[i].weight3 = weightBonesWeights[j];
                         break;
                     default:
                         break;
                 }
             }
         }
         finished = true;
     }
 
     private void GetBonesWithWages()
     {
         float[] distances = new float[weightBonesPositions.Length];
 
         for (int i = 0; i < distances.Length; i++)
             distances[i] = Vector3.Distance(weightBonesVertex, weightBonesPositions[i]);
 
         // Sort distances ascending
         for (int j = distances.Length - 1; j > 0; j--)
         {
             for (int i = 0; i < j; i++)
             {
                 if (distances[i] > distances[i + 1])
                 {
                     // Sort distance, position and index
                     float tmpDist = distances[i];
                     distances[i] = distances[i + 1];
                     distances[i + 1] = tmpDist;
 
                     Vector3 tmpPos = weightBonesPositions[i];
                     weightBonesPositions[i] = weightBonesPositions[i + 1];
                     weightBonesPositions[i + 1] = tmpPos;
 
                     int tmpIndex = weightBonesIndices[i];
                     weightBonesIndices[i] = weightBonesIndices[i + 1];
                     weightBonesIndices[i + 1] = tmpIndex;
                 }
             }
         }
 
         // Take up to 4 distances, calc their weights and find corresponding indices
         float distanceSum = 0f;
         int index = 0;
 
         for (int i = 0; i < 4; i++)
         {
             if (i < distances.Length)
             {
                 distanceSum += distances[i];
                 index++;
             }
             else
                 break;
         }
 
         weightBonesWeights = new float[index];
         for (int i = 0; i < 4; i++)
         {
             if (i < distances.Length)
             {
                 weightBonesWeights[i] = distances[i] / distanceSum;
             }
             else
                 break;
         }
         Array.Reverse(weightBonesWeights);
 
         calcWeightsFinished = true;
     }
 }