FABRIK

FABRIK.cs

using UnityEngine;
using System.Collections.Generic;
using UnityEngine.Assertions;

[ExecuteInEditMode]

public class FABRIK_unsafe : MonoBehaviour {
    public int ChainLength;
	// Target should not be child of root bone
    public Transform Target;
	public bool CopyTargetTransform;
	private List<Transform> bones = new List<Transform>();
	private List<Vector3> tempPos = new List<Vector3>();
	private List<Vector3> tempLocPos = new List<Vector3>();
	private List<Quaternion> tempRot = new List<Quaternion>();
	private List<Quaternion> tempLocRot = new List<Quaternion>();
	private List<float> constraints = new List<float>();

	// Use this for initialization
	void Start () {
		IKInit();		
	}
	void Update(){
	}
	
	// Update is called once per frame
	void LateUpdate () {
		IKRestore();
		Debug.Log("IPOS "+bones[1].position);
		FABRIK_Solver();
		IKStore();
		if(Target.rotation != bones[0].rotation) Debug.Log("Rotation does not match");
	}

	private int maxiteration = 30;
	private Transform root;
	private float reach = 0;
	void IKInit(){
		Transform trns;
		float dist;
		trns = gameObject.transform;
		if(Target == null) Target = trns;
		for(int i = 0; i < ChainLength; i++) {
			bones.Add(trns);
			tempPos.Add(trns.position);
			tempLocPos.Add(trns.localPosition);
			tempRot.Add(trns.rotation);
			tempLocRot.Add(trns.localRotation);
			Debug.Log("Bone Loaded : " + trns.name);
			if(trns.parent!=null) trns = trns.parent;
			dist = Vector3.Distance(bones[i].position, trns.position);
			constraints.Add(dist);
			reach += dist;
		}
		Debug.Log("Copy Rot "+ CopyTargetTransform);
	}
	void IKRestore(){
		for(int i = 0; i < bones.Count; i++) {
			bones[i].position = tempPos[i];
			bones[i].localPosition = tempLocPos[i];
			bones[i].rotation = tempRot[i];
			bones[i].localRotation = tempLocRot[i];
		}
	}

	void IKStore(){
		for(int i = 0; i < bones.Count; i++) {
			tempPos[i] = bones[i].position;
			tempLocPos[i] = bones[i].localPosition;
			tempRot[i] = bones[i].rotation;
			tempLocRot[i] = bones[i].localRotation;
		}
	}
	private float targetDist;
	private float lambda;
	//private Transform b;
	private Vector3 b;
	private float rotAngle;
	private Vector3 rotAxis;

	/// <summary> Forward And Backward Reaching Inverse Kinematics (FABRIK) </summary>
	void FABRIK_Solver() {
		bones[0].position = Target.position;
		targetDist = Vector3.Distance(bones[bones.Count-1].position, Target.position);
		if(targetDist >= reach) {
			// Target is out of reach
			Debug.LogWarning("Target is out of reach");
			for(int i = bones.Count - 1 ; i > 0 ; i--) {
				lambda = constraints[i - 1] / (Vector3.Distance(Target.position, bones[i].position));
				bones[i - 1].position = (1 - lambda)*bones[i].position + lambda*Target.position;
			}
		} else {
			// Target is within reach
			b = bones[bones.Count - 1].position;
			/* Forward reaching */
			// Set the end effector as target
			bones[0].position = Target.position;
			for(int i = 0; i < bones.Count - 1 ; i++) {
				lambda = constraints[i] / Vector3.Distance(bones[i+1].position, bones[i].position);
				bones[i + 1].position = (1 - lambda)*bones[i].position + lambda*bones[i + 1].position;
			}
			/* Backward reaching */
			bones[bones.Count - 1].position = b;
			for(int i = bones.Count - 1; i > 0 ; i--) {
				lambda = constraints[i-1] / Vector3.Distance(bones[i].position, bones[i-1].position);
				bones[i - 1].position = (1-lambda) * bones[i].position + lambda*bones[i - 1].position;
			}
		}
		if(bones.Count > 1) {
			// Fix rotation
			if(CopyTargetTransform) {
				bones[0].rotation = Target.transform.rotation;
			}
			for(int i = 1; i < bones.Count ; i++) {
				rotAngle = Vector3.Angle(bones[i].up, bones[i-1].position - bones[i].position);
				if(rotAngle > 1E-06f) {
					rotAxis = Vector3.Cross(bones[i].up, bones[i-1].position - bones[i].position);
					bones[i].RotateAround(bones[i].position, rotAxis, rotAngle);
					bones[i-1].RotateAround(bones[i].position, rotAxis, -rotAngle);
				}
			}
		}
	}
}

calculation results become unexpected values with animator attached

Added functions to keep position and rotation values between frames, which solved the problem.