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