FBIKChain.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582
  1. using UnityEngine;
  2. using System.Collections;
  3. using System;
  4. namespace RootMotion.FinalIK {
  5. /// <summary>
  6. /// A chain of bones in IKSolverFullBody.
  7. /// </summary>
  8. [System.Serializable]
  9. public class FBIKChain {
  10. #region Main Interface
  11. /// <summary>
  12. /// Linear constraint between child chains of a FBIKChain.
  13. /// </summary>
  14. [System.Serializable]
  15. public class ChildConstraint {
  16. /// <summary>
  17. /// The push elasticity.
  18. /// </summary>
  19. public float pushElasticity = 0f;
  20. /// <summary>
  21. /// The pull elasticity.
  22. /// </summary>
  23. public float pullElasticity = 0f;
  24. /// <summary>
  25. /// The first bone.
  26. /// </summary>
  27. [SerializeField] private Transform bone1;
  28. /// <summary>
  29. /// The second bone.
  30. /// </summary>
  31. [SerializeField] private Transform bone2;
  32. // Gets the nominal (animated) distance between the two bones.
  33. public float nominalDistance { get; private set; }
  34. // The constraint is rigid if both push and pull elasticity are 0.
  35. public bool isRigid { get; private set; }
  36. // The crossFade value between the connected chains
  37. private float crossFade, inverseCrossFade;
  38. private int chain1Index;
  39. private int chain2Index;
  40. /*
  41. * Constructor
  42. * */
  43. public ChildConstraint(Transform bone1, Transform bone2, float pushElasticity = 0f, float pullElasticity = 0f) {
  44. this.bone1 = bone1;
  45. this.bone2 = bone2;
  46. this.pushElasticity = pushElasticity;
  47. this.pullElasticity = pullElasticity;
  48. }
  49. /*
  50. * Initiating the constraint
  51. * */
  52. public void Initiate(IKSolverFullBody solver) {
  53. chain1Index = solver.GetChainIndex(bone1);
  54. chain2Index = solver.GetChainIndex(bone2);
  55. OnPreSolve(solver);
  56. }
  57. /*
  58. * Updating nominal distance because it might have changed in the animation
  59. * */
  60. public void OnPreSolve(IKSolverFullBody solver) {
  61. nominalDistance = Vector3.Distance(solver.chain[chain1Index].nodes[0].transform.position, solver.chain[chain2Index].nodes[0].transform.position);
  62. isRigid = pushElasticity <= 0 && pullElasticity <= 0;
  63. // CrossFade
  64. if (isRigid) {
  65. float offset = solver.chain[chain1Index].pull - solver.chain[chain2Index].pull;
  66. crossFade = 1f - (0.5f + (offset * 0.5f));
  67. } else crossFade = 0.5f;
  68. inverseCrossFade = 1f - crossFade;
  69. }
  70. /*
  71. * Solving the constraint
  72. * */
  73. public void Solve(IKSolverFullBody solver) {
  74. if (pushElasticity >= 1 && pullElasticity >= 1) return;
  75. Vector3 direction = solver.chain[chain2Index].nodes[0].solverPosition - solver.chain[chain1Index].nodes[0].solverPosition;
  76. float distance = direction.magnitude;
  77. if (distance == nominalDistance) return;
  78. if (distance == 0f) return;
  79. float force = 1f;
  80. if (!isRigid) {
  81. float elasticity = distance > nominalDistance? pullElasticity: pushElasticity;
  82. force = 1f - elasticity;
  83. }
  84. force *= 1f - nominalDistance / distance;
  85. Vector3 offset = direction * force;
  86. solver.chain[chain1Index].nodes[0].solverPosition += offset * crossFade;
  87. solver.chain[chain2Index].nodes[0].solverPosition -= offset * inverseCrossFade;
  88. }
  89. }
  90. [System.Serializable]
  91. public enum Smoothing {
  92. None,
  93. Exponential,
  94. Cubic
  95. }
  96. /// <summary>
  97. /// The pin weight. If closer to 1, the chain will be less influenced by child chains.
  98. /// </summary>
  99. [Range(0f, 1f)]
  100. public float pin;
  101. /// <summary>
  102. /// The weight of pulling the parent chain.
  103. /// </summary>
  104. [Range(0f, 1f)]
  105. public float pull = 1f;
  106. /// <summary>
  107. /// The weight of the end-effector pushing the shoulder/thigh when the end-effector is close to it.
  108. /// </summary>
  109. [Range(0f, 1f)]
  110. public float push;
  111. /// <summary>
  112. /// The amount of push force transferred to the parent (from hand or foot to the body).
  113. /// </summary>
  114. [Range(-1f, 1f)]
  115. public float pushParent;
  116. /// <summary>
  117. /// Only used in 3 segmented chains, pulls the first node closer to the third node.
  118. /// </summary>
  119. [Range(0f, 1f)]
  120. public float reach = 0.1f;
  121. /// <summary>
  122. /// Smoothing the effect of the Reach with the expense of some accuracy.
  123. /// </summary>
  124. public Smoothing reachSmoothing = Smoothing.Exponential;
  125. /// <summary>
  126. /// Smoothing the effect of the Push.
  127. /// </summary>
  128. public Smoothing pushSmoothing = Smoothing.Exponential;
  129. /// <summary>
  130. /// The nodes in this chain.
  131. /// </summary>
  132. public IKSolver.Node[] nodes = new IKSolver.Node[0];
  133. /// <summary>
  134. /// The child chains.
  135. /// </summary>
  136. public int[] children = new int[0];
  137. /// <summary>
  138. /// The child constraints are used for example for fixing the distance between left upper arm and right upper arm
  139. /// </summary>
  140. public ChildConstraint[] childConstraints = new ChildConstraint[0];
  141. /// <summary>
  142. /// Gets the bend constraint (if this chain has 3 segments).
  143. /// </summary>
  144. /// <value>The bend constraint.</value>
  145. public IKConstraintBend bendConstraint = new IKConstraintBend();
  146. #endregion Main Interface
  147. private float rootLength;
  148. private bool initiated;
  149. private float length;
  150. private float distance;
  151. private IKSolver.Point p;
  152. private float reachForce;
  153. private float pullParentSum;
  154. private float[] crossFades;
  155. private float sqrMag1, sqrMag2, sqrMagDif;
  156. private const float maxLimbLength = 0.99999f;
  157. public FBIKChain() {}
  158. public FBIKChain (float pin, float pull, params Transform[] nodeTransforms) {
  159. this.pin = pin;
  160. this.pull = pull;
  161. SetNodes(nodeTransforms);
  162. children = new int[0];
  163. }
  164. /*
  165. * Set nodes to the following bone transforms.
  166. * */
  167. public void SetNodes(params Transform[] boneTransforms) {
  168. nodes = new IKSolver.Node[boneTransforms.Length];
  169. for (int i = 0; i < boneTransforms.Length; i++) {
  170. nodes[i] = new IKSolver.Node(boneTransforms[i]);
  171. }
  172. }
  173. public int GetNodeIndex(Transform boneTransform) {
  174. for (int i = 0; i < nodes.Length; i++) {
  175. if (nodes[i].transform == boneTransform) return i;
  176. }
  177. return -1;
  178. }
  179. /*
  180. * Check if this chain is valid or not.
  181. * */
  182. public bool IsValid(ref string message) {
  183. if (nodes.Length == 0) {
  184. message = "FBIK chain contains no nodes.";
  185. return false;
  186. }
  187. foreach (IKSolver.Node node in nodes) if (node.transform == null) {
  188. message = "Node transform is null in FBIK chain.";
  189. return false;
  190. }
  191. return true;
  192. }
  193. /*
  194. * Initiating the chain.
  195. * */
  196. public void Initiate(IKSolverFullBody solver) {
  197. initiated = false;
  198. foreach (IKSolver.Node node in nodes) {
  199. node.solverPosition = node.transform.position;
  200. }
  201. // Calculating bone lengths
  202. CalculateBoneLengths(solver);
  203. // Initiating child constraints
  204. foreach (ChildConstraint c in childConstraints) c.Initiate(solver as IKSolverFullBody);
  205. // Initiating the bend constraint
  206. if (nodes.Length == 3) {
  207. bendConstraint.SetBones(nodes[0].transform, nodes[1].transform, nodes[2].transform);
  208. bendConstraint.Initiate(solver as IKSolverFullBody);
  209. }
  210. crossFades = new float[children.Length];
  211. initiated = true;
  212. }
  213. /*
  214. * Before updating the chain
  215. * */
  216. public void ReadPose(IKSolverFullBody solver, bool fullBody) {
  217. if (!initiated) return;
  218. for (int i = 0; i < nodes.Length; i++) {
  219. nodes[i].solverPosition = nodes[i].transform.position + nodes[i].offset;
  220. }
  221. // Calculating bone lengths
  222. CalculateBoneLengths(solver);
  223. if (fullBody) {
  224. // Pre-update child constraints
  225. for (int i = 0; i < childConstraints.Length; i++) childConstraints[i].OnPreSolve(solver);
  226. if (children.Length > 0) {
  227. // PullSum
  228. float pullSum = nodes[nodes.Length - 1].effectorPositionWeight;
  229. for (int i = 0; i < children.Length; i++) pullSum += solver.chain[children[i]].nodes[0].effectorPositionWeight * solver.chain[children[i]].pull;
  230. pullSum = Mathf.Clamp(pullSum, 1f, Mathf.Infinity);
  231. // CrossFades
  232. for (int i = 0; i < children.Length; i++) {
  233. crossFades[i] = (solver.chain[children[i]].nodes[0].effectorPositionWeight * solver.chain[children[i]].pull) / pullSum;
  234. }
  235. }
  236. // Finding the total pull force by all child chains
  237. pullParentSum = 0f;
  238. for (int i = 0; i < children.Length; i++) pullParentSum += solver.chain[children[i]].pull;
  239. pullParentSum = Mathf.Clamp(pullParentSum, 1f, Mathf.Infinity);
  240. // Reach force
  241. if (nodes.Length == 3) {
  242. reachForce = reach * Mathf.Clamp(nodes[2].effectorPositionWeight, 0f, 1f);
  243. } else reachForce = 0f;
  244. if (push > 0f && nodes.Length > 1) distance = Vector3.Distance(nodes[0].transform.position, nodes[nodes.Length - 1].transform.position);
  245. }
  246. }
  247. // Calculates all bone lengths as well as lenghts between the chains
  248. private void CalculateBoneLengths(IKSolverFullBody solver) {
  249. // Calculating bone lengths
  250. length = 0f;
  251. for (int i = 0; i < nodes.Length - 1; i++) {
  252. nodes[i].length = Vector3.Distance(nodes[i].transform.position, nodes[i + 1].transform.position);
  253. length += nodes[i].length;
  254. if (nodes[i].length == 0) {
  255. Warning.Log("Bone " + nodes[i].transform.name + " - " + nodes[i + 1].transform.name + " length is zero, can not solve.", nodes[i].transform);
  256. return;
  257. }
  258. }
  259. for (int i = 0; i < children.Length; i++) {
  260. solver.chain[children[i]].rootLength = (solver.chain[children[i]].nodes[0].transform.position - nodes[nodes.Length - 1].transform.position).magnitude;
  261. if (solver.chain[children[i]].rootLength == 0f) {
  262. return;
  263. }
  264. }
  265. if (nodes.Length == 3) {
  266. // Square magnitude of the limb lengths
  267. sqrMag1 = nodes[0].length * nodes[0].length;
  268. sqrMag2 = nodes[1].length * nodes[1].length;
  269. sqrMagDif = sqrMag1 - sqrMag2;
  270. }
  271. }
  272. #region Recursive Methods
  273. /*
  274. * Reaching limbs
  275. * */
  276. public void Reach(IKSolverFullBody solver) {
  277. if (!initiated) return;
  278. // Solve children first
  279. for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Reach(solver);
  280. if (reachForce <= 0f) return;
  281. Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
  282. if (solverDirection == Vector3.zero) return;
  283. float solverLength = solverDirection.magnitude;
  284. //Reaching
  285. Vector3 straight = (solverDirection / solverLength) * length;
  286. float delta = Mathf.Clamp(solverLength / length, 1 - reachForce, 1 + reachForce) - 1f;
  287. delta = Mathf.Clamp(delta + reachForce, -1f, 1f);
  288. // Smoothing the effect of Reach with the expense of some accuracy
  289. switch (reachSmoothing) {
  290. case Smoothing.Exponential:
  291. delta *= delta;
  292. break;
  293. case Smoothing.Cubic:
  294. delta *= delta * delta;
  295. break;
  296. }
  297. Vector3 offset = straight * Mathf.Clamp(delta, 0f, solverLength);
  298. nodes[0].solverPosition += offset * (1f - nodes[0].effectorPositionWeight);
  299. nodes[2].solverPosition += offset;
  300. }
  301. /*
  302. * End-effectors pushing the first nodes
  303. * */
  304. public Vector3 Push(IKSolverFullBody solver) {
  305. Vector3 sum = Vector3.zero;
  306. // Get the push from the children
  307. for (int i = 0; i < children.Length; i++) {
  308. sum += solver.chain[children[i]].Push(solver) * solver.chain[children[i]].pushParent;
  309. }
  310. // Apply the push from a child
  311. nodes[nodes.Length - 1].solverPosition += sum;
  312. // Calculating the push of THIS chain (passed on to the parent as we're in a recursive method)
  313. if (nodes.Length < 2) return Vector3.zero;
  314. if (push <= 0f) return Vector3.zero;
  315. Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
  316. float solverLength = solverDirection.magnitude;
  317. if (solverLength == 0f) return Vector3.zero;
  318. // Get the push force factor
  319. float f = 1f - (solverLength / distance);
  320. if (f <= 0f) return Vector3.zero;
  321. // Push smoothing
  322. switch (pushSmoothing) {
  323. case Smoothing.Exponential:
  324. f *= f;
  325. break;
  326. case Smoothing.Cubic:
  327. f *= f * f;
  328. break;
  329. }
  330. // The final push force
  331. Vector3 p = -solverDirection * f * push;
  332. nodes[0].solverPosition += p;
  333. return p;
  334. }
  335. /*
  336. * Applying trigonometric IK solver on the 3 segmented chains to relieve tension from the solver and increase accuracy.
  337. * */
  338. public void SolveTrigonometric(IKSolverFullBody solver, bool calculateBendDirection = false) {
  339. if (!initiated) return;
  340. // Solve children first
  341. for (int i = 0; i < children.Length; i++) solver.chain[children[i]].SolveTrigonometric(solver, calculateBendDirection);
  342. if (nodes.Length != 3) return;
  343. // Direction of the limb in solver
  344. Vector3 solverDirection = nodes[2].solverPosition - nodes[0].solverPosition;
  345. // Distance between the first and the last node solver positions
  346. float solverLength = solverDirection.magnitude;
  347. if (solverLength == 0f) return;
  348. // Maximim stretch of the limb
  349. float maxMag = Mathf.Clamp(solverLength, 0f, length * maxLimbLength);
  350. Vector3 direction = (solverDirection / solverLength) * maxMag;
  351. // Get the general world space bending direction
  352. Vector3 bendDirection = calculateBendDirection && bendConstraint.initiated? bendConstraint.GetDir(solver): nodes[1].solverPosition - nodes[0].solverPosition;
  353. // Get the direction to the trigonometrically solved position of the second node
  354. Vector3 toBendPoint = GetDirToBendPoint(direction, bendDirection, maxMag);
  355. // Position the second node
  356. nodes[1].solverPosition = nodes[0].solverPosition + toBendPoint;
  357. }
  358. /*
  359. * Stage 1 of the FABRIK algorithm
  360. * */
  361. public void Stage1(IKSolverFullBody solver) {
  362. // Stage 1
  363. for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage1(solver);
  364. // If is the last chain in this hierarchy, solve immediatelly and return
  365. if (children.Length == 0) {
  366. ForwardReach(nodes[nodes.Length - 1].solverPosition);
  367. return;
  368. }
  369. Vector3 centroid = nodes[nodes.Length - 1].solverPosition;
  370. // Satisfying child constraints
  371. SolveChildConstraints(solver);
  372. // Finding the centroid position of all child chains according to their individual pull weights
  373. for (int i = 0; i < children.Length; i++) {
  374. Vector3 childPosition = solver.chain[children[i]].nodes[0].solverPosition;
  375. if (solver.chain[children[i]].rootLength > 0) {
  376. childPosition = SolveFABRIKJoint(nodes[nodes.Length - 1].solverPosition, solver.chain[children[i]].nodes[0].solverPosition, solver.chain[children[i]].rootLength);
  377. }
  378. if (pullParentSum > 0) centroid += (childPosition - nodes[nodes.Length - 1].solverPosition) * (solver.chain[children[i]].pull / pullParentSum);
  379. }
  380. // Forward reach to the centroid (unless pinned)
  381. ForwardReach(Vector3.Lerp(centroid, nodes[nodes.Length - 1].solverPosition, pin));
  382. }
  383. /*
  384. * Stage 2 of the FABRIK algorithm.
  385. * */
  386. public void Stage2(IKSolverFullBody solver, Vector3 position) {
  387. // Stage 2
  388. BackwardReach(position);
  389. int it = Mathf.Clamp(solver.iterations, 2, 4);
  390. // Iterating child constraints and child chains to make sure they are not conflicting
  391. if (childConstraints.Length > 0) {
  392. for (int i = 0; i < it; i++) SolveConstraintSystems(solver);
  393. }
  394. // Stage 2 for the children
  395. for (int i = 0; i < children.Length; i++) solver.chain[children[i]].Stage2(solver, nodes[nodes.Length - 1].solverPosition);
  396. }
  397. /*
  398. * Iterating child constraints and child chains to make sure they are not conflicting
  399. * */
  400. public void SolveConstraintSystems(IKSolverFullBody solver) {
  401. // Satisfy child constraints
  402. SolveChildConstraints(solver);
  403. for (int i = 0; i < children.Length; i++) {
  404. SolveLinearConstraint(nodes[nodes.Length - 1], solver.chain[children[i]].nodes[0], crossFades[i], solver.chain[children[i]].rootLength);
  405. }
  406. }
  407. #endregion Recursive Methods
  408. /*
  409. * Interpolates the joint position to match the bone's length
  410. */
  411. private Vector3 SolveFABRIKJoint(Vector3 pos1, Vector3 pos2, float length) {
  412. return pos2 + (pos1 - pos2).normalized * length;
  413. }
  414. /*
  415. * Calculates the bend direction based on the law of cosines (from IKSolverTrigonometric).
  416. * */
  417. protected Vector3 GetDirToBendPoint(Vector3 direction, Vector3 bendDirection, float directionMagnitude) {
  418. float x = ((directionMagnitude * directionMagnitude) + sqrMagDif) / 2f / directionMagnitude;
  419. float y = (float)Math.Sqrt(Mathf.Clamp(sqrMag1 - x * x, 0, Mathf.Infinity));
  420. if (direction == Vector3.zero) return Vector3.zero;
  421. return Quaternion.LookRotation(direction, bendDirection) * new Vector3(0f, y, x);
  422. }
  423. /*
  424. * Satisfying child constraints
  425. * */
  426. private void SolveChildConstraints(IKSolverFullBody solver) {
  427. for (int i = 0; i < childConstraints.Length; i++) {
  428. childConstraints[i].Solve(solver);
  429. }
  430. }
  431. /*
  432. * Solve simple linear constraint
  433. * */
  434. private void SolveLinearConstraint(IKSolver.Node node1, IKSolver.Node node2, float crossFade, float distance) {
  435. Vector3 dir = node2.solverPosition - node1.solverPosition;
  436. float mag = dir.magnitude;
  437. if (distance == mag) return;
  438. if (mag == 0f) return;
  439. Vector3 offset = dir * (1f - distance / mag);
  440. node1.solverPosition += offset * crossFade;
  441. node2.solverPosition -= offset * (1f - crossFade);
  442. }
  443. /*
  444. * FABRIK Forward reach
  445. * */
  446. public void ForwardReach(Vector3 position) {
  447. // Lerp last node's solverPosition to position
  448. nodes[nodes.Length - 1].solverPosition = position;
  449. for (int i = nodes.Length - 2; i > -1; i--) {
  450. // Finding joint positions
  451. nodes[i].solverPosition = SolveFABRIKJoint(nodes[i].solverPosition, nodes[i + 1].solverPosition, nodes[i].length);
  452. }
  453. }
  454. /*
  455. * FABRIK Backward reach
  456. * */
  457. private void BackwardReach(Vector3 position) {
  458. // Solve forst node only if it already hasn't been solved in SolveConstraintSystems
  459. if (rootLength > 0) position = SolveFABRIKJoint(nodes[0].solverPosition, position, rootLength);
  460. nodes[0].solverPosition = position;
  461. // Finding joint positions
  462. for (int i = 1; i < nodes.Length; i++) {
  463. nodes[i].solverPosition = SolveFABRIKJoint(nodes[i].solverPosition, nodes[i - 1].solverPosition, nodes[i - 1].length);
  464. }
  465. }
  466. }
  467. }