public static bool CCD(IKSegment[] segments, IKEndEffector effector, IKTarget target, out Quaternion[] goals)
{
goals = new Quaternion[segments.Length];
Transform contact;
var links = createLinks(segments, effector.transform, out contact);
if (links.Length < 2) return false;
IKLink end = links[links.Length - 1];
end.transform.localRotation = target.transform.rotation;
int link = links.Length - 2;
bool success = false;
for(int i = 0; i < links.Length*30; i++) {
IKLink root = links[link];
if (Vector3.Distance(contact.position, target.transform.position) > target.Radius) {
Vector3 currentVector = (contact.position - root.transform.position).normalized;
Vector3 targetVector = (target.transform.position - root.transform.position).normalized;
float cosAngle = Vector3.Dot(targetVector, currentVector);
if (cosAngle < 0.9999f) {
Vector3 cross = Vector3.Cross(currentVector, targetVector).normalized;
float turn = Mathf.Min(Mathf.Rad2Deg * Mathf.Acos(cosAngle), root.segRef.DampDegrees);
Quaternion result = Quaternion.AngleAxis(turn, cross);
//Constrain the joint if it is to be constrained
if (root.segRef.Constrain) {
if (root.segRef.JointType == IKJointType.Cone) {
result = constrainCone(result, root);
} else {
Vector3 toChild = root.transform.parent.rotation * root.segRef.originalDir;
Vector3 dir = (result * toChild).normalized;
Vector3 xAxis = root.segRef.ParentOffset*root.transform.parent.up;
float actualAngle;
//Debug.Log("Constraining x");
Quaternion xConstrain = constrainAxis(root, xAxis, Vector3.up, toChild, dir, root.segRef.Min.x, root.segRef.Max.x, out actualAngle);
if (root.segRef.JointType == IKJointType.TwoDOF) {
Vector3 dirMinusXAxis = Quaternion.Inverse(Quaternion.AngleAxis(actualAngle, Vector3.up)) * dir;
Vector3 yAxis = root.segRef.ParentOffset*root.transform.parent.forward;
//Debug.Log("Constraining y");
Quaternion yConstrain = constrainAxis(root, yAxis, Vector3.forward, toChild, dirMinusXAxis, root.segRef.Min.y, root.segRef.Max.y, out actualAngle);
result = xConstrain * yConstrain;
} else {
result = xConstrain;
}
}
if (root.segRef.Twist) {
// Debug.Log("Constraining twist");
Vector3 toChild = root.transform.parent.rotation * root.segRef.originalDir;
Vector3 dir = (result * toChild).normalized;
float actualAngle;
//get legal twist
Quaternion zRot = constrainAxis(root, Vector3.right, Vector3.right, toChild, dir, root.segRef.Min.z, root.segRef.Max.z, out actualAngle);
//untwist by actual twist and reapply with legal twist
result = Quaternion.Inverse(Quaternion.AngleAxis(actualAngle, Vector3.right)) * result;
result = zRot * result;
}
}
root.transform.rotation = result*root.transform.rotation;
}
link--;
if (link < 0) link = links.Length - 2;
} else {
success = true;
}
}
if (success) {
for (int i = 0; i < links.Length; i++) {
goals[i] = links[i].transform.localRotation;
}
}
destroyLinks(links);
return success;
}
void OnEnable()
{
t = (IKTarget)target;
}
private IKLink point(IKLink link, IKTarget target)
{
return link;
}
public IKTiming(IKTarget target, float timing, bool grab = false)
{
Timing = Mathf.Clamp01(timing);
Target = target;
GrabTarget = grab;
}
