public IKLink(IKSegment reference)
{
GameObject go = new GameObject();
transform = go.transform;
go.name = "Spoof"+ reference.name;
go.transform.localScale = reference.transform.localScale;
go.transform.rotation = reference.transform.rotation;
go.transform.position = reference.transform.position;
segRef = reference;
}
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;
}
private static IKLink[] createLinks(IKSegment[] segments, Transform effector, out Transform contact)
{
IKLink[] links = new IKLink[segments.Length];
IKLink previous = null;
for(int i = 0; i < links.Length; i++) {
links[i] = new IKLink(segments[i]);
if(previous != null) {
//Debug.LogFormat("from {0} to {1}", previous.transform.name, links[i].transform.name);
connectTransform(previous.segRef.transform, links[i].segRef.transform, links[i].transform, previous.transform);
} else {
links[i].transform.SetParent(segments[0].transform.parent);
}
previous = links[i];
}
GameObject go = new GameObject();
go.name = "Effector";
go.transform.SetParent(links[links.Length - 1].transform);
go.transform.position = effector.position;
go.transform.rotation = effector.rotation;
go.transform.localScale = effector.localScale;
contact = go.transform;
return links;
}
private void addToChain(ref List<IKSegment> chain, IKSegment segment)
{
if (segment != null) chain.Add(segment);
}
IEnumerator ScheduleIK(IKSegment segment, float reachDuration = 1f, float holdDuration = 1f, float revertDuration = 1f, float delay = 0f)
{
yield return new WaitForSeconds(delay);
int process = segment.StartIK();
float elapsed = 0f;
float easeIn = (segment.EaseIn != 0 ? segment.EaseIn : 1f);
float easeOut = segment.EaseOut;
while (process == segment.CurrentIK() && elapsed <= reachDuration) {
float t = IKMath.Catmull(elapsed / reachDuration, easeIn, 0, 1, easeOut);
segment.RotateStep(t);
yield return new WaitForEndOfFrame();
elapsed += Time.deltaTime;
}
elapsed = 0f;
while(process == segment.CurrentIK() && elapsed <= holdDuration) {
segment.RotateStep(1f);
yield return new WaitForEndOfFrame();
elapsed += Time.deltaTime;
}
//yield return new WaitForSeconds(segment.LagFactor * revertDuration);
elapsed = 0f;
if (revertDuration > 0) {
while (process == segment.CurrentIK() && elapsed <= revertDuration) {
float t = IKMath.Catmull(elapsed / revertDuration, easeIn, 0, 1, easeOut);
segment.RotateStep(1 - t, reverting: true);
yield return new WaitForEndOfFrame();
elapsed += Time.deltaTime;
}
}
if (process == segment.CurrentIK()) segment.StopIK();
}
IEnumerator ScheduleIK(IKSegment[] chain, IKTiming[] timings, List<Quaternion[]> rotations, float duration)
{
float lastTiming = 0;
for(int i = 0; i < timings.Length; i++) {
float revert = 0f;
float reach = (timings[i].Timing - lastTiming) * duration;
if (i == timings.Length - 1) {
revert = (1 - timings[i].Timing) * duration;
}
for (int seg = 0; seg < chain.Length; seg++) {
chain[seg].SetTargetRotation(rotations[i][seg]);
StartCoroutine(ScheduleIK(chain[seg], reach, 0, revert, 0));
}
lastTiming = timings[i].Timing;
yield return new WaitForSeconds(reach);
if(timings[i].GrabTarget) {
chain[chain.Length - 1].Grab(timings[i].Target);
}
}
}
void OnEnable()
{
seg = (IKSegment)target;
type = serializedObject.FindProperty("JointType");
coneRad = serializedObject.FindProperty("ConeRadius");
damping = serializedObject.FindProperty("DampDegrees");
easeIn = serializedObject.FindProperty("EaseIn");
easeOut = serializedObject.FindProperty("EaseOut");
end = serializedObject.FindProperty("End");
}