public static TQ WorldSpaceIKGoalToBone(TQ goalTQ, Avatar avatar, AvatarIKGoal avatarIKGoal) { int humanId = (int)HumanIDFromAvatarIKGoal(avatarIKGoal); if (humanId == (int)HumanBodyBones.LastBone) { throw new InvalidOperationException("Invalid human id."); } MethodInfo methodGetAxisLength = typeof(Avatar).GetMethod("GetAxisLength", BindingFlags.Instance | BindingFlags.NonPublic); if (methodGetAxisLength == null) { throw new InvalidOperationException("Cannot find GetAxisLength method."); } MethodInfo methodGetPostRotation = typeof(Avatar).GetMethod("GetPostRotation", BindingFlags.Instance | BindingFlags.NonPublic); if (methodGetPostRotation == null) { throw new InvalidOperationException("Cannot find GetPostRotation method."); } Quaternion postRotation = (Quaternion)methodGetPostRotation.Invoke(avatar, new object[] { humanId }); if (avatarIKGoal == AvatarIKGoal.LeftFoot || avatarIKGoal == AvatarIKGoal.RightFoot) { // Here you could use animator.leftFeetBottomHeight or animator.rightFeetBottomHeight rather than GetAxisLenght // Both are equivalent but GetAxisLength is the generic way and work for all human bone float axislength = (float)methodGetAxisLength.Invoke(avatar, new object[] { humanId }); Vector3 footBottom = new Vector3(axislength, 0, 0); goalTQ.t -= (goalTQ.q * footBottom); } TQ boneTQ = new TQ(goalTQ.t, goalTQ.q * Quaternion.Inverse(postRotation)); return(boneTQ); }
public void SetIKKeyframes(float time, Avatar avatar, Transform root, float humanScale, Vector3 bodyPosition, Quaternion bodyRotation) { Vector3 bonePos = transform.position; Quaternion boneRot = transform.rotation; if (root.parent != null) { bonePos = root.parent.InverseTransformPoint(bonePos); boneRot = Quaternion.Inverse(root.parent.rotation) * boneRot; } TQ IKTQ = AvatarUtility.GetIKGoalTQ(avatar, humanScale, goal, new TQ(bodyPosition, bodyRotation), new TQ(bonePos, boneRot)); Quaternion rot = IKTQ.q; if (lastQSet) { rot = BakerUtilities.EnsureQuaternionContinuity(lastQ, IKTQ.q); } //rot.Normalize(); lastQ = rot; lastQSet = true; rotX.AddKey(time, rot.x); rotY.AddKey(time, rot.y); rotZ.AddKey(time, rot.z); rotW.AddKey(time, rot.w); Vector3 pos = IKTQ.t; posX.AddKey(time, pos.x); posY.AddKey(time, pos.y); posZ.AddKey(time, pos.z); }
/// <summary> /// Get IK position and rotation for foot/hand bone position/rotation. /// </summary> public static TQ GetIKGoalTQ(Avatar avatar, float humanScale, AvatarIKGoal avatarIKGoal, TQ bodyPositionRotation, TQ boneTQ) { int humanId = (int)HumanIDFromAvatarIKGoal(avatarIKGoal); if (humanId == (int)HumanBodyBones.LastBone) { throw new InvalidOperationException("Invalid human id."); } MethodInfo methodGetAxisLength = typeof(Avatar).GetMethod("GetAxisLength", BindingFlags.Instance | BindingFlags.NonPublic); if (methodGetAxisLength == null) { throw new InvalidOperationException("Cannot find GetAxisLength method."); } MethodInfo methodGetPostRotation = typeof(Avatar).GetMethod("GetPostRotation", BindingFlags.Instance | BindingFlags.NonPublic); if (methodGetPostRotation == null) { throw new InvalidOperationException("Cannot find GetPostRotation method."); } Quaternion postRotation = (Quaternion)methodGetPostRotation.Invoke(avatar, new object[] { humanId }); var goalTQ = new TQ(boneTQ.t, boneTQ.q * postRotation); if (avatarIKGoal == AvatarIKGoal.LeftFoot || avatarIKGoal == AvatarIKGoal.RightFoot) { // Here you could use animator.leftFeetBottomHeight or animator.rightFeetBottomHeight rather than GetAxisLenght // Both are equivalent but GetAxisLength is the generic way and work for all human bone float axislength = (float)methodGetAxisLength.Invoke(avatar, new object[] { humanId }); Vector3 footBottom = new Vector3(axislength, 0, 0); goalTQ.t += (goalTQ.q * footBottom); } // IK goal are in avatar body local space Quaternion invRootQ = Quaternion.Inverse(bodyPositionRotation.q); goalTQ.t = invRootQ * (goalTQ.t - bodyPositionRotation.t); goalTQ.q = invRootQ * goalTQ.q; goalTQ.t /= humanScale; goalTQ.q = Quaternion.LookRotation(goalTQ.q * Vector3.forward, goalTQ.q * Vector3.up); return(goalTQ); }