114 lines
3.9 KiB
C#
114 lines
3.9 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
using System;
|
|
|
|
namespace RootMotion.FinalIK {
|
|
|
|
/// <summary>
|
|
/// CCD (Cyclic Coordinate Descent) constrainable heuristic inverse kinematics algorithm.
|
|
/// </summary>
|
|
[System.Serializable]
|
|
public class IKSolverCCD : IKSolverHeuristic {
|
|
|
|
#region Main Interface
|
|
|
|
/// <summary>
|
|
/// 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.
|
|
/// </summary>
|
|
public void FadeOutBoneWeights() {
|
|
if (bones.Length < 2) return;
|
|
|
|
bones[0].weight = 1f;
|
|
float step = 1f / (bones.Length - 1);
|
|
|
|
for (int i = 1; i < bones.Length; i++) {
|
|
bones[i].weight = step * (bones.Length - 1 - i);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Called before each iteration of the solver.
|
|
/// </summary>
|
|
public IterationDelegate OnPreIteration;
|
|
|
|
#endregion Main Interface
|
|
|
|
protected override void OnInitiate() {
|
|
if (firstInitiation || !Application.isPlaying) IKPosition = bones[bones.Length - 1].transform.position;
|
|
|
|
InitiateBones();
|
|
}
|
|
|
|
protected override void OnUpdate() {
|
|
if (IKPositionWeight <= 0) return;
|
|
IKPositionWeight = Mathf.Clamp(IKPositionWeight, 0f, 1f);
|
|
|
|
if (target != null) IKPosition = target.position;
|
|
if (XY) IKPosition.z = bones[0].transform.position.z;
|
|
|
|
Vector3 singularityOffset = maxIterations > 1? GetSingularityOffset(): Vector3.zero;
|
|
|
|
// Iterating the solver
|
|
for (int i = 0; i < maxIterations; i++) {
|
|
|
|
// Optimizations
|
|
if (singularityOffset == Vector3.zero && i >= 1 && tolerance > 0 && positionOffset < tolerance * tolerance) break;
|
|
lastLocalDirection = localDirection;
|
|
|
|
if (OnPreIteration != null) OnPreIteration(i);
|
|
|
|
Solve(IKPosition + (i == 0? singularityOffset: Vector3.zero));
|
|
}
|
|
|
|
lastLocalDirection = localDirection;
|
|
}
|
|
|
|
/*
|
|
* Solve the CCD algorithm
|
|
* */
|
|
protected void Solve(Vector3 targetPosition) {
|
|
// 2D
|
|
if (XY) {
|
|
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 = bones[i].weight * IKPositionWeight;
|
|
|
|
if (w > 0f) {
|
|
Vector3 toLastBone = bones[bones.Length - 1].transform.position - bones[i].transform.position;
|
|
Vector3 toTarget = targetPosition - bones[i].transform.position;
|
|
|
|
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].transform.rotation = Quaternion.AngleAxis(Mathf.DeltaAngle(angleToLastBone, angleToTarget) * w, Vector3.back) * bones[i].transform.rotation;
|
|
}
|
|
|
|
// Rotation Constraints
|
|
if (useRotationLimits && bones[i].rotationLimit != null) bones[i].rotationLimit.Apply();
|
|
}
|
|
// 3D
|
|
} else {
|
|
for (int i = bones.Length - 2; i > -1; i--) {
|
|
// Slerp if weight is < 0
|
|
//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 = bones[i].weight * IKPositionWeight;
|
|
|
|
if (w > 0f) {
|
|
Vector3 toLastBone = bones[bones.Length - 1].transform.position - bones[i].transform.position;
|
|
Vector3 toTarget = targetPosition - bones[i].transform.position;
|
|
|
|
// Get the rotation to direct the last bone to the target
|
|
Quaternion targetRotation = Quaternion.FromToRotation(toLastBone, toTarget) * bones[i].transform.rotation;
|
|
|
|
if (w >= 1) bones[i].transform.rotation = targetRotation;
|
|
else bones[i].transform.rotation = Quaternion.Lerp(bones[i].transform.rotation, targetRotation, w);
|
|
}
|
|
|
|
// Rotation Constraints
|
|
if (useRotationLimits && bones[i].rotationLimit != null) bones[i].rotationLimit.Apply();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|