private static Quaternion GetRotationSpace(BoneWrapper[] bones, Quaternion avatarOrientation, int boneIndex)
{
Quaternion parentDelta = Quaternion.identity;
BonePoseData pose = sBonePoses[boneIndex];
if (!pose.compareInGlobalSpace)
{
int parentIndex = HumanTrait.GetParentBone(boneIndex);
if (parentIndex > 0)
{
BonePoseData parentPose = sBonePoses[parentIndex];
if (bones[parentIndex].bone != null && parentPose != null)
{
Vector3 parentDir = GetBoneAlignmentDirection(bones, avatarOrientation, parentIndex);
if (parentDir != Vector3.zero)
{
Vector3 parentPoseDir = avatarOrientation * parentPose.direction;
parentDelta = Quaternion.FromToRotation(parentPoseDir, parentDir);
}
}
}
}
return(parentDelta * avatarOrientation);
}
private static Vector3 GetBoneAlignmentDirection(BoneWrapper[] bones, Quaternion avatarOrientation, int boneIndex)
{
if (sBonePoses[boneIndex] == null)
{
return(Vector3.zero);
}
BoneWrapper bone = bones[boneIndex];
Vector3 dir;
// Get the child bone
BonePoseData pose = sBonePoses[boneIndex];
int childBoneIndex = -1;
if (pose.childIndices != null)
{
foreach (int i in pose.childIndices)
{
if (bones[i].bone != null)
{
childBoneIndex = i;
break;
}
}
}
else
{
childBoneIndex = GetHumanBoneChild(bones, boneIndex);
}
// TODO@MECANIM Something si wrong with the indexes
//if (boneIndex == (int)HumanBodyBones.LeftHand)
// Debug.Log ("Child bone for left hand: "+childBoneIndex);
if (childBoneIndex >= 0 && bones[childBoneIndex] != null && bones[childBoneIndex].bone != null)
{
// Get direction from bone to child
BoneWrapper childBone = bones[childBoneIndex];
dir = childBone.bone.position - bone.bone.position;
// TODO@MECANIM Something si wrong with the indexes
//if (boneIndex == (int)HumanBodyBones.LeftHand)
// Debug.Log (" - "+childBone.humanBoneName + " - " +childBone.bone.name);
}
else
{
if (bone.bone.childCount != 1)
{
return(Vector3.zero);
}
dir = Vector3.zero;
// Get direction from bone to child
foreach (Transform child in bone.bone)
{
dir = child.position - bone.bone.position;
break;
}
}
return(dir.normalized);
}
public static void MakeBoneAlignmentValid(BoneWrapper[] bones, Quaternion avatarOrientation, int boneIndex)
{
if (boneIndex < 0 || boneIndex >= sBonePoses.Length || boneIndex >= bones.Length)
{
return;
}
BoneWrapper bone = bones[boneIndex];
BonePoseData pose = sBonePoses[boneIndex];
if (bone.bone == null || pose == null)
{
return;
}
if (boneIndex == (int)HumanBodyBones.Hips)
{
float angleX = Vector3.Angle(avatarOrientation * Vector3.right, Vector3.right);
float angleY = Vector3.Angle(avatarOrientation * Vector3.up, Vector3.up);
float angleZ = Vector3.Angle(avatarOrientation * Vector3.forward, Vector3.forward);
if (angleX > pose.maxAngle || angleY > pose.maxAngle || angleZ > pose.maxAngle)
{
bone.bone.rotation = Quaternion.Inverse(avatarOrientation) * bone.bone.rotation;
}
return;
}
Vector3 dir = GetBoneAlignmentDirection(bones, avatarOrientation, boneIndex);
if (dir == Vector3.zero)
{
return;
}
Quaternion space = GetRotationSpace(bones, avatarOrientation, boneIndex);
Vector3 goalDir = space * pose.direction;
if (pose.planeNormal != Vector3.zero)
{
dir = Vector3.ProjectOnPlane(dir, space * pose.planeNormal);
}
// If the bone direction is not close enough to the target direction,
// rotate it so it matches the target direction.
float deltaAngle = Vector3.Angle(dir, goalDir);
if (deltaAngle > pose.maxAngle * 0.99f)
{
Quaternion adjust = Quaternion.FromToRotation(dir, goalDir);
// If this bone is hip or knee, remember global foor rotation and apply it after this adjustment
Transform footBone = null;
Quaternion footRot = Quaternion.identity;
if (boneIndex == (int)HumanBodyBones.LeftUpperLeg || boneIndex == (int)HumanBodyBones.LeftLowerLeg)
{
footBone = bones[(int)HumanBodyBones.LeftFoot].bone;
}
if (boneIndex == (int)HumanBodyBones.RightUpperLeg || boneIndex == (int)HumanBodyBones.RightLowerLeg)
{
footBone = bones[(int)HumanBodyBones.RightFoot].bone;
}
if (footBone != null)
{
footRot = footBone.rotation;
}
// Adjust only enough to fall within maxAngle
float adjustAmount = Mathf.Clamp01(1.05f - (pose.maxAngle / deltaAngle));
adjust = Quaternion.Slerp(Quaternion.identity, adjust, adjustAmount);
bone.bone.rotation = adjust * bone.bone.rotation;
// Revert foot rotation to what it was
if (footBone != null)
{
footBone.rotation = footRot;
}
}
}
