2
0

FloppyHand.cs 9.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233
  1. //======= Copyright (c) Valve Corporation, All rights reserved. ===============
  2. using UnityEngine;
  3. using System.Collections;
  4. namespace Valve.VR.InteractionSystem.Sample
  5. {
  6. public class FloppyHand : MonoBehaviour
  7. {
  8. protected float fingerFlexAngle = 140;
  9. public SteamVR_Action_Single squeezyAction = SteamVR_Input.GetAction<SteamVR_Action_Single>("Squeeze");
  10. public SteamVR_Input_Sources inputSource;
  11. [System.Serializable]
  12. public class Finger
  13. {
  14. public float mass;
  15. [Range(0, 1)]
  16. public float pos;
  17. public Vector3 forwardAxis;
  18. public SkinnedMeshRenderer renderer;
  19. [HideInInspector]
  20. public SteamVR_Action_Single squeezyAction;
  21. public SteamVR_Input_Sources inputSource;
  22. public Transform[] bones;
  23. public Transform referenceBone;
  24. public Vector2 referenceAngles;
  25. public enum eulerAxis
  26. {
  27. X, Y, Z
  28. }
  29. public eulerAxis referenceAxis;
  30. [HideInInspector]
  31. public float flexAngle;
  32. private Vector3[] rotation;
  33. private Vector3[] velocity;
  34. private Transform[] boneTips;
  35. private Vector3[] oldTipPosition;
  36. private Vector3[] oldTipDelta;
  37. private Vector3[,] inertiaSmoothing;
  38. float squeezySmooth;
  39. private int inertiaSteps = 10;
  40. private float k = 400;
  41. private float damping = 8;
  42. private Quaternion[] startRot;
  43. public void ApplyForce(Vector3 worldForce)
  44. {
  45. for (int i = 0; i < startRot.Length; i++)
  46. {
  47. velocity[i] += worldForce / 50;
  48. }
  49. }
  50. public void Init()
  51. {
  52. startRot = new Quaternion[bones.Length];
  53. rotation = new Vector3[bones.Length];
  54. velocity = new Vector3[bones.Length];
  55. oldTipPosition = new Vector3[bones.Length];
  56. oldTipDelta = new Vector3[bones.Length];
  57. boneTips = new Transform[bones.Length];
  58. inertiaSmoothing = new Vector3[bones.Length, inertiaSteps];
  59. for (int i = 0; i < bones.Length; i++)
  60. {
  61. startRot[i] = bones[i].localRotation;
  62. if (i < bones.Length - 1)
  63. {
  64. boneTips[i] = bones[i + 1];
  65. }
  66. }
  67. }
  68. public void UpdateFinger(float deltaTime)
  69. {
  70. if (deltaTime == 0)
  71. return;
  72. float squeezeValue = 0;
  73. if (squeezyAction != null && squeezyAction.GetActive(inputSource))
  74. squeezeValue = squeezyAction.GetAxis(inputSource);
  75. squeezySmooth = Mathf.Lerp(squeezySmooth, Mathf.Sqrt(squeezeValue), deltaTime * 10);
  76. if (renderer.sharedMesh.blendShapeCount > 0)
  77. {
  78. renderer.SetBlendShapeWeight(0, squeezySmooth * 100);
  79. }
  80. float boneRot = 0;
  81. if (referenceAxis == eulerAxis.X)
  82. boneRot = referenceBone.localEulerAngles.x;
  83. if (referenceAxis == eulerAxis.Y)
  84. boneRot = referenceBone.localEulerAngles.y;
  85. if (referenceAxis == eulerAxis.Z)
  86. boneRot = referenceBone.localEulerAngles.z;
  87. boneRot = FixAngle(boneRot);
  88. pos = Mathf.InverseLerp(referenceAngles.x, referenceAngles.y, boneRot);
  89. if (mass > 0)
  90. {
  91. for (int boneIndex = 0; boneIndex < bones.Length; boneIndex++)
  92. {
  93. bool useOffset = boneTips[boneIndex] != null;
  94. if (useOffset) // inertia sim
  95. {
  96. Vector3 offset = (boneTips[boneIndex].localPosition - bones[boneIndex].InverseTransformPoint(oldTipPosition[boneIndex])) / deltaTime;
  97. Vector3 inertia = (offset - oldTipDelta[boneIndex]) / deltaTime;
  98. oldTipDelta[boneIndex] = offset;
  99. Vector3 drag = offset * -2;
  100. inertia *= -2f;
  101. for (int offsetIndex = inertiaSteps - 1; offsetIndex > 0; offsetIndex--) // offset inertia steps
  102. {
  103. inertiaSmoothing[boneIndex, offsetIndex] = inertiaSmoothing[boneIndex, offsetIndex - 1];
  104. }
  105. inertiaSmoothing[boneIndex, 0] = inertia;
  106. Vector3 smoothedInertia = Vector3.zero;
  107. for (int offsetIndex = 0; offsetIndex < inertiaSteps; offsetIndex++) // offset inertia steps
  108. {
  109. smoothedInertia += inertiaSmoothing[boneIndex, offsetIndex];
  110. }
  111. smoothedInertia = smoothedInertia / inertiaSteps;
  112. //if (boneIndex == 0 && Input.GetKey(KeyCode.Space))
  113. // Debug.Log(smoothedInertia);
  114. smoothedInertia = PowVector(smoothedInertia / 20, 3) * 20;
  115. Vector3 forward = forwardAxis;
  116. Vector3 forwardDrag = forwardAxis + drag;
  117. Vector3 forwardInertia = forwardAxis + smoothedInertia;
  118. Quaternion dragQuaternion = Quaternion.FromToRotation(forward, forwardDrag);
  119. Quaternion inertiaQuaternion = Quaternion.FromToRotation(forward, forwardInertia);
  120. velocity[boneIndex] += FixVector(dragQuaternion.eulerAngles) * 2 * deltaTime;
  121. velocity[boneIndex] += FixVector(inertiaQuaternion.eulerAngles) * 50 * deltaTime;
  122. velocity[boneIndex] = Vector3.ClampMagnitude(velocity[boneIndex], 1000);
  123. }
  124. Vector3 targetPos = pos * Vector3.right * (flexAngle / bones.Length);
  125. Vector3 springForce = -k * (rotation[boneIndex] - targetPos);
  126. var dampingForce = damping * velocity[boneIndex];
  127. var force = springForce - dampingForce;
  128. var acceleration = force / mass;
  129. velocity[boneIndex] += acceleration * deltaTime;
  130. rotation[boneIndex] += velocity[boneIndex] * Time.deltaTime;
  131. rotation[boneIndex] = Vector3.ClampMagnitude(rotation[boneIndex], 180);
  132. if (useOffset)
  133. {
  134. oldTipPosition[boneIndex] = boneTips[boneIndex].position;
  135. }
  136. }
  137. }
  138. else
  139. {
  140. Debug.LogError("<b>[SteamVR Interaction]</b> finger mass is zero");
  141. }
  142. }
  143. public void ApplyTransforms()
  144. {
  145. for (int i = 0; i < bones.Length; i++)
  146. {
  147. bones[i].localRotation = startRot[i];
  148. bones[i].Rotate(rotation[i], Space.Self);
  149. }
  150. }
  151. private Vector3 FixVector(Vector3 ang)
  152. {
  153. return new Vector3(FixAngle(ang.x), FixAngle(ang.y), FixAngle(ang.z));
  154. }
  155. private float FixAngle(float ang)
  156. {
  157. if (ang > 180)
  158. ang = -360 + ang;
  159. return ang;
  160. }
  161. private Vector3 PowVector(Vector3 vector, float power)
  162. {
  163. Vector3 sign = new Vector3(Mathf.Sign(vector.x), Mathf.Sign(vector.y), Mathf.Sign(vector.z));
  164. vector.x = Mathf.Pow(Mathf.Abs(vector.x), power) * sign.x;
  165. vector.y = Mathf.Pow(Mathf.Abs(vector.y), power) * sign.y;
  166. vector.z = Mathf.Pow(Mathf.Abs(vector.z), power) * sign.z;
  167. return vector;
  168. }
  169. }
  170. public Finger[] fingers;
  171. public Vector3 constforce;
  172. private void Start()
  173. {
  174. for (int fingerIndex = 0; fingerIndex < fingers.Length; fingerIndex++)
  175. {
  176. fingers[fingerIndex].Init();
  177. fingers[fingerIndex].flexAngle = fingerFlexAngle;
  178. fingers[fingerIndex].squeezyAction = squeezyAction;
  179. fingers[fingerIndex].inputSource = inputSource;
  180. }
  181. }
  182. private void Update()
  183. {
  184. for (int fingerIndex = 0; fingerIndex < fingers.Length; fingerIndex++)
  185. {
  186. fingers[fingerIndex].ApplyForce(constforce);
  187. fingers[fingerIndex].UpdateFinger(Time.deltaTime);
  188. fingers[fingerIndex].ApplyTransforms();
  189. }
  190. }
  191. }
  192. }