private void Solve(UnityEngine.Animations.AnimationStream s, Vector3 targetPosition, bool XY, float weight, int it, bool useRotationLimits)
{
for (int i = bones.Length - 2; i > -1; i--)
{
//CCD tends to overemphasise the rotations of the bones closer to the target position. Reducing bone weight down the hierarchy will compensate for this effect.
float w = weight * boneWeights[i].GetFloat(s);
if (w > 0f)
{
Vector3 bonePos = bones[i].GetPosition(s);
Vector3 toLastBone = bones[bones.Length - 1].GetPosition(s) - bonePos;
Vector3 toTarget = targetPosition - bonePos;
// Get the rotation to direct the last bone to the target
if (XY)
{
float angleToLastBone = Mathf.Atan2(toLastBone.x, toLastBone.y) * Mathf.Rad2Deg;
float angleToTarget = Mathf.Atan2(toTarget.x, toTarget.y) * Mathf.Rad2Deg;
// Rotation to direct the last bone to the target
bones[i].SetRotation(s, Quaternion.AngleAxis(Mathf.DeltaAngle(angleToLastBone, angleToTarget) * w, Vector3.back) * bones[i].GetRotation(s));
}
else
{
Quaternion boneRot = bones[i].GetRotation(s);
Quaternion targetRotation = Quaternion.FromToRotation(toLastBone, toTarget) * boneRot;
if (w >= 1)
{
bones[i].SetRotation(s, targetRotation);
}
else
{
bones[i].SetRotation(s, Quaternion.Lerp(boneRot, targetRotation, w));
}
}
}
// Rotation Constraints
if (useRotationLimits)
{
if (hingeFlags[i] == 1)
{
Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
Quaternion lastRotation = hingeLastRotations[i];
float lastAngle = hingeLastAngles[i];
Quaternion r = RotationLimitUtilities.LimitHinge(localRotation, hingeMinArray[i].GetFloat(s), hingeMaxArray[i].GetFloat(s), hingeUseLimitsArray[i].GetBool(s), limitAxisArray[i], ref lastRotation, ref lastAngle);
hingeLastRotations[i] = lastRotation;
hingeLastAngles[i] = lastAngle;
bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
}
else if (angleFlags[i] == 1)
{
Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
Quaternion r = RotationLimitUtilities.LimitAngle(localRotation, limitAxisArray[i], angleSecondaryAxisArray[i], angleLimitArray[i].GetFloat(s), angleTwistLimitArray[i].GetFloat(s));
bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
}
}
}
}
// Rotating bone to get transform aim closer to target
private void RotateToTarget(UnityEngine.Animations.AnimationStream s, Vector3 targetPosition, Vector3 polePosition, int i, float weight, float poleWeight, bool XY, bool useRotationLimits, Vector3 axis, Vector3 poleAxis)
{
// Swing
if (XY)
{
if (weight >= 0f)
{
Vector3 dir = _transform.GetRotation(s) * axis;
Vector3 targetDir = targetPosition - _transform.GetPosition(s);
float angleDir = Mathf.Atan2(dir.x, dir.y) * Mathf.Rad2Deg;
float angleTarget = Mathf.Atan2(targetDir.x, targetDir.y) * Mathf.Rad2Deg;
bones[i].SetRotation(s, Quaternion.AngleAxis(Mathf.DeltaAngle(angleDir, angleTarget), Vector3.back) * bones[i].GetRotation(s));
}
}
else
{
if (weight >= 0f)
{
Quaternion rotationOffset = Quaternion.FromToRotation(_transform.GetRotation(s) * axis, targetPosition - _transform.GetPosition(s));
if (weight >= 1f)
{
bones[i].SetRotation(s, rotationOffset * bones[i].GetRotation(s));
}
else
{
bones[i].SetRotation(s, Quaternion.Lerp(Quaternion.identity, rotationOffset, weight) * bones[i].GetRotation(s));
}
}
// Pole
if (poleWeight > 0f)
{
Vector3 poleDirection = polePosition - _transform.GetPosition(s);
// Ortho-normalize to transform axis to make this a twisting only operation
Vector3 poleDirOrtho = poleDirection;
Vector3 normal = _transform.GetRotation(s) * axis;
Vector3.OrthoNormalize(ref normal, ref poleDirOrtho);
Quaternion toPole = Quaternion.FromToRotation(_transform.GetRotation(s) * poleAxis, poleDirOrtho);
bones[i].SetRotation(s, Quaternion.Lerp(Quaternion.identity, toPole, weight * poleWeight) * bones[i].GetRotation(s));
}
}
// Rotation Constraints
if (useRotationLimits)
{
if (hingeFlags[i] == 1)
{
Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
Quaternion lastRotation = hingeLastRotations[i];
float lastAngle = hingeLastAngles[i];
Quaternion r = RotationLimitUtilities.LimitHinge(localRotation, hingeMinArray[i].GetFloat(s), hingeMaxArray[i].GetFloat(s), hingeUseLimitsArray[i].GetBool(s), limitAxisArray[i], ref lastRotation, ref lastAngle);
hingeLastRotations[i] = lastRotation;
hingeLastAngles[i] = lastAngle;
bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
}
else if (angleFlags[i] == 1)
{
Quaternion localRotation = Quaternion.Inverse(limitDefaultLocalRotationArray[i]) * bones[i].GetLocalRotation(s);
Quaternion r = RotationLimitUtilities.LimitAngle(localRotation, limitAxisArray[i], angleSecondaryAxisArray[i], angleLimitArray[i].GetFloat(s), angleTwistLimitArray[i].GetFloat(s));
bones[i].SetLocalRotation(s, limitDefaultLocalRotationArray[i] * r);
}
}
}
