123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582 |
- using UnityEngine;
- using System.Collections;
- using System;
- namespace RootMotion.FinalIK {
-
-
-
- [System.Serializable]
- public class FBIKChain {
-
- #region Main Interface
-
-
-
-
- [System.Serializable]
- public class ChildConstraint {
-
-
-
- public float pushElasticity = 0f;
-
-
-
- public float pullElasticity = 0f;
-
-
-
- [SerializeField] private Transform bone1;
-
-
-
- [SerializeField] private Transform bone2;
-
- public float nominalDistance { get; private set; }
-
- public bool isRigid { get; private set; }
-
- private float crossFade, inverseCrossFade;
- private int chain1Index;
- private int chain2Index;
-
- public ChildConstraint(Transform bone1, Transform bone2, float pushElasticity = 0f, float pullElasticity = 0f) {
- this.bone1 = bone1;
- this.bone2 = bone2;
- this.pushElasticity = pushElasticity;
- this.pullElasticity = pullElasticity;
- }
-
-
- public void Initiate(IKSolverFullBody solver) {
- chain1Index = solver.GetChainIndex(bone1);
- chain2Index = solver.GetChainIndex(bone2);
-
- OnPreSolve(solver);
- }
-
-
- 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;
-
- 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;
- }
-
-
- 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;
- }
- }
- [System.Serializable]
- public enum Smoothing {
- None,
- Exponential,
- Cubic
- }
-
-
-
- [Range(0f, 1f)]
- public float pin;
-
-
-
- [Range(0f, 1f)]
- public float pull = 1f;
-
-
-
- [Range(0f, 1f)]
- public float push;
-
-
-
- [Range(-1f, 1f)]
- public float pushParent;
-
-
-
- [Range(0f, 1f)]
- public float reach = 0.1f;
-
-
-
- public Smoothing reachSmoothing = Smoothing.Exponential;
-
-
-
- public Smoothing pushSmoothing = Smoothing.Exponential;
-
-
-
- public IKSolver.Node[] nodes = new IKSolver.Node[0];
-
-
-
- public int[] children = new int[0];
-
-
-
- public ChildConstraint[] childConstraints = new ChildConstraint[0];
-
-
-
-
- public IKConstraintBend bendConstraint = new IKConstraintBend();
- #endregion Main Interface
- private float rootLength;
- private bool initiated;
- private float length;
- private float distance;
- private IKSolver.Point p;
- private float reachForce;
- private float pullParentSum;
- private float[] crossFades;
- private float sqrMag1, sqrMag2, sqrMagDif;
- private const float maxLimbLength = 0.99999f;
- public FBIKChain() {}
-
- public FBIKChain (float pin, float pull, params Transform[] nodeTransforms) {
- this.pin = pin;
- this.pull = pull;
-
- SetNodes(nodeTransforms);
-
- children = new int[0];
- }
-
-
- public void SetNodes(params Transform[] boneTransforms) {
- nodes = new IKSolver.Node[boneTransforms.Length];
- for (int i = 0; i < boneTransforms.Length; i++) {
- nodes[i] = new IKSolver.Node(boneTransforms[i]);
- }
- }
- public int GetNodeIndex(Transform boneTransform) {
- for (int i = 0; i < nodes.Length; i++) {
- if (nodes[i].transform == boneTransform) return i;
- }
- return -1;
- }
-
- public bool IsValid(ref string message) {
- if (nodes.Length == 0) {
- message = "FBIK chain contains no nodes.";
- return false;
- }
-
- foreach (IKSolver.Node node in nodes) if (node.transform == null) {
- message = "Node transform is null in FBIK chain.";
- return false;
- }
-
- return true;
- }
-
-
- public void Initiate(IKSolverFullBody solver) {
- initiated = false;
-
- foreach (IKSolver.Node node in nodes) {
- node.solverPosition = node.transform.position;
- }
-
-
- CalculateBoneLengths(solver);
-
-
- foreach (ChildConstraint c in childConstraints) c.Initiate(solver as IKSolverFullBody);
-
- 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 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;
- }
-
- CalculateBoneLengths(solver);
- if (fullBody) {
-
- for (int i = 0; i < childConstraints.Length; i++) childConstraints[i].OnPreSolve(solver);
- if (children.Length > 0) {
-
- 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);
-
- for (int i = 0; i < children.Length; i++) {
- crossFades[i] = (solver.chain[children[i]].nodes[0].effectorPositionWeight * solver.chain[children[i]].pull) / pullSum;
- }
- }
-
- pullParentSum = 0f;
- for (int i = 0; i < children.Length; i++) pullParentSum += solver.chain[children[i]].pull;
- pullParentSum = Mathf.Clamp(pullParentSum, 1f, Mathf.Infinity);
-
- 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);
- }
- }
-
- private void CalculateBoneLengths(IKSolverFullBody solver) {
-
- 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) {
-
- sqrMag1 = nodes[0].length * nodes[0].length;
- sqrMag2 = nodes[1].length * nodes[1].length;
- sqrMagDif = sqrMag1 - sqrMag2;
- }
- }
- #region Recursive Methods
-
-
- public void Reach(IKSolverFullBody solver) {
- if (!initiated) return;
-
- 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;
-
- Vector3 straight = (solverDirection / solverLength) * length;
-
- float delta = Mathf.Clamp(solverLength / length, 1 - reachForce, 1 + reachForce) - 1f;
- delta = Mathf.Clamp(delta + reachForce, -1f, 1f);
-
- 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;
- }
-
- public Vector3 Push(IKSolverFullBody solver) {
- Vector3 sum = Vector3.zero;
-
- for (int i = 0; i < children.Length; i++) {
- sum += solver.chain[children[i]].Push(solver) * solver.chain[children[i]].pushParent;
- }
-
- nodes[nodes.Length - 1].solverPosition += sum;
-
- 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;
-
- float f = 1f - (solverLength / distance);
- if (f <= 0f) return Vector3.zero;
-
- switch (pushSmoothing) {
- case Smoothing.Exponential:
- f *= f;
- break;
- case Smoothing.Cubic:
- f *= f * f;
- break;
- }
-
- Vector3 p = -solverDirection * f * push;
-
- nodes[0].solverPosition += p;
- return p;
- }
-
- public void SolveTrigonometric(IKSolverFullBody solver, bool calculateBendDirection = false) {
- if (!initiated) return;
-
- for (int i = 0; i < children.Length; i++) solver.chain[children[i]].SolveTrigonometric(solver, calculateBendDirection);
-
- if (nodes.Length != 3) return;
-
- Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
-
- float solverLength = solverDirection.magnitude;
- if (solverLength == 0f) return;
-
- float maxMag = Mathf.Clamp(solverLength, 0f, length * maxLimbLength);
- Vector3 direction = (solverDirection / solverLength) * maxMag;
-
- Vector3 bendDirection = calculateBendDirection && bendConstraint.initiated? bendConstraint.GetDir(solver): nodes[1].solverPosition - nodes[0].solverPosition;
-
- Vector3 toBendPoint = GetDirToBendPoint(direction, bendDirection, maxMag);
-
- nodes[1].solverPosition = nodes[0].solverPosition + toBendPoint;
- }
-
- public void Stage1(IKSolverFullBody solver) {
-
- for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage1(solver);
-
-
- if (children.Length == 0) {
- ForwardReach(nodes[nodes.Length - 1].solverPosition);
- return;
- }
- Vector3 centroid = nodes[nodes.Length - 1].solverPosition;
-
-
- SolveChildConstraints(solver);
-
- 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);
- }
-
-
- ForwardReach(Vector3.Lerp(centroid, nodes[nodes.Length - 1].solverPosition, pin));
- }
-
-
- public void Stage2(IKSolverFullBody solver, Vector3 position) {
-
- BackwardReach(position);
- int it = Mathf.Clamp(solver.iterations, 2, 4);
-
- if (childConstraints.Length > 0) {
- for (int i = 0; i < it; i++) SolveConstraintSystems(solver);
- }
-
- for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage2(solver, nodes[nodes.Length - 1].solverPosition);
- }
-
- public void SolveConstraintSystems(IKSolverFullBody solver) {
-
- 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);
- }
- }
- #endregion Recursive Methods
-
- private Vector3 SolveFABRIKJoint(Vector3 pos1, Vector3 pos2, float length) {
- return pos2 + (pos1 - pos2).normalized * length;
- }
-
-
- protected Vector3 GetDirToBendPoint(Vector3 direction, Vector3 bendDirection, float directionMagnitude) {
- float x = ((directionMagnitude * directionMagnitude) + sqrMagDif) / 2f / directionMagnitude;
- float y = (float)Math.Sqrt(Mathf.Clamp(sqrMag1 - x * x, 0, Mathf.Infinity));
- if (direction == Vector3.zero) return Vector3.zero;
- return Quaternion.LookRotation(direction, bendDirection) * new Vector3(0f, y, x);
- }
-
- private void SolveChildConstraints(IKSolverFullBody solver) {
- for (int i = 0; i < childConstraints.Length; i++) {
- childConstraints[i].Solve(solver);
- }
- }
-
- private void SolveLinearConstraint(IKSolver.Node node1, IKSolver.Node node2, float crossFade, float distance) {
- Vector3 dir = node2.solverPosition - node1.solverPosition;
- float mag = dir.magnitude;
- if (distance == mag) return;
- if (mag == 0f) return;
- Vector3 offset = dir * (1f - distance / mag);
-
- node1.solverPosition += offset * crossFade;
- node2.solverPosition -= offset * (1f - crossFade);
- }
-
-
- public void ForwardReach(Vector3 position) {
-
- nodes[nodes.Length - 1].solverPosition = position;
-
- for (int i = nodes.Length - 2; i > -1; i--) {
-
- nodes[i].solverPosition = SolveFABRIKJoint(nodes[i].solverPosition, nodes[i + 1].solverPosition, nodes[i].length);
- }
- }
-
-
- private void BackwardReach(Vector3 position) {
-
- if (rootLength > 0) position = SolveFABRIKJoint(nodes[0].solverPosition, position, rootLength);
- nodes[0].solverPosition = position;
-
- for (int i = 1; i < nodes.Length; i++) {
- nodes[i].solverPosition = SolveFABRIKJoint(nodes[i].solverPosition, nodes[i - 1].solverPosition, nodes[i - 1].length);
- }
- }
- }
- }
|