public FABRIKChain(FABRIKChain parent, List <FABRIKEffector> effectors, int layer)
{
FABRIKEffector endEffector = effectors[effectors.Count - 1];
this.isEndChain = endEffector.transform.childCount == 0;
// If the current endEffector has no children, then the chain is considered an "end chain"
// Add a NEW end effector that is offset from the CURRENT end effector's position ...
// ... where the offset reflects its positioning within the CURRENT end effector's bounding box
// e.g. Vector3.zero is centered in the box
if (this.isEndChain)
{
effectors.Add(CreateEndEffector(endEffector));
}
// Now that we have all effectors accounted for, calculate the length of each segment
for (int i = 1; i < effectors.Count; i++)
{
effectors[i - 1].Length = Vector3.Distance(effectors[i].transform.position, effectors[i - 1].transform.position);
}
this.effectors = new List <FABRIKEffector>(effectors);
this.layer = layer;
// Add this chain to the parent chain's children
if (parent != null)
{
this.parent = parent;
parent.children.Add(this);
}
}
/// <summary>
/// Performs a FABRIK inverse kinematic solve, modifying the FABRIKChain points array.
/// </summary>
public static void Solve(FABRIKChain chain,
int maxIterations = 8,
float solveDistance = 0.01f)
{
var start = chain.start;
var target = chain.target;
var sumLengths = chain.lengths.Query().Fold((sum, l) => sum + l);
var sqrDistToTarget = (target - start).sqrMagnitude;
if (sqrDistToTarget > sumLengths * sumLengths)
{
// Target out of reach, straighten towards it.
var toTarget = (target - start).normalized;
for (int i = 0; i + 1 < chain.points.Length; i++)
{
chain.points[i + 1] = chain.points[i] + toTarget * chain.lengths[i];
}
}
else
{
// Target within reach.
var iterations = 0;
var sqrDistToGoal = (chain.points[chain.points.Length - 1] - target).sqrMagnitude;
var solveDistanceSqr = solveDistance * solveDistance;
while (sqrDistToGoal > solveDistanceSqr && iterations++ < maxIterations)
{
BackwardSolve(chain);
ForwardSolve(chain);
sqrDistToGoal = (chain.points[chain.points.Length - 1] - target).sqrMagnitude;
}
}
}
private FABRIKChain LoadSystem(Transform transform, FABRIKChain parent = null, int layer = 0)
{
List <FABRIKEffector> effectors = new List <FABRIKEffector>();
// Use parent chain's end effector as our sub-base effector, e.g:
// [D]---[E]
// 1 / 2 1 = [A, B, C]
// [A]---[B]---[C] 2 = [C, D, E]
// \ 3 3 = [C, F, G]
// [F]---[G]
if (parent != null)
{
effectors.Add(parent.EndEffector);
}
// childCount > 1 is a new sub-base
// childCount = 0 is an end chain (added to our list below)
// childCount = 1 is continuation of chain
while (transform != null)
{
FABRIKEffector effector = transform.gameObject.GetComponent <FABRIKEffector>();
if (effector == null)
{
break;
}
effectors.Add(effector);
if (transform.childCount != 1)
{
break;
}
transform = transform.GetChild(0);
}
FABRIKChain chain = new FABRIKChain(parent, effectors, layer);
chains.Add(chain);
// Add to our end chain list if it is an end chain
if (chain.IsEndChain)
{
endChains.Add(transform.gameObject.name, chain);
}
// Else iterate over each of the end effector's children to create a new chain in the layer above
else
{
foreach (Transform child in transform)
{
LoadSystem(child, chain, layer + 1);
}
}
return(chain);
}
/// <summary>
/// Performs a single forward FABRIK solver iteration on the argument chain.
/// </summary>
public static void ForwardSolve(FABRIKChain chain)
{
// Move the first point to the start and project forward along chain.
chain.points[0] = chain.start;
for (int i = 0; i + 1 < chain.points.Length; i++)
{
var linkDir = (chain.points[i + 1] - chain.points[i]).normalized;
chain.points[i + 1] = chain.points[i] + linkDir * chain.lengths[i];
}
}
public override void OnFABRIK()
{
float speed = 100.0F;
float step = Time.deltaTime * speed;
FABRIKChain right = GetEndChain("mixamorig:RightHand_end_effector");
FABRIKChain left = GetEndChain("mixamorig:LeftHand_end_effector");
right.Target = Vector3.MoveTowards(right.EndEffector.Position, sphere_right.position, step);
left.Target = Vector3.MoveTowards(left.EndEffector.Position, sphere_left.position, step);
}
public void Start()
{
rootChain = CreateSystem(rootObject.transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return(y.layer.CompareTo(x.layer)); });
if (initialTarget != null)
{
endChains[0].Target = initialTarget.position;
}
}
public void Awake()
{
// Load our IK system from the root transform
rootChain = LoadSystem(transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return(y.Layer.CompareTo(x.Layer)); });
foreach (FABRIKChain chain in chains)
{
chain.CalculateSummedWeight();
}
}
// this can be expanded to build branching limbs and so return a list of chains.
// the root chain should be tracked, and in the update function each chain will need backward() called on it
private FABRIKChain buildLimb(Transform limbOrigin, FABRIKChain parentChain = null, int layer = 0)
{
List <FABRIKEffector> sections = new List <FABRIKEffector>(config.sectionCount + (parentChain == null ? 1 : 2));
if (parentChain != null)
{
sections.Add(parentChain.EndEffector);
}
buildLimbSection(config.sectionCount, limbOrigin, Vector3.zero, in sections);
return(new FABRIKChain(parentChain, sections, layer));
}
public void Awake()
{
rootObject = new GameObject("FABRIK-Root-" + transform.name);
rootObject.transform.position = transform.position;
rootObject.transform.rotation = transform.rotation;
transform.parent = rootObject.transform;
rootChain = CreateSystem(rootObject.transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return(y.layer.CompareTo(x.layer)); });
}
public void Awake()
{
rootObject = new GameObject("FABRIK-Root-" + transform.name);
rootObject.transform.position = transform.position;
rootObject.transform.rotation = transform.rotation;
transform.parent = rootObject.transform;
rootChain = CreateSystem(rootObject.transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return y.layer.CompareTo(x.layer); } );
}
public override void OnFABRIK()
{
float speed = 10.0F;
float step = Time.deltaTime * speed;
FABRIKChain up_left = GetEndChain("up_left");
FABRIKChain down_left = GetEndChain("down_left");
FABRIKChain up_right = GetEndChain("up_right");
FABRIKChain down_right = GetEndChain("down_right");
up_left.Target = Vector3.MoveTowards(up_left.EndEffector.Position, targetTransformA.position, step);
down_left.Target = Vector3.MoveTowards(down_left.EndEffector.Position, targetTransformB.position, step);
up_right.Target = Vector3.MoveTowards(up_right.EndEffector.Position, targetTransformC.position, step);
down_right.Target = Vector3.MoveTowards(down_right.EndEffector.Position, targetTransformD.position, step);
}
public FABRIKChain(int layer, FABRIKChain parent, params FABRIKEffector [] effectors)
{
this.layer = layer;
this.endEffector = effectors.Length - 1;
this.effectors.AddRange(effectors);
this.parent = parent;
if (parent != null)
{
parent.children.Add(this);
}
}
public override void OnFABRIK()
{
target.position = new Vector3(target.position.x, 0, target.position.z);
FABRIKChain end = GetEndChain("Cylinder (6)_end_effector");
end.Target = Vector3.MoveTowards(end.EndEffector.Position, target.position, speed * Time.deltaTime);
UpdateLinks();
//if (Input.GetKeyDown(KeyCode.P))
//{
// float dist = 0f;
// Debug.Log(TargetReachable(ref dist).ToString() + "__" + dist.ToString());
//}
}
public FABRIKChain(int layer, FABRIKChain parent, params FABRIKEffector [] effectors)
{
this.layer = layer;
this.endEffector = effectors.Length - 1;
this.effectors.AddRange(effectors);
this.parent = parent;
if(parent != null)
{
parent.children.Add(this);
}
}
private FABRIKChain CreateSystem(Transform transform, FABRIKChain parent = null, int layer = 0)
{
List <FABRIKEffector> effectors = new List <FABRIKEffector>();
FABRIKEffector effector = null;
// Use parent chain's end effector as our sub-base effector
if (parent != null)
{
effector = parent.EndEffector;
effectors.Add(effector);
}
// childCount > 1 is a new sub-base, childCount = 0 is an end chain (added to our list below), childCount = 1 is continuation of chain
while (transform)
{
effector = FABRIKEffector.FetchComponent(transform.gameObject).Constructor(effector);
effectors.Add(effector);
if (transform.childCount != 1)
{
break;
}
transform = transform.GetChild(0);
}
FABRIKChain chain = new FABRIKChain(layer, parent, effectors.ToArray());
chains.Add(chain);
if (transform.childCount == 0)
{
endChains.Add(chain);
}
else
{
foreach (Transform child in transform)
{
CreateSystem(child, chain, layer + 1);
}
}
return(chain);
}
public void Awake()
{
// Load our IK system from the root transform
rootChain = LoadSystem(transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return(y.Layer.CompareTo(x.Layer)); });
foreach (FABRIKChain chain in chains)
{
chain.CalculateSummedWeight();
Debug.Log($"chain.EndEffector {chain.EndEffector} chain.BaseEffector {chain.BaseEffector}");
}
Debug.Log($"rootChain.EndEffector {rootChain.EndEffector} rootChain.BaseEffector {rootChain.BaseEffector}");
Debug.Log(endChains.Count);
}
public FABRIKChain(int layer, FABRIKChain parent, bool constrain, float constrainAngle, params FABRIKEffector[] effectors)
{
this.layer = layer;
this.Constrain = constrain;
this.ConstrainAngle = constrainAngle;
this.endEffector = effectors.Length - 1;
this.effectors.AddRange(effectors);
this.parent = parent;
if (parent != null)
{
parent.children.Add(this);
}
}
/// <summary>
/// Performs a single backwards FABRIK solver iteration on the argument chain.
/// </summary>
public static void BackwardSolve(FABRIKChain chain, Vector3?warmStartDir = null)
{
// Move the last point to the target and project backwards along chain.
chain.points[chain.points.Length - 1] = chain.target;
for (int i = chain.points.Length - 1; i - 1 >= 0; i--)
{
Vector3 linkDir;
if (warmStartDir.HasValue && i == chain.points.Length - 1)
{
linkDir = warmStartDir.Value.normalized;
}
else
{
linkDir = (chain.points[i - 1] - chain.points[i]).normalized;
}
chain.points[i - 1] = chain.points[i] + linkDir * chain.lengths[i - 1];
}
}
private FABRIKChain CreateSystem(Transform transform, FABRIKChain parent = null, int layer = 0)
{
List<FABRIKEffector> effectors = new List<FABRIKEffector>();
FABRIKEffector effector = null;
// Use parent chain's end effector as our sub-base effector
if(parent != null)
{
effector = parent.EndEffector;
effectors.Add(effector);
}
// childCount > 1 is a new sub-base, childCount = 0 is an end chain (added to our list below), childCount = 1 is continuation of chain
while(transform)
{
effector = FABRIKEffector.FetchComponent(transform.gameObject).Constructor(effector);
effectors.Add(effector);
if(transform.childCount != 1)
{
break;
}
transform = transform.GetChild(0);
}
FABRIKChain chain = new FABRIKChain(layer, parent, effectors.ToArray());
chains.Add(chain);
if(transform.childCount == 0)
{
endChains.Add(chain);
}
else foreach(Transform child in transform)
{
CreateSystem(child, chain, layer + 1);
}
return chain;
}
public FABRIKChain(FABRIKChain parent, List <FABRIKEffector> effectors, int layer)
{
for (int i = 1; i < effectors.Count; i++)
{
effectors[i - 1].Length = Vector3.Distance(effectors[i].transform.position, effectors[i - 1].transform.position);
}
this.effectors = new List <FABRIKEffector>(effectors);
this.layer = layer;
// Add this chain to the parent chain's children
if (parent != null)
{
this.parent = parent;
parent.children.Add(this);
}
}
public void Awake()
{
Quaternion q = Quaternion.LookRotation(Vector3.right, Vector3.up);
Debug.Log("Forward = " + (q * Vector3.forward));
Debug.Log("Right = " + (q * Vector3.right));
Debug.Log("Up = " + (q * Vector3.up));
// Load our IK system from the root transform
rootChain = LoadSystem(transform);
// Inversely sort by layer, greater-first
chains.Sort(delegate(FABRIKChain x, FABRIKChain y) { return(y.Layer.CompareTo(x.Layer)); });
foreach (var key in endChains)
{
Debug.Log(key);
}
}
private void Awake()
{
limbChain = buildLimb(transform);
length = config.sectionCount * config.limbSectionPrefab.childOffset.z;
}
