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; }