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