2
0

SteamVR_IK.cs 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172
  1. //======= Copyright (c) Valve Corporation, All rights reserved. ===============
  2. //
  3. // Purpose: Simple two bone ik solver.
  4. //
  5. //=============================================================================
  6. using UnityEngine;
  7. namespace Valve.VR
  8. {
  9. public class SteamVR_IK : MonoBehaviour
  10. {
  11. public Transform target;
  12. public Transform start, joint, end;
  13. public Transform poleVector, upVector;
  14. public float blendPct = 1.0f;
  15. [HideInInspector]
  16. public Transform startXform, jointXform, endXform;
  17. void LateUpdate()
  18. {
  19. const float epsilon = 0.001f;
  20. if (blendPct < epsilon)
  21. return;
  22. var preUp = upVector ? upVector.up : Vector3.Cross(end.position - start.position, joint.position - start.position).normalized;
  23. var targetPosition = target.position;
  24. var targetRotation = target.rotation;
  25. Vector3 forward, up, result = joint.position;
  26. Solve(start.position, targetPosition, poleVector.position,
  27. (joint.position - start.position).magnitude,
  28. (end.position - joint.position).magnitude,
  29. ref result, out forward, out up);
  30. if (up == Vector3.zero)
  31. return;
  32. var startPosition = start.position;
  33. var jointPosition = joint.position;
  34. var endPosition = end.position;
  35. var startRotationLocal = start.localRotation;
  36. var jointRotationLocal = joint.localRotation;
  37. var endRotationLocal = end.localRotation;
  38. var startParent = start.parent;
  39. var jointParent = joint.parent;
  40. var endParent = end.parent;
  41. var startScale = start.localScale;
  42. var jointScale = joint.localScale;
  43. var endScale = end.localScale;
  44. if (startXform == null)
  45. {
  46. startXform = new GameObject("startXform").transform;
  47. startXform.parent = transform;
  48. }
  49. startXform.position = startPosition;
  50. startXform.LookAt(joint, preUp);
  51. start.parent = startXform;
  52. if (jointXform == null)
  53. {
  54. jointXform = new GameObject("jointXform").transform;
  55. jointXform.parent = startXform;
  56. }
  57. jointXform.position = jointPosition;
  58. jointXform.LookAt(end, preUp);
  59. joint.parent = jointXform;
  60. if (endXform == null)
  61. {
  62. endXform = new GameObject("endXform").transform;
  63. endXform.parent = jointXform;
  64. }
  65. endXform.position = endPosition;
  66. end.parent = endXform;
  67. startXform.LookAt(result, up);
  68. jointXform.LookAt(targetPosition, up);
  69. endXform.rotation = targetRotation;
  70. start.parent = startParent;
  71. joint.parent = jointParent;
  72. end.parent = endParent;
  73. end.rotation = targetRotation; // optionally blend?
  74. // handle blending in/out
  75. if (blendPct < 1.0f)
  76. {
  77. start.localRotation = Quaternion.Slerp(startRotationLocal, start.localRotation, blendPct);
  78. joint.localRotation = Quaternion.Slerp(jointRotationLocal, joint.localRotation, blendPct);
  79. end.localRotation = Quaternion.Slerp(endRotationLocal, end.localRotation, blendPct);
  80. }
  81. // restore scale so it doesn't blow out
  82. start.localScale = startScale;
  83. joint.localScale = jointScale;
  84. end.localScale = endScale;
  85. }
  86. public static bool Solve(
  87. Vector3 start, // shoulder / hip
  88. Vector3 end, // desired hand / foot position
  89. Vector3 poleVector, // point to aim elbow / knee toward
  90. float jointDist, // distance from start to elbow / knee
  91. float targetDist, // distance from joint to hand / ankle
  92. ref Vector3 result, // original and output elbow / knee position
  93. out Vector3 forward, out Vector3 up) // plane formed by root, joint and target
  94. {
  95. var totalDist = jointDist + targetDist;
  96. var start2end = end - start;
  97. var poleVectorDir = (poleVector - start).normalized;
  98. var baseDist = start2end.magnitude;
  99. result = start;
  100. const float epsilon = 0.001f;
  101. if (baseDist < epsilon)
  102. {
  103. // move jointDist toward jointTarget
  104. result += poleVectorDir * jointDist;
  105. forward = Vector3.Cross(poleVectorDir, Vector3.up);
  106. up = Vector3.Cross(forward, poleVectorDir).normalized;
  107. }
  108. else
  109. {
  110. forward = start2end * (1.0f / baseDist);
  111. up = Vector3.Cross(forward, poleVectorDir).normalized;
  112. if (baseDist + epsilon < totalDist)
  113. {
  114. // calculate the area of the triangle to determine its height
  115. var p = (totalDist + baseDist) * 0.5f; // half perimeter
  116. if (p > jointDist + epsilon && p > targetDist + epsilon)
  117. {
  118. var A = Mathf.Sqrt(p * (p - jointDist) * (p - targetDist) * (p - baseDist));
  119. var height = 2.0f * A / baseDist; // distance of joint from line between root and target
  120. var dist = Mathf.Sqrt((jointDist * jointDist) - (height * height));
  121. var right = Vector3.Cross(up, forward); // no need to normalized - already orthonormal
  122. result += (forward * dist) + (right * height);
  123. return true; // in range
  124. }
  125. else
  126. {
  127. // move jointDist toward jointTarget
  128. result += poleVectorDir * jointDist;
  129. }
  130. }
  131. else
  132. {
  133. // move elboDist toward target
  134. result += forward * jointDist;
  135. }
  136. }
  137. return false; // edge cases
  138. }
  139. }
  140. }