// 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());
}
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());
}