public void SetIKKeyframes(float time, Avatar avatar, float humanScale, Vector3 bodyPosition, Quaternion bodyRotation)
{
// TODO Use character scale
TQ IKTQ = AvatarUtility.GetIKGoalTQ(avatar, humanScale, goal, new TQ(bodyPosition, bodyRotation), new TQ(transform.position, transform.rotation));
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);
}
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);
}