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