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