public void SolveTrigonometric(IKSolverFullBody solver, bool calculateBendDirection = false)
{
if (!this.initiated)
{
return;
}
for (int i = 0; i < this.children.Length; i++)
{
solver.chain[this.children[i]].SolveTrigonometric(solver, calculateBendDirection);
}
if (this.nodes.Length != 3)
{
return;
}
Vector3 a = this.nodes[2].solverPosition - this.nodes[0].solverPosition;
float magnitude = a.magnitude;
if (magnitude == 0f)
{
return;
}
float num = Mathf.Clamp(magnitude, 0f, this.length * 0.99999f);
Vector3 direction = a / magnitude * num;
Vector3 bendDirection = (calculateBendDirection && this.bendConstraint.initiated) ? this.bendConstraint.GetDir(solver) : (this.nodes[1].solverPosition - this.nodes[0].solverPosition);
Vector3 dirToBendPoint = this.GetDirToBendPoint(direction, bendDirection, num);
this.nodes[1].solverPosition = this.nodes[0].solverPosition + dirToBendPoint;
}
public void Stage1(IKSolverFullBody solver)
{
for (int i = 0; i < this.children.Length; i++)
{
solver.chain[this.children[i]].Stage1(solver);
}
if (this.children.Length == 0)
{
this.ForwardReach(this.nodes[this.nodes.Length - 1].solverPosition);
return;
}
Vector3 a = this.nodes[this.nodes.Length - 1].solverPosition;
this.SolveChildConstraints(solver);
for (int j = 0; j < this.children.Length; j++)
{
Vector3 a2 = solver.chain[this.children[j]].nodes[0].solverPosition;
if (solver.chain[this.children[j]].rootLength > 0f)
{
a2 = this.SolveFABRIKJoint(this.nodes[this.nodes.Length - 1].solverPosition, solver.chain[this.children[j]].nodes[0].solverPosition, solver.chain[this.children[j]].rootLength);
}
if (this.pullParentSum > 0f)
{
a += (a2 - this.nodes[this.nodes.Length - 1].solverPosition) * (solver.chain[this.children[j]].pull / this.pullParentSum);
}
}
this.ForwardReach(Vector3.Lerp(a, this.nodes[this.nodes.Length - 1].solverPosition, this.pin));
}
private void CalculateBoneLengths(IKSolverFullBody solver)
{
this.length = 0f;
for (int i = 0; i < this.nodes.Length - 1; i++)
{
this.nodes[i].length = Vector3.Distance(this.nodes[i].transform.position, this.nodes[i + 1].transform.position);
this.length += this.nodes[i].length;
if (this.nodes[i].length == 0f)
{
Warning.Log(string.Concat(new string[]
{
"Bone ",
this.nodes[i].transform.name,
" - ",
this.nodes[i + 1].transform.name,
" length is zero, can not solve."
}), this.nodes[i].transform, false);
return;
}
}
for (int j = 0; j < this.children.Length; j++)
{
solver.chain[this.children[j]].rootLength = (solver.chain[this.children[j]].nodes[0].transform.position - this.nodes[this.nodes.Length - 1].transform.position).magnitude;
if (solver.chain[this.children[j]].rootLength == 0f)
{
return;
}
}
if (this.nodes.Length == 3)
{
this.sqrMag1 = this.nodes[0].length * this.nodes[0].length;
this.sqrMag2 = this.nodes[1].length * this.nodes[1].length;
this.sqrMagDif = this.sqrMag1 - this.sqrMag2;
}
}
/*
* Ortho-Normalize a vector to the first bone direction
* */
private Vector3 OrthoToBone1(IKSolverFullBody solver, Vector3 tangent)
{
Vector3 normal = solver.GetNode(chainIndex2, nodeIndex2).solverPosition - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
Vector3.OrthoNormalize(ref normal, ref tangent);
return(tangent);
}
//private IKSolver.Node planeNode1, planeNode2, planeNode3;
public void Initiate(Transform transform, IKSolverFullBody solver) {
this.transform = transform;
solver.GetChainAndNodeIndexes(transform, out chainIndex, out nodeIndex);
//IKSolver.Point point = solver.GetPoint(transform);
//this.node = point as IKSolver.Node;
}
/*
* Satisfying child constraints
* */
private void SolveChildConstraints(IKSolverFullBody solver)
{
for (int i = 0; i < childConstraints.Length; i++)
{
childConstraints[i].Solve(solver);
}
}
/*
* Draws the scene view helpers for IKSolverFullBody
* */
public static void AddScene(UnityEngine.Object target, IKSolverFullBody solver, Color color, bool modifiable, ref int selectedEffector, float size) {
if (!modifiable) return;
if (!solver.initiated) return;
if (!Application.isPlaying && !solver.IsValid()) return;
// Effectors
for (int i = 0; i < solver.effectors.Length; i++) {
bool rotate = solver.effectors[i].isEndEffector;
float weight = rotate? Mathf.Max(solver.effectors[i].positionWeight, solver.effectors[i].rotationWeight): solver.effectors[i].positionWeight;
if (weight > 0 && selectedEffector != i) {
Handles.color = color;
if (rotate) {
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size * 0.5f, size * 0.5f, Handles.DotCap)) {
selectedEffector = i;
return;
}
} else {
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size, size, Handles.SphereCap)) {
selectedEffector = i;
return;
}
}
}
}
for (int i = 0; i < solver.effectors.Length; i++) IKEffectorInspector.AddScene(solver.effectors[i], color, modifiable && i == selectedEffector, size);
if (GUI.changed) EditorUtility.SetDirty(target);
}
public void Initiate(IKSolverFullBody solver)
{
this.position = this.bone.position;
this.rotation = this.bone.rotation;
this.animatedPlaneRotation = Quaternion.identity;
solver.GetChainAndNodeIndexes(this.bone, out this.chainIndex, out this.nodeIndex);
this.childChainIndexes = new int[this.childBones.Length];
this.childNodeIndexes = new int[this.childBones.Length];
for (int i = 0; i < this.childBones.Length; i++)
{
solver.GetChainAndNodeIndexes(this.childBones[i], out this.childChainIndexes[i], out this.childNodeIndexes[i]);
}
this.localPositions = new Vector3[this.childBones.Length];
this.usePlaneNodes = false;
if (this.planeBone1 != null)
{
solver.GetChainAndNodeIndexes(this.planeBone1, out this.plane1ChainIndex, out this.plane1NodeIndex);
if (this.planeBone2 != null)
{
solver.GetChainAndNodeIndexes(this.planeBone2, out this.plane2ChainIndex, out this.plane2NodeIndex);
if (this.planeBone3 != null)
{
solver.GetChainAndNodeIndexes(this.planeBone3, out this.plane3ChainIndex, out this.plane3NodeIndex);
this.usePlaneNodes = true;
}
}
this.isEndEffector = true;
}
else
{
this.isEndEffector = false;
}
}
/*
* Solving the constraint
* */
public void Solve(IKSolverFullBody solver)
{
if (pushElasticity >= 1 && pullElasticity >= 1)
{
return;
}
Vector3 direction = solver.chain[chain2Index].nodes[0].solverPosition - solver.chain[chain1Index].nodes[0].solverPosition;
float distance = direction.magnitude;
if (distance == nominalDistance)
{
return;
}
if (distance == 0f)
{
return;
}
float force = 1f;
if (!isRigid)
{
float elasticity = distance > nominalDistance? pullElasticity: pushElasticity;
force = 1f - elasticity;
}
force *= 1f - nominalDistance / distance;
Vector3 offset = direction * force;
solver.chain[chain1Index].nodes[0].solverPosition += offset * crossFade;
solver.chain[chain2Index].nodes[0].solverPosition -= offset * inverseCrossFade;
}
private Vector3 OrthoToBone1(IKSolverFullBody solver, Vector3 tangent)
{
Vector3 vector = solver.GetNode(this.chainIndex2, this.nodeIndex2).solverPosition - solver.GetNode(this.chainIndex1, this.nodeIndex1).solverPosition;
Vector3.OrthoNormalize(ref vector, ref tangent);
return(tangent);
}
/*
* Draws the scene view helpers for IKSolverFullBody
* */
public static void AddScene(IKSolverFullBody solver, Color color, bool modifiable, ref int selectedEffector, float size)
{
if (!solver.IsValid(false)) return;
if (!Application.isPlaying) return;
// Effectors
for (int i = 0; i < solver.effectors.Length; i++) {
bool rotate = solver.effectors[i].isEndEffector;
float weight = rotate? Mathf.Max(solver.effectors[i].positionWeight, solver.effectors[i].rotationWeight): solver.effectors[i].positionWeight;
if (weight > 0 && selectedEffector != i) {
Handles.color = color;
if (rotate) {
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size * 0.5f, size * 0.5f, Handles.DotCap)) {
selectedEffector = i;
return;
}
} else {
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size, size, Handles.SphereCap)) {
selectedEffector = i;
return;
}
}
}
}
for (int i = 0; i < solver.effectors.Length; i++) IKEffectorInspector.AddScene(solver.effectors[i], color, modifiable && i == selectedEffector, size);
}
/*
* Positioning and rotating the spine bones to match the solver positions
* */
private void MapToSolverPositions(IKSolverFullBody solver)
{
// Translating the first bone
// Note: spine here also includes the pelvis
spine[0].SetToIKPosition();
spine[0].RotateToPlane(solver, 1f);
// Translating all the bones between the first and the last
for (int i = 1; i < spine.Length - 1; i++)
{
spine[i].Swing(spine[i + 1].ikPosition, 1f);
if (twistWeight > 0)
{
float bWeight = (float)i / ((float)spine.Length - 2);
Vector3 s1 = solver.GetNode(leftUpperArm.chainIndex, leftUpperArm.nodeIndex).solverPosition;
Vector3 s2 = solver.GetNode(rightUpperArm.chainIndex, rightUpperArm.nodeIndex).solverPosition;
spine[i].Twist(s1 - s2, spine[i + 1].ikPosition - spine[i].transform.position, bWeight * twistWeight);
}
}
// Translating the last bone
spine[spine.Length - 1].SetToIKPosition();
spine[spine.Length - 1].RotateToPlane(solver, 1f);
}
public void WritePose(IKSolverFullBody solver)
{
Vector3 planePosition = this.spine[0].GetPlanePosition(solver);
Vector3 solverPosition = solver.GetNode(this.spine[this.rootNodeIndex].chainIndex, this.spine[this.rootNodeIndex].nodeIndex).solverPosition;
Vector3 planePosition2 = this.spine[this.spine.Length - 1].GetPlanePosition(solver);
if (this.useFABRIK)
{
Vector3 b = solver.GetNode(this.spine[this.rootNodeIndex].chainIndex, this.spine[this.rootNodeIndex].nodeIndex).solverPosition - this.spine[this.rootNodeIndex].transform.position;
for (int i = 0; i < this.spine.Length; i++)
{
this.spine[i].ikPosition = this.spine[i].transform.position + b;
}
for (int j = 0; j < this.iterations; j++)
{
this.ForwardReach(planePosition2);
this.BackwardReach(planePosition);
this.spine[this.rootNodeIndex].ikPosition = solverPosition;
}
}
else
{
this.spine[0].ikPosition = planePosition;
this.spine[this.rootNodeIndex].ikPosition = solverPosition;
}
this.spine[this.spine.Length - 1].ikPosition = planePosition2;
this.MapToSolverPositions(solver);
}
/*
* Initiating the constraint
* */
public void Initiate(IKSolverFullBody solver)
{
chain1Index = solver.GetChainIndex(bone1);
chain2Index = solver.GetChainIndex(bone2);
OnPreSolve(solver);
}
public void Solve(IKSolverFullBody solver)
{
if (this.pushElasticity >= 1f && this.pullElasticity >= 1f)
{
return;
}
Vector3 a = solver.chain[this.chain2Index].nodes[0].solverPosition - solver.chain[this.chain1Index].nodes[0].solverPosition;
float magnitude = a.magnitude;
if (magnitude == this.nominalDistance)
{
return;
}
if (magnitude == 0f)
{
return;
}
float num = 1f;
if (!this.isRigid)
{
float num2 = (magnitude > this.nominalDistance) ? this.pullElasticity : this.pushElasticity;
num = 1f - num2;
}
num *= 1f - this.nominalDistance / magnitude;
Vector3 a2 = a * num;
solver.chain[this.chain1Index].nodes[0].solverPosition += a2 * this.crossFade;
solver.chain[this.chain2Index].nodes[0].solverPosition -= a2 * this.inverseCrossFade;
}
// Calculates all bone lengths as well as lenghts between the chains
private void CalculateBoneLengths(IKSolverFullBody solver)
{
// 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++)
{
solver.chain[children[i]].rootLength = (solver.chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
if (solver.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;
}
}
/*
* Gets the starting position of the iteration
* */
private Vector3 GetPosition(IKSolverFullBody solver, out Quaternion planeRotationOffset)
{
planeRotationOffset = Quaternion.identity;
if (!isEndEffector)
{
return(solver.GetNode(chainIndex, nodeIndex).solverPosition); // non end-effectors are always free
}
if (maintainRelativePositionWeight <= 0f)
{
return(animatedPosition);
}
// Maintain relative position
Vector3 p = bone.position;
Vector3 dir = p - planeBone1.position;
planeRotationOffset = GetPlaneRotation(solver) * Quaternion.Inverse(animatedPlaneRotation);
p = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + planeRotationOffset * dir;
// Interpolate the rotation offset
planeRotationOffset = Quaternion.Lerp(Quaternion.identity, planeRotationOffset, maintainRelativePositionWeight);
return(Vector3.Lerp(animatedPosition, p + solver.GetNode(chainIndex, nodeIndex).offset, maintainRelativePositionWeight));
}
public void WritePose(IKSolverFullBody solver, bool fullBody)
{
if (weight <= 0f)
{
return;
}
// Swing the parent bone to look at the first node's position
if (fullBody)
{
if (parentBone != null)
{
boneMapParent.Swing(solver.GetNode(boneMap1.chainIndex, boneMap1.nodeIndex).solverPosition, weight);
//boneMapParent.Swing(boneMap1.node.solverPosition, weight);
}
// Fix the first bone to it's node
boneMap1.FixToNode(solver, weight);
}
// Rotate the 2 first bones to the plane points
boneMap1.RotateToPlane(solver, weight);
boneMap2.RotateToPlane(solver, weight);
// Rotate the third bone to the rotation it had before solving
boneMap3.RotateToMaintain(maintainRotationWeight * weight * solver.IKPositionWeight);
// Rotate the third bone to the effector rotation
boneMap3.RotateToEffector(solver, weight);
}
public override void Initiate(IKSolverFullBody solver)
{
if (this.boneMapParent == null)
{
this.boneMapParent = new IKMapping.BoneMap();
}
if (this.boneMap1 == null)
{
this.boneMap1 = new IKMapping.BoneMap();
}
if (this.boneMap2 == null)
{
this.boneMap2 = new IKMapping.BoneMap();
}
if (this.boneMap3 == null)
{
this.boneMap3 = new IKMapping.BoneMap();
}
if (this.parentBone != null)
{
this.boneMapParent.Initiate(this.parentBone, solver);
}
this.boneMap1.Initiate(this.bone1, solver);
this.boneMap2.Initiate(this.bone2, solver);
this.boneMap3.Initiate(this.bone3, solver);
this.boneMap1.SetPlane(solver, this.boneMap1.transform, this.boneMap2.transform, this.boneMap3.transform);
this.boneMap2.SetPlane(solver, this.boneMap2.transform, this.boneMap3.transform, this.boneMap1.transform);
if (this.parentBone != null)
{
this.boneMapParent.SetLocalSwingAxis(this.boneMap1);
}
}
/*
* Clear node offset
* */
public void ResetOffset(IKSolverFullBody solver) {
solver.GetNode(chainIndex, nodeIndex).offset = Vector3.zero;
for (int i = 0; i < childChainIndexes.Length; i++) {
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).offset = Vector3.zero;
}
}
/*
* Initiating the chain.
* */
public void Initiate(IKSolverFullBody solver)
{
initiated = false;
foreach (IKSolver.Node node in nodes)
{
node.solverPosition = node.transform.position;
}
// Calculating bone lengths
CalculateBoneLengths(solver);
// 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;
}
//private IKSolver.Node planeNode1, planeNode2, planeNode3;
public void Initiate(Transform transform, IKSolverFullBody solver)
{
this.transform = transform;
solver.GetChainAndNodeIndexes(transform, out chainIndex, out nodeIndex);
//IKSolver.Point point = solver.GetPoint(transform);
//this.node = point as IKSolver.Node;
}
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();
}
public override void Initiate(IKSolverFullBody solver)
{
if (this.boneMap == null)
{
this.boneMap = new IKMapping.BoneMap();
}
this.boneMap.Initiate(this.bone, solver);
}
public void SolveConstraintSystems(IKSolverFullBody solver)
{
this.SolveChildConstraints(solver);
for (int i = 0; i < this.children.Length; i++)
{
this.SolveLinearConstraint(this.nodes[this.nodes.Length - 1], solver.chain[this.children[i]].nodes[0], this.crossFades[i], solver.chain[this.children[i]].rootLength);
}
}
public override void Initiate(IKSolverFullBody solver)
{
if (this.iterations <= 0)
{
this.iterations = 3;
}
if (this.spine == null || this.spine.Length != this.spineBones.Length)
{
this.spine = new IKMapping.BoneMap[this.spineBones.Length];
}
this.rootNodeIndex = -1;
for (int i = 0; i < this.spineBones.Length; i++)
{
if (this.spine[i] == null)
{
this.spine[i] = new IKMapping.BoneMap();
}
this.spine[i].Initiate(this.spineBones[i], solver);
if (this.spine[i].isNodeBone)
{
this.rootNodeIndex = i;
}
}
if (this.leftUpperArm == null)
{
this.leftUpperArm = new IKMapping.BoneMap();
}
if (this.rightUpperArm == null)
{
this.rightUpperArm = new IKMapping.BoneMap();
}
if (this.leftThigh == null)
{
this.leftThigh = new IKMapping.BoneMap();
}
if (this.rightThigh == null)
{
this.rightThigh = new IKMapping.BoneMap();
}
this.leftUpperArm.Initiate(this.leftUpperArmBone, solver);
this.rightUpperArm.Initiate(this.rightUpperArmBone, solver);
this.leftThigh.Initiate(this.leftThighBone, solver);
this.rightThigh.Initiate(this.rightThighBone, solver);
for (int j = 0; j < this.spine.Length; j++)
{
this.spine[j].SetIKPosition();
}
this.spine[0].SetPlane(solver, this.spine[this.rootNodeIndex].transform, this.leftThigh.transform, this.rightThigh.transform);
for (int k = 0; k < this.spine.Length - 1; k++)
{
this.spine[k].SetLength(this.spine[k + 1]);
this.spine[k].SetLocalSwingAxis(this.spine[k + 1]);
this.spine[k].SetLocalTwistAxis(this.leftUpperArm.transform.position - this.rightUpperArm.transform.position, this.spine[k + 1].transform.position - this.spine[k].transform.position);
}
this.spine[this.spine.Length - 1].SetPlane(solver, this.spine[this.rootNodeIndex].transform, this.leftUpperArm.transform, this.rightUpperArm.transform);
this.spine[this.spine.Length - 1].SetLocalSwingAxis(this.leftUpperArm, this.rightUpperArm);
this.useFABRIK = this.UseFABRIK();
}
/*
* End-effectors pushing the first nodes
* */
public Vector3 Push(IKSolverFullBody solver)
{
Vector3 sum = Vector3.zero;
// Get the push from the children
for (int i = 0; i < children.Length; i++)
{
sum += solver.chain[children[i]].Push(solver) * solver.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);
}
/*
* 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 and setting defaults
* */
public override void Initiate(IKSolverFullBody solver)
{
if (boneMap == null)
{
boneMap = new BoneMap();
}
boneMap.Initiate(bone, solver);
}
public void SetPlane(IKSolverFullBody solver, Transform planeBone1, Transform planeBone2, Transform planeBone3)
{
this.planeBone1 = planeBone1;
this.planeBone2 = planeBone2;
this.planeBone3 = planeBone3;
solver.GetChainAndNodeIndexes(planeBone1, out this.plane1ChainIndex, out this.plane1NodeIndex);
solver.GetChainAndNodeIndexes(planeBone2, out this.plane2ChainIndex, out this.plane2NodeIndex);
solver.GetChainAndNodeIndexes(planeBone3, out this.plane3ChainIndex, out this.plane3NodeIndex);
this.UpdatePlane(true, true);
}
/*
* Iterating child constraints and child chains to make sure they are not conflicting
* */
public void SolveConstraintSystems(IKSolverFullBody solver)
{
// Satisfy child constraints
SolveChildConstraints(solver);
for (int i = 0; i < children.Length; i++)
{
SolveLinearConstraint(nodes[nodes.Length - 1], solver.chain[children[i]].nodes[0], crossFades[i], solver.chain[children[i]].rootLength);
}
}
/*
* 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();
}
/*
* Draws the scene view helpers for IKSolverFullBody
* */
public static void AddScene(UnityEngine.Object target, IKSolverFullBody solver, Color color, bool modifiable, ref int selectedEffector, float size)
{
if (!modifiable)
{
return;
}
if (!solver.initiated)
{
return;
}
if (!Application.isPlaying && !solver.IsValid())
{
return;
}
// Effectors
for (int i = 0; i < solver.effectors.Length; i++)
{
bool rotate = solver.effectors[i].isEndEffector;
float weight = rotate? Mathf.Max(solver.effectors[i].positionWeight, solver.effectors[i].rotationWeight): solver.effectors[i].positionWeight;
if (weight > 0 && selectedEffector != i)
{
Handles.color = color;
if (rotate)
{
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size * 0.5f, size * 0.5f, Handles.DotCap))
{
selectedEffector = i;
return;
}
}
else
{
if (Handles.Button(solver.effectors[i].position, solver.effectors[i].rotation, size, size, Handles.SphereCap))
{
selectedEffector = i;
return;
}
}
}
}
for (int i = 0; i < solver.effectors.Length; i++)
{
IKEffectorInspector.AddScene(solver.effectors[i], color, modifiable && i == selectedEffector, size);
}
if (GUI.changed)
{
EditorUtility.SetDirty(target);
}
}
public void RotateToPlane(IKSolverFullBody solver, float weight)
{
Quaternion quaternion = this.GetTargetRotation(solver) * this.defaultLocalTargetRotation;
if (weight >= 1f)
{
this.transform.rotation = quaternion;
return;
}
this.transform.rotation = Quaternion.Lerp(this.transform.rotation, quaternion, weight);
}
/*
* Updating nominal distance because it might have changed in the animation
* */
public void OnPreSolve(IKSolverFullBody solver) {
nominalDistance = Vector3.Distance(solver.chain[chain1Index].nodes[0].transform.position, solver.chain[chain2Index].nodes[0].transform.position);
isRigid = pushElasticity <= 0 && pullElasticity <= 0;
// CrossFade
if (isRigid) {
float offset = solver.chain[chain1Index].pull - solver.chain[chain2Index].pull;
crossFade = 1f - (0.5f + (offset * 0.5f));
} else crossFade = 0.5f;
inverseCrossFade = 1f - crossFade;
}
/*
* Initiate the constraint and set defaults
* */
public void Initiate(IKSolverFullBody solver)
{
node1 = solver.GetPoint(bone1) as IKSolver.Node;
node2 = solver.GetPoint(bone2) as IKSolver.Node;
node3 = solver.GetPoint(bone3) as IKSolver.Node;
// Find the default bend direction orthogonal to the chain direction
direction = OrthoToBone1(OrthoToLimb(node2.transform.position - node1.transform.position));
// Default bend direction relative to the first node
defaultLocalDirection = Quaternion.Inverse(node1.transform.rotation) * direction;
// Default plane normal
Vector3 defaultNormal = Vector3.Cross((node3.transform.position - node1.transform.position).normalized, direction);
// Default plane normal relative to the third node
defaultChildDirection = Quaternion.Inverse(node3.transform.rotation) * defaultNormal;
}
/// <summary>
/// Determines whether this IKConstraintBend is valid.
/// </summary>
public bool IsValid(IKSolverFullBody solver, Warning.Logger logger) {
if (bone1 == null || bone2 == null || bone3 == null) {
if (logger != null) logger("Bend Constraint contains a null reference.");
return false;
}
if (solver.GetPoint(bone1) == null) {
if (logger != null) logger("Bend Constraint is referencing to a bone '" + bone1.name + "' that does not excist in the Node Chain.");
return false;
}
if (solver.GetPoint(bone2) == null) {
if (logger != null) logger("Bend Constraint is referencing to a bone '" + bone2.name + "' that does not excist in the Node Chain.");
return false;
}
if (solver.GetPoint(bone3) == null) {
if (logger != null) logger("Bend Constraint is referencing to a bone '" + bone3.name + "' that does not excist in the Node Chain.");
return false;
}
return true;
}
/*
* Before updating the chain
* */
public void ReadPose(IKSolverFullBody solver, 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(solver);
if (fullBody) {
// Pre-update child constraints
for (int i = 0; i < childConstraints.Length; i++) childConstraints[i].OnPreSolve(solver);
if (children.Length > 0) {
// PullSum
float pullSum = nodes[nodes.Length - 1].effectorPositionWeight;
for (int i = 0; i < children.Length; i++) pullSum += solver.chain[children[i]].nodes[0].effectorPositionWeight * solver.chain[children[i]].pull;
pullSum = Mathf.Clamp(pullSum, 1f, Mathf.Infinity);
// CrossFades
for (int i = 0; i < children.Length; i++) {
crossFades[i] = (solver.chain[children[i]].nodes[0].effectorPositionWeight * solver.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 += solver.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);
}
}
/*
* End-effectors pushing the first nodes
* */
public Vector3 Push(IKSolverFullBody solver) {
Vector3 sum = Vector3.zero;
// Get the push from the children
for (int i = 0; i < children.Length; i++) {
sum += solver.chain[children[i]].Push(solver) * solver.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;
}
/*
* Initiate the effector, set default values
* */
public void Initiate(IKSolverFullBody solver) {
position = bone.position;
rotation = bone.rotation;
animatedPlaneRotation = Quaternion.identity;
// Getting the node
solver.GetChainAndNodeIndexes(bone, out chainIndex, out nodeIndex);
// Child nodes
childChainIndexes = new int[childBones.Length];
childNodeIndexes = new int[childBones.Length];
for (int i = 0; i < childBones.Length; i++) {
solver.GetChainAndNodeIndexes(childBones[i], out childChainIndexes[i], out childNodeIndexes[i]);
}
localPositions = new Vector3[childBones.Length];
// Plane nodes
usePlaneNodes = false;
if (planeBone1 != null) {
solver.GetChainAndNodeIndexes(planeBone1, out plane1ChainIndex, out plane1NodeIndex);
if (planeBone2 != null) {
solver.GetChainAndNodeIndexes(planeBone2, out plane2ChainIndex, out plane2NodeIndex);
if (planeBone3 != null) {
solver.GetChainAndNodeIndexes(planeBone3, out plane3ChainIndex, out plane3NodeIndex);
usePlaneNodes = true;
}
}
isEndEffector = true;
} else isEndEffector = false;
}
/// <summary>
/// Gets the main node.
/// </summary>
public IKSolver.Node GetNode(IKSolverFullBody solver) {
return solver.chain[chainIndex].nodes[nodeIndex];
}
/*
* Presolving, applying offset
* */
public void OnPreSolve(IKSolverFullBody solver) {
positionWeight = Mathf.Clamp(positionWeight, 0f, 1f);
rotationWeight = Mathf.Clamp(rotationWeight, 0f, 1f);
maintainRelativePositionWeight = Mathf.Clamp(maintainRelativePositionWeight, 0f, 1f);
// Calculating weights
posW = positionWeight * solver.IKPositionWeight;
rotW = rotationWeight * solver.IKPositionWeight;
solver.GetNode(chainIndex, nodeIndex).effectorPositionWeight = posW;
solver.GetNode(chainIndex, nodeIndex).effectorRotationWeight = rotW;
solver.GetNode(chainIndex, nodeIndex).solverRotation = rotation;
if (float.IsInfinity(positionOffset.x) ||
float.IsInfinity(positionOffset.y) ||
float.IsInfinity(positionOffset.z)
) Debug.LogError("Invalid IKEffector.positionOffset (contains Infinity)! Please make sure not to set IKEffector.positionOffset to infinite values.", bone);
if (float.IsNaN(positionOffset.x) ||
float.IsNaN(positionOffset.y) ||
float.IsNaN(positionOffset.z)
) Debug.LogError("Invalid IKEffector.positionOffset (contains NaN)! Please make sure not to set IKEffector.positionOffset to NaN values.", bone);
if (positionOffset.sqrMagnitude > 10000000000f) Debug.LogError("Additive effector positionOffset detected in Full Body IK (extremely large value). Make sure you are not circularily adding to effector positionOffset each frame.", bone);
if (float.IsInfinity(position.x) ||
float.IsInfinity(position.y) ||
float.IsInfinity(position.z)
) Debug.LogError("Invalid IKEffector.position (contains Infinity)!");
solver.GetNode(chainIndex, nodeIndex).offset += positionOffset * solver.IKPositionWeight;
if (effectChildNodes && solver.iterations > 0) {
for (int i = 0; i < childBones.Length; i++) {
localPositions[i] = childBones[i].transform.position - bone.transform.position;
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).offset += positionOffset * solver.IKPositionWeight;
}
}
// Relative to Plane
if (usePlaneNodes && maintainRelativePositionWeight > 0f) {
animatedPlaneRotation = Quaternion.LookRotation(planeBone2.position - planeBone1.position, planeBone3.position - planeBone1.position);;
}
firstUpdate = true;
}
/*
* Iterating child constraints and child chains to make sure they are not conflicting
* */
public void SolveConstraintSystems(IKSolverFullBody solver) {
// Satisfy child constraints
SolveChildConstraints(solver);
for (int i = 0; i < children.Length; i++) {
SolveLinearConstraint(nodes[nodes.Length - 1], solver.chain[children[i]].nodes[0], crossFades[i], solver.chain[children[i]].rootLength);
}
}
/*
* Initiating and setting defaults
* */
public override void Initiate(IKSolverFullBody solver) {
if (boneMap == null) boneMap = new BoneMap();
boneMap.Initiate(bone, solver);
}
/*
* Manipulating node solverPosition
* */
public void Update(IKSolverFullBody solver) {
if (firstUpdate) {
animatedPosition = bone.position + solver.GetNode(chainIndex, nodeIndex).offset;
firstUpdate = false;
}
solver.GetNode(chainIndex, nodeIndex).solverPosition = Vector3.Lerp(GetPosition(solver, out planeRotationOffset), position, posW);
// Child nodes
if (!effectChildNodes) return;
for (int i = 0; i < childBones.Length; i++) {
solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).solverPosition = Vector3.Lerp(solver.GetNode(childChainIndexes[i], childNodeIndexes[i]).solverPosition, solver.GetNode(chainIndex, nodeIndex).solverPosition + localPositions[i], posW);
}
}
/*
* Gets the starting position of the iteration
* */
private Vector3 GetPosition(IKSolverFullBody solver, out Quaternion planeRotationOffset) {
planeRotationOffset = Quaternion.identity;
if (!isEndEffector) return solver.GetNode(chainIndex, nodeIndex).solverPosition; // non end-effectors are always free
if (maintainRelativePositionWeight <= 0f) return animatedPosition;
// Maintain relative position
Vector3 p = bone.position;
Vector3 dir = p - planeBone1.position;
planeRotationOffset = GetPlaneRotation(solver) * Quaternion.Inverse(animatedPlaneRotation);
p = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition + planeRotationOffset * dir;
// Interpolate the rotation offset
planeRotationOffset = Quaternion.Lerp(Quaternion.identity, planeRotationOffset, maintainRelativePositionWeight);
return Vector3.Lerp(animatedPosition, p + solver.GetNode(chainIndex, nodeIndex).offset, maintainRelativePositionWeight);
}
/*
* Stage 1 of the FABRIK algorithm
* */
public void Stage1(IKSolverFullBody solver) {
// Stage 1
for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage1(solver);
// If is the last chain in this hierarchy, solve immediatelly and return
if (children.Length == 0) {
ForwardReach(nodes[nodes.Length - 1].solverPosition);
return;
}
Vector3 centroid = nodes[nodes.Length - 1].solverPosition;
// Satisfying child constraints
SolveChildConstraints(solver);
// Finding the centroid position of all child chains according to their individual pull weights
for (int i = 0; i < children.Length; i++) {
Vector3 childPosition = solver.chain[children[i]].nodes[0].solverPosition;
if (solver.chain[children[i]].rootLength > 0) {
childPosition = SolveFABRIKJoint(nodes[nodes.Length - 1].solverPosition, solver.chain[children[i]].nodes[0].solverPosition, solver.chain[children[i]].rootLength);
}
if (pullParentSum > 0) centroid += (childPosition - nodes[nodes.Length - 1].solverPosition) * (solver.chain[children[i]].pull / pullParentSum);
}
// Forward reach to the centroid (unless pinned)
ForwardReach(Vector3.Lerp(centroid, nodes[nodes.Length - 1].solverPosition, pin));
}
/*
* Initiating the constraint
* */
public void Initiate(IKSolverFullBody solver) {
chain1Index = solver.GetChainIndex(bone1);
chain2Index = solver.GetChainIndex(bone2);
OnPreSolve(solver);
}
/*
* Satisfying child constraints
* */
private void SolveChildConstraints(IKSolverFullBody solver) {
for (int i = 0; i < childConstraints.Length; i++) {
childConstraints[i].Solve(solver);
}
}
/*
* Computes the direction from the first node to the second node
* */
public Vector3 GetDir(IKSolverFullBody solver) {
if (!initiated) return Vector3.zero;
float w = weight * solver.IKPositionWeight;
// Apply the bend goal
if (bendGoal != null) {
Vector3 b = bendGoal.position - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
if (b != Vector3.zero) direction = b;
}
if (w >= 1f) return direction.normalized;
Vector3 solverDirection = solver.GetNode(chainIndex3, nodeIndex3).solverPosition - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
// Get rotation from animated limb direction to solver limb direction
Quaternion f = Quaternion.FromToRotation(bone3.position - bone1.position, solverDirection);
// Rotate the default bend direction by f
Vector3 dir = f * (bone2.position - bone1.position);
// Effector rotation
if (solver.GetNode(chainIndex3, nodeIndex3).effectorRotationWeight > 0f) {
// Bend direction according to the effector rotation
Vector3 effectorDirection = -Vector3.Cross(solverDirection, solver.GetNode(chainIndex3, nodeIndex3).solverRotation * defaultChildDirection);
dir = Vector3.Lerp(dir, effectorDirection, solver.GetNode(chainIndex3, nodeIndex3).effectorRotationWeight);
}
// Rotation Offset
if (rotationOffset != Quaternion.identity) {
Quaternion toOrtho = Quaternion.FromToRotation(rotationOffset * solverDirection, solverDirection);
dir = toOrtho * rotationOffset * dir;
}
if (w <= 0f) return dir;
return Vector3.Lerp(dir, direction.normalized, w);
}
/*
* Applying trigonometric IK solver on the 3 segmented chains to relieve tension from the solver and increase accuracy.
* */
public void SolveTrigonometric(IKSolverFullBody solver, bool calculateBendDirection = false) {
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) solver.chain[children[i]].SolveTrigonometric(solver, 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(solver): 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;
}
/*
* Initiate the constraint and set defaults
* */
public void Initiate(IKSolverFullBody solver) {
solver.GetChainAndNodeIndexes(bone1, out chainIndex1, out nodeIndex1);
solver.GetChainAndNodeIndexes(bone2, out chainIndex2, out nodeIndex2);
solver.GetChainAndNodeIndexes(bone3, out chainIndex3, out nodeIndex3);
// Find the default bend direction orthogonal to the chain direction
direction = OrthoToBone1(solver, OrthoToLimb(solver, bone2.position - bone1.position));
// Default bend direction relative to the first node
defaultLocalDirection = Quaternion.Inverse(bone1.rotation) * direction;
// Default plane normal
Vector3 defaultNormal = Vector3.Cross((bone3.position - bone1.position).normalized, direction);
// Default plane normal relative to the third node
defaultChildDirection = Quaternion.Inverse(bone3.rotation) * defaultNormal;
initiated = true;
}
/*
* Rotation of plane nodes in the solver
* */
private Quaternion GetPlaneRotation(IKSolverFullBody solver) {
Vector3 p1 = solver.GetNode(plane1ChainIndex, plane1NodeIndex).solverPosition;
Vector3 p2 = solver.GetNode(plane2ChainIndex, plane2NodeIndex).solverPosition;
Vector3 p3 = solver.GetNode(plane3ChainIndex, plane3NodeIndex).solverPosition;
Vector3 viewingVector = p2 - p1;
Vector3 upVector = p3 - p1;
if (viewingVector == Vector3.zero) {
Warning.Log("Make sure you are not placing 2 or more FBBIK effectors of the same chain to exactly the same position.", bone);
return Quaternion.identity;
}
return Quaternion.LookRotation(viewingVector, upVector);
}
/*
* Solving the constraint
* */
public void Solve(IKSolverFullBody solver) {
if (pushElasticity >= 1 && pullElasticity >= 1) return;
Vector3 direction = solver.chain[chain2Index].nodes[0].solverPosition - solver.chain[chain1Index].nodes[0].solverPosition;
float distance = direction.magnitude;
if (distance == nominalDistance) return;
if (distance == 0f) return;
float force = 1f;
if (!isRigid) {
float elasticity = distance > nominalDistance? pullElasticity: pushElasticity;
force = 1f - elasticity;
}
force *= 1f - nominalDistance / distance;
Vector3 offset = direction * force;
solver.chain[chain1Index].nodes[0].solverPosition += offset * crossFade;
solver.chain[chain2Index].nodes[0].solverPosition -= offset * inverseCrossFade;
}
/*
* Stage 2 of the FABRIK algorithm.
* */
public void Stage2(IKSolverFullBody solver, Vector3 position) {
// Stage 2
BackwardReach(position);
int it = Mathf.Clamp(solver.iterations, 2, 4);
// Iterating child constraints and child chains to make sure they are not conflicting
if (childConstraints.Length > 0) {
for (int i = 0; i < it; i++) SolveConstraintSystems(solver);
}
// Stage 2 for the children
for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage2(solver, nodes[nodes.Length - 1].solverPosition);
}
// Calculates all bone lengths as well as lenghts between the chains
private void CalculateBoneLengths(IKSolverFullBody solver) {
// 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++) {
solver.chain[children[i]].rootLength = (solver.chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
if (solver.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;
}
}
/*
* Ortho-Normalize a vector to the first bone direction
* */
private Vector3 OrthoToBone1(IKSolverFullBody solver, Vector3 tangent) {
Vector3 normal = solver.GetNode(chainIndex2, nodeIndex2).solverPosition - solver.GetNode(chainIndex1, nodeIndex1).solverPosition;
Vector3.OrthoNormalize(ref normal, ref tangent);
return tangent;
}
/*
* Initiating the chain.
* */
public void Initiate(IKSolverFullBody solver) {
initiated = false;
foreach (IKSolver.Node node in nodes) {
node.solverPosition = node.transform.position;
}
// Calculating bone lengths
CalculateBoneLengths(solver);
// 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;
}
/*
* Reaching limbs
* */
public void Reach(IKSolverFullBody solver) {
if (!initiated) return;
// Solve children first
for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Reach(solver);
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;
}
