public override void Execute()
{
LeftArm = Solver.leftArmChain;
RightArm = Solver.rightArmChain;
LeftLeg = Solver.leftLegChain;
RightLeg = Solver.rightLegChain;
}
public void Initiate(IKSolverFullBody solver)
{
this.chain1 = solver.GetChain(this.bone1);
this.chain2 = solver.GetChain(this.bone2);
this.node1 = this.chain1.nodes[0];
this.node2 = this.chain2.nodes[0];
this.OnPreSolve();
}
/*
* Initiating the constraint
* */
public void Initiate(IKSolverFullBody solver) {
chain1 = solver.GetChain(bone1);
chain2 = solver.GetChain(bone2);
node1 = chain1.nodes[0];
node2 = chain2.nodes[0];
OnPreSolve();
}
/*
* Initiating the constraint
* */
public void Initiate(IKSolverFullBody solver)
{
chain1 = solver.GetChain(bone1);
chain2 = solver.GetChain(bone2);
node1 = chain1.nodes[0];
node2 = chain2.nodes[0];
OnPreSolve();
}
public static void AddChain(FBIKChain[] chain, int index, Color color, float size) {
Handles.color = color;
for (int i = 0; i < chain[index].nodes.Length - 1; i++) {
Handles.DrawLine(GetNodePosition(chain[index].nodes[i]), GetNodePosition(chain[index].nodes[i + 1]));
Handles.SphereCap(0, GetNodePosition(chain[index].nodes[i]), Quaternion.identity, size);
}
Handles.SphereCap(0, GetNodePosition(chain[index].nodes[chain[index].nodes.Length - 1]), Quaternion.identity, size);
for (int i = 0; i < chain[index].children.Length; i++) {
Handles.DrawLine(GetNodePosition(chain[index].nodes[chain[index].nodes.Length - 1]), GetNodePosition(chain[chain[index].children[i]].nodes[0]));
}
}
private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset)
{
Vector3 a = effector.position - (arm.nodes[0].transform.position + offset);
float num = arm.nodes[0].length + arm.nodes[1].length;
float magnitude = a.magnitude;
if (magnitude < num)
{
return(Vector3.zero);
}
float d = magnitude - num;
return(a / magnitude * d);
}
public static void AddChain(FBIKChain chain, Color color, float size)
{
Handles.color = color;
for (int i = 0; i < chain.nodes.Length - 1; i++) {
Handles.DrawLine(GetNodePosition(chain.nodes[i]), GetNodePosition(chain.nodes[i + 1]));
Handles.SphereCap(0, GetNodePosition(chain.nodes[i]), Quaternion.identity, size);
}
Handles.SphereCap(0, GetNodePosition(chain.nodes[chain.nodes.Length - 1]), Quaternion.identity, size);
foreach (FBIKChain c in chain.children) {
Handles.DrawLine(GetNodePosition(chain.nodes[chain.nodes.Length - 1]), GetNodePosition(c.nodes[0]));
AddChain(c, color, size);
}
}
/*
* Scene view handles to help with limb setup
* */
private static void AddLimbHelper(FBIKChain chain, float size, Transform root = null)
{
Vector3 cross = Vector3.Cross((chain.nodes[1].transform.position - chain.nodes[0].transform.position).normalized, (chain.nodes[2].transform.position - chain.nodes[0].transform.position).normalized);
Vector3 bendDirection = -Vector3.Cross(cross.normalized, (chain.nodes[2].transform.position - chain.nodes[0].transform.position).normalized);
if (bendDirection != Vector3.zero)
{
Color c = Handles.color;
bool inverted = root != null && Vector3.Dot(root.forward, bendDirection.normalized) < 0f;
// Inverted bend direction
if (inverted)
{
GUI.color = new Color(1f, 0.75f, 0.75f);
Handles.color = Color.yellow;
if (Handles.Button(chain.nodes[1].transform.position, Quaternion.identity, size * 0.5f, size, Handles.DotCap))
{
Warning.logged = false;
Warning.Log("The bend direction of this limb appears to be inverted. Please rotate this bone so that the limb is bent in it's natural bending direction. If this limb is supposed to be bent in the direction pointed by the arrow, ignore this warning.", root, true);
}
}
Handles.ArrowCap(0, chain.nodes[1].transform.position, Quaternion.LookRotation(bendDirection), size * 2f);
GUI.color = Color.white;
Handles.color = c;
}
else
{
// The limb is completely stretched out
Color c = Handles.color;
Handles.color = Color.red;
GUI.color = new Color(1f, 0.75f, 0.75f);
if (Handles.Button(chain.nodes[1].transform.position, Quaternion.identity, size * 0.5f, size, Handles.DotCap))
{
Warning.logged = false;
Warning.Log("The limb is completely stretched out. Full Body Biped IK does not know which way the limb should be bent. Please rotate this bone slightly in it's bending direction.", root, true);
}
GUI.color = Color.white;
Handles.color = c;
}
}
/*
* Get pull offset of a hand
* */
private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset)
{
// Get the vector from shoulder to hand effector
Vector3 direction = effector.position - (arm.nodes[0].transform.position + offset);
float armLength = arm.nodes[0].length + arm.nodes[1].length;
// Find delta of effector distance and arm length
float dirMag = direction.magnitude;
if (dirMag < armLength)
{
return(Vector3.zero);
}
float x = dirMag - armLength;
return((direction / dirMag) * x);
}
public static void AddChain(FBIKChain chain, Color color, float size)
{
Handles.color = color;
for (int i = 0; i < chain.nodes.Length - 1; i++)
{
Handles.DrawLine(GetNodePosition(chain.nodes[i]), GetNodePosition(chain.nodes[i + 1]));
Handles.SphereCap(0, GetNodePosition(chain.nodes[i]), Quaternion.identity, size);
}
Handles.SphereCap(0, GetNodePosition(chain.nodes[chain.nodes.Length - 1]), Quaternion.identity, size);
foreach (FBIKChain c in chain.children)
{
Handles.DrawLine(GetNodePosition(chain.nodes[chain.nodes.Length - 1]), GetNodePosition(c.nodes[0]));
AddChain(c, color, size);
}
}
// Scene view handles to help with limb setup
private static void AddLimbHelper(FBIKChain chain, float size, Transform root = null) {
Vector3 cross = Vector3.Cross((chain.nodes[1].transform.position - chain.nodes[0].transform.position).normalized, (chain.nodes[2].transform.position - chain.nodes[0].transform.position).normalized);
Vector3 bendDirection = -Vector3.Cross(cross.normalized, (chain.nodes[2].transform.position - chain.nodes[0].transform.position).normalized);
if (bendDirection != Vector3.zero) {
Color c = Handles.color;
bool inverted = root != null && Vector3.Dot(root.forward, bendDirection.normalized) < 0f;
// Inverted bend direction
if (inverted) {
GUI.color = new Color(1f, 0.75f, 0.75f);
Handles.color = Color.yellow;
if (Handles.Button(chain.nodes[1].transform.position, Quaternion.identity, size * 0.5f, size, Handles.DotCap)) {
Warning.logged = false;
Warning.Log("The bend direction of this limb appears to be inverted. Please rotate this bone so that the limb is bent in it's natural bending direction. If this limb is supposed to be bent in the direction pointed by the arrow, ignore this warning.", root, true);
}
}
Handles.ArrowCap(0, chain.nodes[1].transform.position, Quaternion.LookRotation(bendDirection), size * 2f);
GUI.color = Color.white;
Handles.color = c;
} else {
// The limb is completely stretched out
Color c = Handles.color;
Handles.color = Color.red;
GUI.color = new Color(1f, 0.75f, 0.75f);
if (Handles.Button(chain.nodes[1].transform.position, Quaternion.identity, size * 0.5f, size, Handles.DotCap)) {
Warning.logged = false;
Warning.Log("The limb is completely stretched out. Full Body Biped IK does not know which way the limb should be bent. Please rotate this bone slightly in it's bending direction.", root, true);
}
GUI.color = Color.white;
Handles.color = c;
}
}
/*
* Before updating the chain
* */
public void ReadPose(FBIKChain[] chain)
{
if (!initiated) return;
for (int i = 0; i < nodes.Length; i++) {
nodes[i].solverPosition = nodes[i].transform.position + nodes[i].offset;
}
// Calculating bone lengths
for (int i = 0; i < nodes.Length - 1; i++) {
nodes[i].length = Vector3.Distance(nodes[i].transform.position, nodes[i + 1].transform.position);
}
for (int i = 0; i < children.Length; i++) {
chain[children[i]].rootLength = (chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
}
// Pre-update child constraints
PreSolveConstraints();
}
/*
* End-effectors pushing the first nodes
* */
public Vector3 Push(FBIKChain[] chain) {
Vector3 sum = Vector3.zero;
// Get the push from the children
for (int i = 0; i < children.Length; i++) {
sum += chain[children[i]].Push(chain) * chain[children[i]].pushParent;
}
// Apply the push from a child
nodes[nodes.Length - 1].solverPosition += sum;
// Calculating the push of THIS chain (passed on to the parent as we're in a recursive method)
if (nodes.Length < 2) return Vector3.zero;
if (push <= 0f) return Vector3.zero;
Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
float solverLength = solverDirection.magnitude;
if (solverLength == 0f) return Vector3.zero;
// Get the push force factor
float f = 1f - (solverLength / distance);
if (f <= 0f) return Vector3.zero;
// Push smoothing
switch (pushSmoothing) {
case Smoothing.Exponential:
f *= f;
break;
case Smoothing.Cubic:
f *= f * f;
break;
}
// The final push force
Vector3 p = -solverDirection * f * push;
nodes[0].solverPosition += p;
return p;
}
/// <summary>
/// Sets up the solver to BipedReferences and reinitiates (if in runtime).
/// </summary>
/// <param name="references">Biped references.</param>
/// <param name="rootNode">Root node (optional). if null, will try to detect the root node bone automatically. </param>
public void SetToReferences(BipedReferences references, Transform rootNode = null)
{
if (rootNode == null)
{
rootNode = DetectRootNodeBone(references);
}
this.rootNode = rootNode;
// Root Node
if (chain == null)
{
chain = new FBIKChain();
}
chain.pin = 0f;
chain.SetNodes(rootNode);
if (chain.children.Length != 4)
{
chain.children = new FBIKChain[4];
}
// Left Arm
if (chain.children[0] == null)
{
chain.children[0] = new FBIKChain();
chain.children[0].reach = 0.05f;
}
chain.children[0].SetNodes(references.leftUpperArm, references.leftForearm, references.leftHand);
// Right Arm
if (chain.children[1] == null)
{
chain.children[1] = new FBIKChain();
chain.children[1].reach = 0.05f;
}
chain.children[1].SetNodes(references.rightUpperArm, references.rightForearm, references.rightHand);
// Left Leg
if (chain.children[2] == null)
{
chain.children[2] = new FBIKChain();
chain.children[2].reach = 0.05f;
}
chain.children[2].SetNodes(references.leftThigh, references.leftCalf, references.leftFoot);
// Right Leg
if (chain.children[3] == null)
{
chain.children[3] = new FBIKChain();
chain.children[3].reach = 0.05f;
}
chain.children[3].SetNodes(references.rightThigh, references.rightCalf, references.rightFoot);
// Effectors
if (effectors.Length != 9)
{
effectors = new IKEffector[9] {
new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector()
}
}
;
effectors[0].bone = rootNode;
effectors[0].childBones = new Transform[2] {
references.leftThigh, references.rightThigh
};
effectors[1].bone = references.leftUpperArm;
effectors[2].bone = references.rightUpperArm;
effectors[3].bone = references.leftThigh;
effectors[4].bone = references.rightThigh;
effectors[5].bone = references.leftHand;
effectors[6].bone = references.rightHand;
effectors[7].bone = references.leftFoot;
effectors[8].bone = references.rightFoot;
effectors[5].planeBone1 = references.leftUpperArm;
effectors[5].planeBone2 = references.rightUpperArm;
effectors[5].planeBone3 = rootNode;
effectors[6].planeBone1 = references.rightUpperArm;
effectors[6].planeBone2 = references.leftUpperArm;
effectors[6].planeBone3 = rootNode;
effectors[7].planeBone1 = references.leftThigh;
effectors[7].planeBone2 = references.rightThigh;
effectors[7].planeBone3 = rootNode;
effectors[7].mode = IKEffector.Mode.MaintainAnimatedPosition;
effectors[8].planeBone1 = references.rightThigh;
effectors[8].planeBone2 = references.leftThigh;
effectors[8].planeBone3 = rootNode;
effectors[8].mode = IKEffector.Mode.MaintainAnimatedPosition;
effectors[5].isEndEffector = true;
effectors[6].isEndEffector = true;
effectors[7].isEndEffector = true;
effectors[8].isEndEffector = true;
// Child Constraints
chain.childConstraints = new FBIKChain.ChildConstraint[4] {
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightThigh, 0f, 1f),
new FBIKChain.ChildConstraint(references.rightUpperArm, references.leftThigh, 0f, 1f),
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightUpperArm),
new FBIKChain.ChildConstraint(references.leftThigh, references.rightThigh)
};
// Bend Constraints
if (bendConstraints.Length != 4)
{
bendConstraints = new IKConstraintBend[4] {
new IKConstraintBend(), new IKConstraintBend(), new IKConstraintBend(), new IKConstraintBend()
};
}
bendConstraints[0].SetBones(references.leftUpperArm, references.leftForearm, references.leftHand);
bendConstraints[1].SetBones(references.rightUpperArm, references.rightForearm, references.rightHand);
bendConstraints[2].SetBones(references.leftThigh, references.leftCalf, references.leftFoot);
bendConstraints[3].SetBones(references.rightThigh, references.rightCalf, references.rightFoot);
// IKMappingSpine
Transform[] spineBones = new Transform[references.spine.Length + 1];
spineBones[0] = references.pelvis;
for (int i = 0; i < references.spine.Length; i++)
{
spineBones[i + 1] = references.spine[i];
}
if (spineMapping == null)
{
spineMapping = new IKMappingSpine();
spineMapping.iterations = 3;
}
spineMapping.SetBones(spineBones, references.leftUpperArm, references.rightUpperArm, references.leftThigh, references.rightThigh);
// IKMappingBone
int boneMappingsCount = references.head != null? 1: 0;
if (boneMappings.Length != boneMappingsCount)
{
boneMappings = new IKMappingBone[boneMappingsCount];
for (int i = 0; i < boneMappings.Length; i++)
{
boneMappings[i] = new IKMappingBone();
}
if (boneMappingsCount == 1)
{
boneMappings[0].maintainRotationWeight = 0f;
}
}
if (boneMappings.Length > 0)
{
boneMappings[0].bone = references.head;
}
// IKMappingLimb
if (limbMappings.Length != 4)
{
limbMappings = new IKMappingLimb[4] {
new IKMappingLimb(), new IKMappingLimb(), new IKMappingLimb(), new IKMappingLimb()
};
limbMappings[2].maintainRotationWeight = 1f;
limbMappings[3].maintainRotationWeight = 1f;
}
limbMappings[0].SetBones(references.leftUpperArm, references.leftForearm, references.leftHand, GetLeftClavicle(references));
limbMappings[1].SetBones(references.rightUpperArm, references.rightForearm, references.rightHand, GetRightClavicle(references));
limbMappings[2].SetBones(references.leftThigh, references.leftCalf, references.leftFoot);
limbMappings[3].SetBones(references.rightThigh, references.rightCalf, references.rightFoot);
if (Application.isPlaying)
{
Initiate(references.root);
}
}
/// <summary>
/// Sets up the solver to BipedReferences and reinitiates (if in runtime).
/// </summary>
/// <param name="references">Biped references.</param>
/// <param name="rootNode">Root node (optional). if null, will try to detect the root node bone automatically. </param>
public void SetToReferences(BipedReferences references, Transform rootNode = null) {
root = references.root;
if (rootNode == null) rootNode = DetectRootNodeBone(references);
this.rootNode = rootNode;
// Root Node
if (chain == null || chain.Length != 5) chain = new FBIKChain[5];
for (int i = 0; i < chain.Length; i++) {
if (chain[i] == null) {
chain[i] = new FBIKChain();
}
}
chain[0].pin = 0f;
chain[0].SetNodes(rootNode);
chain[0].children = new int[4] { 1, 2, 3, 4 };
// Left Arm
chain[1].SetNodes(references.leftUpperArm, references.leftForearm, references.leftHand);
// Right Arm
chain[2].SetNodes(references.rightUpperArm, references.rightForearm, references.rightHand);
// Left Leg
chain[3].SetNodes(references.leftThigh, references.leftCalf, references.leftFoot);
// Right Leg
chain[4].SetNodes(references.rightThigh, references.rightCalf, references.rightFoot);
// Effectors
if (effectors.Length != 9) effectors = new IKEffector[9] {
new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector(), new IKEffector()
};
effectors[0].bone = rootNode;
effectors[0].childBones = new Transform[2] { references.leftThigh, references.rightThigh };
effectors[1].bone = references.leftUpperArm;
effectors[2].bone = references.rightUpperArm;
effectors[3].bone = references.leftThigh;
effectors[4].bone = references.rightThigh;
effectors[5].bone = references.leftHand;
effectors[6].bone = references.rightHand;
effectors[7].bone = references.leftFoot;
effectors[8].bone = references.rightFoot;
effectors[5].planeBone1 = references.leftUpperArm;
effectors[5].planeBone2 = references.rightUpperArm;
effectors[5].planeBone3 = rootNode;
effectors[6].planeBone1 = references.rightUpperArm;
effectors[6].planeBone2 = references.leftUpperArm;
effectors[6].planeBone3 = rootNode;
effectors[7].planeBone1 = references.leftThigh;
effectors[7].planeBone2 = references.rightThigh;
effectors[7].planeBone3 = rootNode;
effectors[8].planeBone1 = references.rightThigh;
effectors[8].planeBone2 = references.leftThigh;
effectors[8].planeBone3 = rootNode;
// Child Constraints
chain[0].childConstraints = new FBIKChain.ChildConstraint[4] {
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightThigh, 0f, 1f),
new FBIKChain.ChildConstraint(references.rightUpperArm, references.leftThigh, 0f, 1f),
new FBIKChain.ChildConstraint(references.leftUpperArm, references.rightUpperArm),
new FBIKChain.ChildConstraint(references.leftThigh, references.rightThigh)
};
// IKMappingSpine
Transform[] spineBones = new Transform[references.spine.Length + 1];
spineBones[0] = references.pelvis;
for (int i = 0; i < references.spine.Length; i++) {
spineBones[i + 1] = references.spine[i];
}
if (spineMapping == null) {
spineMapping = new IKMappingSpine();
spineMapping.iterations = 3;
}
spineMapping.SetBones(spineBones, references.leftUpperArm, references.rightUpperArm, references.leftThigh, references.rightThigh);
// IKMappingBone
int boneMappingsCount = references.head != null? 1: 0;
if (boneMappings.Length != boneMappingsCount) {
boneMappings = new IKMappingBone[boneMappingsCount];
for (int i = 0; i < boneMappings.Length; i++) {
boneMappings[i] = new IKMappingBone();
}
if (boneMappingsCount == 1) boneMappings[0].maintainRotationWeight = 0f;
}
if (boneMappings.Length > 0) boneMappings[0].bone = references.head;
// IKMappingLimb
if (limbMappings.Length != 4) {
limbMappings = new IKMappingLimb[4] {
new IKMappingLimb(), new IKMappingLimb(), new IKMappingLimb(), new IKMappingLimb()
};
limbMappings[2].maintainRotationWeight = 1f;
limbMappings[3].maintainRotationWeight = 1f;
}
limbMappings[0].SetBones(references.leftUpperArm, references.leftForearm, references.leftHand, GetLeftClavicle(references));
limbMappings[1].SetBones(references.rightUpperArm, references.rightForearm, references.rightHand, GetRightClavicle(references));
limbMappings[2].SetBones(references.leftThigh, references.leftCalf, references.leftFoot);
limbMappings[3].SetBones(references.rightThigh, references.rightCalf, references.rightFoot);
if (Application.isPlaying) Initiate(references.root);
}
/*
* Get pull offset of a hand
* */
private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset) {
// Get the vector from shoulder to hand effector
Vector3 direction = effector.position - (arm.nodes[0].transform.position + offset);
float armLength = arm.nodes[0].length + arm.nodes[1].length;
// Find delta of effector distance and arm length
float dirMag = direction.magnitude;
if (dirMag < armLength) return Vector3.zero;
float x = dirMag - armLength;
return (direction / dirMag) * x;
}
/*
* Stage 1 of the FABRIK algorithm
* */
public void Stage1(FBIKChain[] chain)
{
// Stage 1
for (int i = 0; i < children.Length; i++) chain[children[i]].Stage1(chain);
// If is the last chain in this hierarchy, solve immediatelly and return
if (children.Length == 0) {
ForwardReach(nodes[nodes.Length - 1].solverPosition);
return;
}
// Finding the total pull force by all child chains
float pullParentSum = 0f;
for (int i = 0; i < children.Length; i++) pullParentSum += chain[children[i]].pull;
Vector3 centroid = nodes[nodes.Length - 1].solverPosition;
// Satisfying child constraints
SolveChildConstraints();
// Finding the centroid position of all child chains according to their individual pull weights
for (int i = 0; i < children.Length; i++) {
Vector3 childPosition = chain[children[i]].nodes[0].solverPosition;
if (chain[children[i]].rootLength > 0) {
childPosition = IKSolverFABRIK.SolveJoint(nodes[nodes.Length - 1].solverPosition, chain[children[i]].nodes[0].solverPosition, chain[children[i]].rootLength);
}
if (pullParentSum > 0) centroid += (childPosition - nodes[nodes.Length - 1].solverPosition) * (chain[children[i]].pull / Mathf.Clamp(pullParentSum, 1f, Mathf.Infinity));
}
// Forward reach to the centroid (unless pinned)
ForwardReach(Vector3.Lerp(centroid, nodes[nodes.Length - 1].solverPosition, pin));
}
/*
* Stage 2 of the FABRIK algorithm.
* */
public void Stage2(Vector3 position, int iterations, FBIKChain[] chain)
{
// Stage 2
BackwardReach(position);
// Iterating child constraints and child chains to make sure they are not conflicting
for (int i = 0; i < Mathf.Min(iterations, 4); i++) SolveConstraintSystems(chain);
// Stage 2 for the children
for (int i = 0; i < children.Length; i++) chain[children[i]].Stage2(nodes[nodes.Length - 1].solverPosition, iterations, chain);
}
/*
* Reaching limbs
* */
public void Reach(FBIKChain[] chain) {
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) chain[children[i]].Reach(chain);
if (reachForce <= 0f) return;
Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
if (solverDirection == Vector3.zero) return;
float solverLength = solverDirection.magnitude;
//Reaching
Vector3 straight = (solverDirection / solverLength) * length;
float delta = Mathf.Clamp(solverLength / length, 1 - reachForce, 1 + reachForce) - 1f;
delta = Mathf.Clamp(delta + reachForce, -1f, 1f);
// Smoothing the effect of Reach with the expense of some accuracy
switch (reachSmoothing) {
case Smoothing.Exponential:
delta *= delta;
break;
case Smoothing.Cubic:
delta *= delta * delta;
break;
}
Vector3 offset = straight * Mathf.Clamp(delta, 0f, solverLength);
nodes[0].solverPosition += offset * (1f - nodes[0].effectorPositionWeight);
nodes[2].solverPosition += offset;
}
/*
* Applying trigonometric IK solver on the 3 segmented chains to relieve tension from the solver and increase accuracy.
* */
public void SolveTrigonometric(FBIKChain[] chain)
{
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) chain[children[i]].SolveTrigonometric(chain);
if (nodes.Length != 3) return;
float limbLength = nodes[0].length + nodes[1].length;
// Trigonometry
Vector3 limbDirection = nodes[2].solverPosition - nodes[0].solverPosition;
if (limbDirection == Vector3.zero) return;
float limbMag = limbDirection.magnitude;
float maxMag = Mathf.Clamp(limbMag, 0f, limbLength * 0.999f);
Vector3 direction = (limbDirection / limbMag) * maxMag;
Vector3 bendDirection = GetBendDirection(direction, maxMag, nodes[0], nodes[1]);
nodes[1].solverPosition = nodes[0].solverPosition + bendDirection;
}
/*
* Reaching limbs
* */
public void Reach(int iteration, FBIKChain[] chain)
{
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) chain[children[i]].Reach(iteration, chain);
if (nodes.Length != 3) return;
float r = reach * Mathf.Clamp(nodes[2].effectorPositionWeight, 0f, 1f);
if (r > 0) {
float limbLength = nodes[0].length + nodes[1].length;
Vector3 limbDirection = nodes[2].solverPosition - nodes[0].solverPosition;
if (limbDirection == Vector3.zero) return;
float currentLength = limbDirection.magnitude;
//Reaching
Vector3 straight = (limbDirection / currentLength) * limbLength;
float delta = currentLength / limbLength;
delta = Mathf.Clamp(delta, 1 - r, 1 + r);
delta -= 1f;
delta = Mathf.Clamp(delta + r, -1f, 1f);
// Smoothing the effect of Reach with the expense of some accuracy
switch (reachSmoothing) {
case ReachSmoothing.Exponential:
delta *= delta;
break;
case ReachSmoothing.Cubic:
delta *= delta * delta;
break;
}
Vector3 offset = straight * Mathf.Clamp(delta, 0f, currentLength);
nodes[0].solverPosition += offset * (1f - nodes[0].effectorPositionWeight);
nodes[2].solverPosition += offset;
}
}
/*
* Initiating the chain.
* */
public void Initiate(IKSolver solver, FBIKChain[] chain) {
initiated = false;
foreach (IKSolver.Node node in nodes) {
node.solverPosition = node.transform.position;
}
// Calculating bone lengths
CalculateBoneLengths(chain);
// Initiating child constraints
foreach (ChildConstraint c in childConstraints) c.Initiate(solver as IKSolverFullBody);
// Initiating the bend constraint
if (nodes.Length == 3) {
bendConstraint.SetBones(nodes[0].transform, nodes[1].transform, nodes[2].transform);
bendConstraint.Initiate(solver as IKSolverFullBody);
}
crossFades = new float[children.Length];
initiated = true;
}
/*
* Initiating the chain.
* */
public void Initiate(IKSolver solver, FBIKChain[] chain)
{
initiated = false;
foreach (IKSolver.Node node in nodes) {
node.solverPosition = node.transform.position;
}
// Calculating bone lengths
for (int i = 0; i < nodes.Length - 1; i++) {
nodes[i].length = Vector3.Distance(nodes[i].transform.position, nodes[i + 1].transform.position);
if (nodes[i].length == 0) return;
}
for (int i = 0; i < children.Length; i++) {
chain[children[i]].rootLength = (chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
if (chain[children[i]].rootLength == 0f) return;
}
// Initiating child constraints
InitiateConstraints(solver);
initiated = true;
}
private Vector3 GetBodyOffset() => default; // 0x00000001809E3ED0-0x00000001809E4220 private Vector3 GetHandBodyPull(IKEffector effector, FBIKChain arm, Vector3 offset) => default; // 0x00000001809E47C0-0x00000001809E4A90
/*
* Applying trigonometric IK solver on the 3 segmented chains to relieve tension from the solver and increase accuracy.
* */
public void SolveTrigonometric(FBIKChain[] chain, bool calculateBendDirection = false) {
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) chain[children[i]].SolveTrigonometric(chain, calculateBendDirection);
if (nodes.Length != 3) return;
// Direction of the limb in solver
Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
// Distance between the first and the last node solver positions
float solverLength = solverDirection.magnitude;
if (solverLength == 0f) return;
// Maximim stretch of the limb
float maxMag = Mathf.Clamp(solverLength, 0f, length * maxLimbLength);
Vector3 direction = (solverDirection / solverLength) * maxMag;
// Get the general world space bending direction
Vector3 bendDirection = calculateBendDirection && bendConstraint.initiated? bendConstraint.GetDir(): nodes[1].solverPosition - nodes[0].solverPosition;
// Get the direction to the trigonometrically solved position of the second node
Vector3 toBendPoint = GetDirToBendPoint(direction, bendDirection, maxMag);
// Position the second node
nodes[1].solverPosition = nodes[0].solverPosition + toBendPoint;
}
/*
* Before updating the chain
* */
public void ReadPose(FBIKChain[] chain, bool fullBody) {
if (!initiated) return;
for (int i = 0; i < nodes.Length; i++) {
nodes[i].solverPosition = nodes[i].transform.position + nodes[i].offset;
}
// Calculating bone lengths
CalculateBoneLengths(chain);
if (fullBody) {
// Pre-update child constraints
for (int i = 0; i < childConstraints.Length; i++) childConstraints[i].OnPreSolve();
if (children.Length > 0) {
// PullSum
float pullSum = nodes[nodes.Length - 1].effectorPositionWeight;
for (int i = 0; i < children.Length; i++) pullSum += chain[children[i]].nodes[0].effectorPositionWeight * chain[children[i]].pull;
pullSum = Mathf.Clamp(pullSum, 1f, Mathf.Infinity);
// CrossFades
for (int i = 0; i < children.Length; i++) {
crossFades[i] = (chain[children[i]].nodes[0].effectorPositionWeight * chain[children[i]].pull) / pullSum;
}
}
// Finding the total pull force by all child chains
pullParentSum = 0f;
for (int i = 0; i < children.Length; i++) pullParentSum += chain[children[i]].pull;
pullParentSum = Mathf.Clamp(pullParentSum, 1f, Mathf.Infinity);
// Reach force
if (nodes.Length == 3) {
reachForce = reach * Mathf.Clamp(nodes[2].effectorPositionWeight, 0f, 1f);
} else reachForce = 0f;
if (push > 0f && nodes.Length > 1) distance = Vector3.Distance(nodes[0].transform.position, nodes[nodes.Length - 1].transform.position);
}
}
/*
* Iterating child constraints and child chains to make sure they are not conflicting
* */
public void SolveConstraintSystems(FBIKChain[] chain) {
// Satisfy child constraints
SolveChildConstraints();
for (int i = 0; i < children.Length; i++) {
SolveLinearConstraint(nodes[nodes.Length - 1], chain[children[i]].nodes[0], crossFades[i], chain[children[i]].rootLength);
}
}
/*
* Iterating child constraints and child chains to make sure they are not conflicting
* */
public void SolveConstraintSystems(FBIKChain[] chain)
{
if (childConstraints.Length == 0) return;
// Satisfy child constraints
SolveChildConstraints();
float pullSum = nodes[nodes.Length - 1].effectorPositionWeight;
for (int i = 0; i < children.Length; i++) pullSum += chain[children[i]].nodes[0].effectorPositionWeight * chain[children[i]].pull;
for (int i = 0; i < children.Length; i++) {
float crossFade = ((chain[children[i]].nodes[0].effectorPositionWeight * chain[children[i]].pull) / Mathf.Clamp(pullSum, 1f, Mathf.Infinity));
SolveLinearConstraint(nodes[nodes.Length - 1], chain[children[i]].nodes[0], crossFade, chain[children[i]].rootLength);
}
}
// Calculates all bone lengths as well as lenghts between the chains
private void CalculateBoneLengths(FBIKChain[] chain) {
// Calculating bone lengths
length = 0f;
for (int i = 0; i < nodes.Length - 1; i++) {
nodes[i].length = Vector3.Distance(nodes[i].transform.position, nodes[i + 1].transform.position);
length += nodes[i].length;
if (nodes[i].length == 0) {
Warning.Log("Bone " + nodes[i].transform.name + " - " + nodes[i + 1].transform.name + " length is zero, can not solve.", nodes[i].transform);
return;
}
}
for (int i = 0; i < children.Length; i++) {
chain[children[i]].rootLength = (chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
if (chain[children[i]].rootLength == 0f) {
return;
}
}
if (nodes.Length == 3) {
// Square magnitude of the limb lengths
sqrMag1 = nodes[0].length * nodes[0].length;
sqrMag2 = nodes[1].length * nodes[1].length;
sqrMagDif = sqrMag1 - sqrMag2;
}
}
