void calculateBoneLengths()
{
// Posiitons are in the lsp order.
int[] boneJoints = new int[]
{
// 14 means hip
2, 3,
2, 1,
1, 0,
3, 4,
4, 5,
14, 12,
12, 8,
8, 7,
7, 6,
12, 9,
9, 10,
10, 11,
12, 13,
};
var positions = Positions.ToList();
var hip = (positions[2] + positions[3]) * 0.5f;
positions.Add(hip);
for (int i = 0; i < 13; i++)
{
int src = boneJoints[2 * i];
int dst = boneJoints[2 * i + 1];
var translate = positions[src] - positions[dst];
var boneLength = translate.magnitude;
BoneLengths.Add(boneLength);
}
}
// Get the joint positions of the avatar in Unity by the forward kinematics
public List <Vector3> ForwardKinematics()
{
// get the joint positions, bone lengths and the joint local rotations
var positions = Positions.ToList();
var lengths = BoneLengths.ToList();
var rots = LocalRotations.ToList();
// get the joint order and the parent joint order(MPII order)
int[] joints = new int[] { 6, 2, 1, 0, 3, 4, 5, 7, 8, 9, 12, 11, 10, 13, 14, 15 };
int[] parentJoints = new int[] { 6, 6, 2, 1, 6, 3, 4, 6, 7, 8, 8, 12, 11, 8, 13, 14 }; // pelvis's parent is self(6)
// get the new joint positions of the avatar by FK
Vector3[] fkPositions = new Vector3[16];
Quaternion[] globalRotations = new Quaternion[16];
for (int i = 0; i < joints.Length; i++)
{
float len = lengths[i];
Quaternion localRot = rots[i];
int j = joints[i];
int pj = parentJoints[i];
Vector3 pPos = fkPositions[pj];
Quaternion parentRot = globalRotations[pj];
Quaternion globalRot = parentRot * localRot;
Vector3 translate = globalRot * Vector3.left * len;
Vector3 pos = pPos + translate;
fkPositions[j] = pos;
globalRotations[j] = globalRot;
}
// return the new joint positions of the avatar
return(fkPositions.Take(16).ToList());
}
// Clear the class buffer for BoneLengths/HumanToAvatarTransformRotations/Positions/Rotations/LocalRotations
public void Clear()
{
Positions.Clear();
HumanToAvatarTransformRotations.Clear();
BoneLengths.Clear();
Rotations.Clear();
LocalRotations.Clear();
}
public List <Vector3> FK()
{
var positions = Positions.ToList();
var hip = (positions[2] + positions[3]) * 0.5f;
positions.Add(hip);
var lengths = BoneLengths.ToList();
var hipLength = lengths[0] * 0.5f;
lengths[0] = hipLength;
lengths.Insert(0, -hipLength);
var rots = LocalRotations.ToList();
rots.Insert(0, rots[0]);
int[] joints = new int[] { 2, 3, 1, 0, 4, 5, 12, 8, 7, 6, 9, 10, 11, 13, };
// 14 means hip
int[] parentJoints = new int[] { 14, 14, 2, 1, 3, 4, 14, 12, 8, 7, 12, 9, 10, 12, };
Vector3[] fkPositions = new Vector3[15];
fkPositions[14] = hip;
Quaternion[] globalRotations = new Quaternion[15];
globalRotations[14] = Quaternion.identity;
for (int i = 0; i < joints.Length; i++)
{
float len = lengths[i];
Quaternion localRot = rots[i];
int j = joints[i];
int p = parentJoints[i];
Vector3 pPos = fkPositions[p];
Quaternion parentRot = globalRotations[p];
Quaternion globalRot = parentRot * localRot;
Vector3 translate = globalRot * Vector3.left * len;
Vector3 pos = pPos + translate;
fkPositions[j] = pos;
globalRotations[j] = globalRot;
}
return(fkPositions.Take(14).ToList());
}
public void CalculateBoneLengths()
{
// Positions of human joint keypoints are in the MPII order
int[] boneJoints = new int[]
{
// 15 human bones in total and 6 means pelvis(hip)
6, 2, // rightHipBone
2, 1, // rightThigh
1, 0, // rightCalf
6, 3, // leftHipBone
3, 4, // leftThigh
4, 5, // leftCalf
6, 7, // waist
7, 8, // chest
8, 9, // neck
8, 12, // rightClavicle
12, 11, // rightUpperArm
11, 10, // rightForearm
8, 13, // leftClavicle
13, 14, // leftUpperArm
14, 15, // leftForearm
};
var positions = Positions.ToList();
for (int i = 0; i < 15; i++)
{
int src = boneJoints[2 * i];
int dst = boneJoints[2 * i + 1];
var translate = positions[dst] - positions[src]; // vector points to dst from src
var boneLength = translate.magnitude;
BoneLengths.Add(boneLength);
}
}
