// 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 Wall[] GetValue(Wall[] walls) { var count = walls.Length; var curRotation = Rotations[0]; var curLocalRotation = LocalRotations[0]; var prevRotation = Rotations[1]; var prevLocalRotation = LocalRotations[1]; var rotationEasingStore = new Vector3EasingStore { Easing = Easing, StartVector = curRotation.Item1, EndVector = Rotations[1].Item1 }; var localRotationEasingStore = new Vector3EasingStore { Easing = Easing, StartVector = curLocalRotation.Item1, EndVector = LocalRotations[1].Item1 }; for (var i = 0; i < count; i++) { var time = i / (count - 1f); var nextRotation = Rotations.First(t => t.Item2 >= time); if (nextRotation.Item2 > prevRotation.Item2) { curRotation = prevRotation; rotationEasingStore.NextPoint(nextRotation.Item1); } var rotationTime = Math.Abs(time) <= 0 ? time : (time - curRotation.Item2) / (nextRotation.Item2 - curRotation.Item2); var nextLocalRotation = LocalRotations.First(t => t.Item2 >= time); if (nextLocalRotation.Item2 > prevLocalRotation.Item2) { curLocalRotation = prevLocalRotation; localRotationEasingStore.NextPoint(nextLocalRotation.Item1); } var localRotationTime = Math.Abs(time) <= 0 ? time : (time - curLocalRotation.Item2) / (nextLocalRotation.Item2 - curLocalRotation.Item2); walls[i].Rotation = rotationEasingStore.GetValue(rotationTime); walls[i].LocalRotation = localRotationEasingStore.GetValue(localRotationTime); if (Math.Abs(time) > 0) { prevRotation = nextRotation; prevLocalRotation = nextLocalRotation; } } return(walls); }
// Clear the class buffer for Positions/AvatarBoneLengths/HumanBoneVectors/HumanBoneLengths/Rotations/LocalRotations/FKPositions public void AllClear() { Positions.Clear(); AvatarBoneLengths.Clear(); HumanBoneVectors.Clear(); HumanBoneLengths.Clear(); Rotations.Clear(); LocalRotations.Clear(); FKPositions.Clear(); }
// Clear the class buffer for Positions/HumanToAvatarTransformRotations/AvatarBoneLengths/HumanBoneVectors/HumanBoneLengths/Rotations/LocalRotations/FKPositions public void Clear() { Positions.Clear(); HumanToAvatarTransformRotations.Clear(); AvatarBoneLengths.Clear(); HumanBoneVectors.Clear(); HumanBoneLengths.Clear(); Rotations.Clear(); LocalRotations.Clear(); FKPositions.Clear(); }
// AllClear is called for clearing the class buffer for Positions/AvatarBoneLengths/Rotations/LocalRotations/HumanBoneVectors/HumanBoneLengths/FKPositions and Frame/UseAbsoluteCoordinate/UseLocalRotation/UseForwardKinematics/SetFootOnGround public void AllClear() { Frame = 0; Positions.Clear(); AvatarBoneLengths.Clear(); UseAbsoluteCoordinate = false; UseLocalRotation = false; UseForwardKinematics = false; SetFootOnGround = false; Rotations.Clear(); LocalRotations.Clear(); HumanBoneVectors.Clear(); HumanBoneLengths.Clear(); FKPositions.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()); }
// CalculateBoneRotations is called for calculating the bone rotations(global/local) in the method of joint angle mapping public void CalculateBoneRotations() { // calculate the Bone Global Rotations for (int i = 0; i < 15; i++) { Rotations.Add(Quaternion.identity); // initialize the Rotations } // check whether using absolute coordinate rotation and pick the different methods(aligning coordinates/relatively rotating) to calculate the bone rotations if (UseAbsoluteCoordinate) { // calculate the bone global rotations using the method of aligning bone coordinates(make sure that the coordinate system will be the same(fixed to the bones) for every frame based on the human joint keypoints) Rotations[0] = CalculateGlobalRotation(Positions[6] - Positions[2], Positions[7] - Positions[6]); // Rotations[1] = CalculateGlobalRotation(Positions[2] - Positions[1], Positions[1] - Positions[0]); // rightThigh Rotations[2] = CalculateGlobalRotation(Positions[1] - Positions[0], Positions[1] - Positions[2]); // rightCalf Rotations[3] = CalculateGlobalRotation(Positions[6] - Positions[3], Positions[6] - Positions[7]); // Rotations[4] = CalculateGlobalRotation(Positions[3] - Positions[4], Positions[4] - Positions[5]); // leftThigh Rotations[5] = CalculateGlobalRotation(Positions[4] - Positions[5], Positions[4] - Positions[3]); // leftCalf Rotations[6] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2]); // Rotations[7] = CalculateGlobalRotation(Positions[7] - Positions[8], Positions[6] - Positions[7]); // Rotations[8] = CalculateGlobalRotation(Positions[8] - Positions[9], Positions[7] - Positions[8]); // Rotations[9] = CalculateGlobalRotation(Positions[8] - Positions[12], Positions[8] - Positions[7]); // rightClavicle Rotations[10] = CalculateGlobalRotation(Positions[12] - Positions[11], Positions[11] - Positions[10]); // rightUpperArm Rotations[11] = CalculateGlobalRotation(Positions[11] - Positions[10], Positions[11] - Positions[12]); // rightForearm Rotations[12] = CalculateGlobalRotation(Positions[8] - Positions[13], Positions[7] - Positions[8]); // Rotations[13] = CalculateGlobalRotation(Positions[13] - Positions[14], Positions[14] - Positions[15]); // leftUpperArm Rotations[14] = CalculateGlobalRotation(Positions[14] - Positions[15], Positions[14] - Positions[13]); // leftForearm } else { // define the initial reference coordinate rotations var rotLeftForward = Quaternion.LookRotation(Vector3.left, Vector3.forward); var rotLeftBack = Quaternion.LookRotation(Vector3.left, Vector3.back); var rotLeftDown = Quaternion.LookRotation(Vector3.left, Vector3.down); var rotLeftUp = Quaternion.LookRotation(Vector3.left, Vector3.up); // TODO: calculate the bone global rotations using the method of relatively rotating Rotations[0] = CalculateGlobalRotation(Positions[2] - Positions[6], Positions[7] - Positions[6], rotLeftDown); // rightHipBone Rotations[1] = CalculateGlobalRotation(Positions[1] - Positions[2], Positions[0] - Positions[1], rotLeftForward); // rightThigh Rotations[2] = CalculateGlobalRotation(Positions[0] - Positions[1], Positions[2] - Positions[1], rotLeftForward); // rightCalf Rotations[3] = CalculateGlobalRotation(Positions[3] - Positions[6], Positions[6] - Positions[7], rotLeftDown); // leftHipBone Rotations[4] = CalculateGlobalRotation(Positions[4] - Positions[3], Positions[5] - Positions[4], rotLeftForward); // leftThigh Rotations[5] = CalculateGlobalRotation(Positions[5] - Positions[4], Positions[3] - Positions[4], rotLeftForward); // leftCalf Rotations[6] = CalculateGlobalRotation(Positions[7] - Positions[6], Positions[3] - Positions[2], rotLeftDown); // waist Rotations[7] = CalculateGlobalRotation(Positions[8] - Positions[7], Positions[6] - Positions[7], rotLeftDown); // chest Rotations[8] = CalculateGlobalRotation(Positions[9] - Positions[8], Positions[13] - Positions[12], rotLeftDown); // neck Rotations[9] = CalculateGlobalRotation(Positions[12] - Positions[8], Positions[8] - Positions[7], rotLeftUp); // rightClavicle Rotations[10] = CalculateGlobalRotation(Positions[11] - Positions[12], Positions[10] - Positions[11], rotLeftBack); // rightUpperArm Rotations[11] = CalculateGlobalRotation(Positions[10] - Positions[11], Positions[12] - Positions[11], rotLeftBack); // rightForearm Rotations[12] = CalculateGlobalRotation(Positions[13] - Positions[8], Positions[7] - Positions[8], rotLeftUp); // leftClavicle Rotations[13] = CalculateGlobalRotation(Positions[14] - Positions[13], Positions[15] - Positions[14], rotLeftBack); // leftUpperArm Rotations[14] = CalculateGlobalRotation(Positions[15] - Positions[14], Positions[13] - Positions[14], rotLeftBack); // leftForearm } // calculate the Bone Local Rotations for (int j = 0; j < 15; j++) { LocalRotations.Add(Quaternion.identity); // initialize the LocalRotations } // check whether using local rotation of bones for driving avatar if (UseLocalRotation) { // calculate the bone local rotations using the method of relatively rotating LocalRotations[0] = CalculateLocalRotation(Rotations[6], Rotations[0]); // rightHipBone LocalRotations[1] = CalculateLocalRotation(Rotations[0], Rotations[1]); // rightThigh LocalRotations[2] = CalculateLocalRotation(Rotations[1], Rotations[2]); // rightCalf LocalRotations[3] = CalculateLocalRotation(Rotations[6], Rotations[3]); // leftHipBone LocalRotations[4] = CalculateLocalRotation(Rotations[3], Rotations[4]); // leftThigh LocalRotations[5] = CalculateLocalRotation(Rotations[4], Rotations[5]); // leftCalf LocalRotations[6] = Rotations[6]; // waist LocalRotations[7] = CalculateLocalRotation(Rotations[6], Rotations[7]); // chest LocalRotations[8] = CalculateLocalRotation(Rotations[7], Rotations[8]); // neck LocalRotations[9] = CalculateLocalRotation(Rotations[8], Rotations[9]); // rightClavicle LocalRotations[10] = CalculateLocalRotation(Rotations[9], Rotations[10]); // rightUpperArm LocalRotations[11] = CalculateLocalRotation(Rotations[10], Rotations[11]); // rightForearm LocalRotations[12] = CalculateLocalRotation(Rotations[8], Rotations[12]); // leftClavicle LocalRotations[13] = CalculateLocalRotation(Rotations[12], Rotations[13]); // leftUpperArm LocalRotations[14] = CalculateLocalRotation(Rotations[13], Rotations[14]); // leftForearm } }
// CalculateBoneRotations is called for calculating the bone rotations(global/local) in the method of joint angle mapping public void CalculateBoneRotations() { // calculate the Bone Global Rotations for (int i = 0; i < 16; i++) { Rotations.Add(Quaternion.identity); // initialize the Rotations } // check whether using absolute coordinate rotation and pick the different methods(aligning coordinates/relatively rotating) to calculate the bone rotations if (UseAbsoluteCoordinate) { // calculate the bone global rotations using the method of aligning bone coordinates(make sure that the coordinate system will be the same(fixed to the bones) for every frame based on the human joint keypoints) Rotations[0] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // rightHipBone(rotTypeFlag:3) Rotations[1] = CalculateGlobalRotation(Positions[2] - Positions[1], Positions[1] - Positions[0], 1); // rightThigh(rotTypeFlag:1) Rotations[2] = CalculateGlobalRotation(Positions[1] - Positions[0], Positions[1] - Positions[2], 1); // rightCalf(rotTypeFlag:1) Rotations[3] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // leftHipBone(rotTypeFlag:3) Rotations[4] = CalculateGlobalRotation(Positions[3] - Positions[4], Positions[4] - Positions[5], 1); // leftThigh(rotTypeFlag:1) Rotations[5] = CalculateGlobalRotation(Positions[4] - Positions[5], Positions[4] - Positions[3], 1); // leftCalf(rotTypeFlag:1) Rotations[6] = CalculateGlobalRotation(Positions[6] - Positions[7], Positions[3] - Positions[2], 3); // waist(rotTypeFlag:3) Rotations[7] = CalculateGlobalRotation(Positions[7] - Positions[8], Positions[6] - Positions[7], 1); // chest(rotTypeFlag:1) Rotations[8] = CalculateGlobalRotation(Positions[8] - Positions[9], Positions[7] - Positions[8], 1) * new Quaternion(0.0f, 0.0f, 0.342f, 0.940f); // neck(rotTypeFlag:1)(correct the neck up for 40 degrees in z axis rotation direction) Rotations[9] = CalculateGlobalRotation(Positions[9] - Positions[10], Positions[8] - Positions[9], 1); // head(rotTypeFlag:1) Rotations[10] = CalculateGlobalRotation(Positions[8] - Positions[13], Vector3.Cross(Positions[8] - Positions[13], Positions[8] - Positions[7]), 2); // rightClavicle(rotTypeFlag:2) Rotations[11] = CalculateGlobalRotation(Positions[13] - Positions[12], Positions[12] - Positions[11], 1); // rightUpperArm(rotTypeFlag:1) Rotations[12] = CalculateGlobalRotation(Positions[12] - Positions[11], Positions[12] - Positions[13], 1); // rightForearm(rotTypeFlag:1) Rotations[13] = CalculateGlobalRotation(Positions[8] - Positions[14], Vector3.Cross(Positions[8] - Positions[14], Positions[7] - Positions[8]), 2); // leftClavicle(rotTypeFlag:2) Rotations[14] = CalculateGlobalRotation(Positions[14] - Positions[15], Positions[15] - Positions[16], 1); // leftUpperArm(rotTypeFlag:1) Rotations[15] = CalculateGlobalRotation(Positions[15] - Positions[16], Positions[15] - Positions[14], 1); // leftForearm(rotTypeFlag:1) } else { // define the initial reference coordinate rotations var rotLeftForward = Quaternion.LookRotation(Vector3.left, Vector3.forward); var rotLeftBack = Quaternion.LookRotation(Vector3.left, Vector3.back); var rotLeftDown = Quaternion.LookRotation(Vector3.left, Vector3.down); var rotLeftUp = Quaternion.LookRotation(Vector3.left, Vector3.up); // calculate the bone global rotations using the method of relatively rotating Rotations[0] = CalculateGlobalRotation(Positions[2] - Positions[6], Positions[7] - Positions[6], rotLeftDown); // rightHipBone Rotations[1] = CalculateGlobalRotation(Positions[1] - Positions[2], Positions[0] - Positions[1], rotLeftForward); // rightThigh Rotations[2] = CalculateGlobalRotation(Positions[0] - Positions[1], Positions[2] - Positions[1], rotLeftForward); // rightCalf Rotations[3] = CalculateGlobalRotation(Positions[3] - Positions[6], Positions[6] - Positions[7], rotLeftDown); // leftHipBone Rotations[4] = CalculateGlobalRotation(Positions[4] - Positions[3], Positions[5] - Positions[4], rotLeftForward); // leftThigh Rotations[5] = CalculateGlobalRotation(Positions[5] - Positions[4], Positions[3] - Positions[4], rotLeftForward); // leftCalf Rotations[6] = CalculateGlobalRotation(Positions[7] - Positions[6], Positions[3] - Positions[2], rotLeftDown); // waist Rotations[7] = CalculateGlobalRotation(Positions[8] - Positions[7], Positions[6] - Positions[7], rotLeftDown); // chest Rotations[8] = CalculateGlobalRotation(Positions[9] - Positions[8], Positions[13] - Positions[12], rotLeftDown); // neck Rotations[9] = CalculateGlobalRotation(Positions[10] - Positions[9], Positions[8] - Positions[9], rotLeftDown); // head Rotations[10] = CalculateGlobalRotation(Positions[13] - Positions[8], Positions[8] - Positions[7], rotLeftUp); // rightClavicle Rotations[11] = CalculateGlobalRotation(Positions[12] - Positions[13], Positions[11] - Positions[12], rotLeftBack); // rightUpperArm Rotations[12] = CalculateGlobalRotation(Positions[11] - Positions[12], Positions[13] - Positions[12], rotLeftBack); // rightForearm Rotations[13] = CalculateGlobalRotation(Positions[14] - Positions[8], Positions[7] - Positions[8], rotLeftUp); // leftClavicle Rotations[14] = CalculateGlobalRotation(Positions[15] - Positions[14], Positions[16] - Positions[15], rotLeftBack); // leftUpperArm Rotations[15] = CalculateGlobalRotation(Positions[16] - Positions[15], Positions[14] - Positions[15], rotLeftBack); // leftForearm } // calculate the Bone Local Rotations for (int j = 0; j < 16; j++) { LocalRotations.Add(Quaternion.identity); // initialize the LocalRotations } // check whether using local rotation of bones for driving avatar if (UseLocalRotation) { // calculate the bone local rotations using the method of relatively rotating LocalRotations[0] = CalculateLocalRotation(Rotations[6], Rotations[0]); // rightHipBone LocalRotations[1] = CalculateLocalRotation(Rotations[0], Rotations[1]); // rightThigh LocalRotations[2] = CalculateLocalRotation(Rotations[1], Rotations[2]); // rightCalf LocalRotations[3] = CalculateLocalRotation(Rotations[6], Rotations[3]); // leftHipBone LocalRotations[4] = CalculateLocalRotation(Rotations[3], Rotations[4]); // leftThigh LocalRotations[5] = CalculateLocalRotation(Rotations[4], Rotations[5]); // leftCalf // LocalRotations[6] = Rotations[6]; // waist LocalRotations[6] = CalculateLocalRotation(new Quaternion(-0.5f, 0.5f, 0.5f, 0.5f), Rotations[6]); // waist(relative rotation of pelvis to the world coordinate system) LocalRotations[7] = CalculateLocalRotation(Rotations[6], Rotations[7]); // chest LocalRotations[8] = CalculateLocalRotation(Rotations[7], Rotations[8]); // neck LocalRotations[9] = CalculateLocalRotation(Rotations[8], Rotations[9]); // head LocalRotations[10] = CalculateLocalRotation(Rotations[8], Rotations[10]); // rightClavicle LocalRotations[11] = CalculateLocalRotation(Rotations[10], Rotations[11]); // rightUpperArm LocalRotations[12] = CalculateLocalRotation(Rotations[11], Rotations[12]); // rightForearm LocalRotations[13] = CalculateLocalRotation(Rotations[8], Rotations[13]); // leftClavicle LocalRotations[14] = CalculateLocalRotation(Rotations[13], Rotations[14]); // leftUpperArm LocalRotations[15] = CalculateLocalRotation(Rotations[14], Rotations[15]); // leftForearm } }