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; }
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); } }
/* * 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; }
/* * 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)); }
/* * 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); }
/* * 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; }