DynamicBoneCollider.cs 7.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235
  1. using UnityEngine;
  2. [AddComponentMenu("Dynamic Bone/Dynamic Bone Collider")]
  3. public class DynamicBoneCollider : DynamicBoneColliderBase
  4. {
  5. #if UNITY_5_3_OR_NEWER
  6. [Tooltip("The radius of the sphere or capsule.")]
  7. #endif
  8. public float m_Radius = 0.5f;
  9. #if UNITY_5_3_OR_NEWER
  10. [Tooltip("The height of the capsule.")]
  11. #endif
  12. public float m_Height = 0;
  13. void OnValidate()
  14. {
  15. m_Radius = Mathf.Max(m_Radius, 0);
  16. m_Height = Mathf.Max(m_Height, 0);
  17. }
  18. public override bool Collide(ref Vector3 particlePosition, float particleRadius)
  19. {
  20. float radius = m_Radius * Mathf.Abs(transform.lossyScale.x);
  21. float h = m_Height * 0.5f - m_Radius;
  22. if (h <= 0)
  23. {
  24. if (m_Bound == Bound.Outside)
  25. return OutsideSphere(ref particlePosition, particleRadius, transform.TransformPoint(m_Center), radius);
  26. else
  27. return InsideSphere(ref particlePosition, particleRadius, transform.TransformPoint(m_Center), radius);
  28. }
  29. else
  30. {
  31. Vector3 c0 = m_Center;
  32. Vector3 c1 = m_Center;
  33. switch (m_Direction)
  34. {
  35. case Direction.X:
  36. c0.x -= h;
  37. c1.x += h;
  38. break;
  39. case Direction.Y:
  40. c0.y -= h;
  41. c1.y += h;
  42. break;
  43. case Direction.Z:
  44. c0.z -= h;
  45. c1.z += h;
  46. break;
  47. }
  48. if (m_Bound == Bound.Outside)
  49. return OutsideCapsule(ref particlePosition, particleRadius, transform.TransformPoint(c0), transform.TransformPoint(c1), radius);
  50. else
  51. return InsideCapsule(ref particlePosition, particleRadius, transform.TransformPoint(c0), transform.TransformPoint(c1), radius);
  52. }
  53. }
  54. static bool OutsideSphere(ref Vector3 particlePosition, float particleRadius, Vector3 sphereCenter, float sphereRadius)
  55. {
  56. float r = sphereRadius + particleRadius;
  57. float r2 = r * r;
  58. Vector3 d = particlePosition - sphereCenter;
  59. float len2 = d.sqrMagnitude;
  60. // if is inside sphere, project onto sphere surface
  61. if (len2 > 0 && len2 < r2)
  62. {
  63. float len = Mathf.Sqrt(len2);
  64. particlePosition = sphereCenter + d * (r / len);
  65. return true;
  66. }
  67. return false;
  68. }
  69. static bool InsideSphere(ref Vector3 particlePosition, float particleRadius, Vector3 sphereCenter, float sphereRadius)
  70. {
  71. float r = sphereRadius - particleRadius;
  72. float r2 = r * r;
  73. Vector3 d = particlePosition - sphereCenter;
  74. float len2 = d.sqrMagnitude;
  75. // if is outside sphere, project onto sphere surface
  76. if (len2 > r2)
  77. {
  78. float len = Mathf.Sqrt(len2);
  79. particlePosition = sphereCenter + d * (r / len);
  80. return true;
  81. }
  82. return false;
  83. }
  84. static bool OutsideCapsule(ref Vector3 particlePosition, float particleRadius, Vector3 capsuleP0, Vector3 capsuleP1, float capsuleRadius)
  85. {
  86. float r = capsuleRadius + particleRadius;
  87. float r2 = r * r;
  88. Vector3 dir = capsuleP1 - capsuleP0;
  89. Vector3 d = particlePosition - capsuleP0;
  90. float t = Vector3.Dot(d, dir);
  91. if (t <= 0)
  92. {
  93. // check sphere1
  94. float len2 = d.sqrMagnitude;
  95. if (len2 > 0 && len2 < r2)
  96. {
  97. float len = Mathf.Sqrt(len2);
  98. particlePosition = capsuleP0 + d * (r / len);
  99. return true;
  100. }
  101. }
  102. else
  103. {
  104. float dl = dir.sqrMagnitude;
  105. if (t >= dl)
  106. {
  107. // check sphere2
  108. d = particlePosition - capsuleP1;
  109. float len2 = d.sqrMagnitude;
  110. if (len2 > 0 && len2 < r2)
  111. {
  112. float len = Mathf.Sqrt(len2);
  113. particlePosition = capsuleP1 + d * (r / len);
  114. return true;
  115. }
  116. }
  117. else if (dl > 0)
  118. {
  119. // check cylinder
  120. t /= dl;
  121. d -= dir * t;
  122. float len2 = d.sqrMagnitude;
  123. if (len2 > 0 && len2 < r2)
  124. {
  125. float len = Mathf.Sqrt(len2);
  126. particlePosition += d * ((r - len) / len);
  127. return true;
  128. }
  129. }
  130. }
  131. return false;
  132. }
  133. static bool InsideCapsule(ref Vector3 particlePosition, float particleRadius, Vector3 capsuleP0, Vector3 capsuleP1, float capsuleRadius)
  134. {
  135. float r = capsuleRadius - particleRadius;
  136. float r2 = r * r;
  137. Vector3 dir = capsuleP1 - capsuleP0;
  138. Vector3 d = particlePosition - capsuleP0;
  139. float t = Vector3.Dot(d, dir);
  140. if (t <= 0)
  141. {
  142. // check sphere1
  143. float len2 = d.sqrMagnitude;
  144. if (len2 > r2)
  145. {
  146. float len = Mathf.Sqrt(len2);
  147. particlePosition = capsuleP0 + d * (r / len);
  148. return true;
  149. }
  150. }
  151. else
  152. {
  153. float dl = dir.sqrMagnitude;
  154. if (t >= dl)
  155. {
  156. // check sphere2
  157. d = particlePosition - capsuleP1;
  158. float len2 = d.sqrMagnitude;
  159. if (len2 > r2)
  160. {
  161. float len = Mathf.Sqrt(len2);
  162. particlePosition = capsuleP1 + d * (r / len);
  163. return true;
  164. }
  165. }
  166. else if (dl > 0)
  167. {
  168. // check cylinder
  169. t /= dl;
  170. d -= dir * t;
  171. float len2 = d.sqrMagnitude;
  172. if (len2 > r2)
  173. {
  174. float len = Mathf.Sqrt(len2);
  175. particlePosition += d * ((r - len) / len);
  176. return true;
  177. }
  178. }
  179. }
  180. return false;
  181. }
  182. void OnDrawGizmosSelected()
  183. {
  184. if (!enabled)
  185. return;
  186. if (m_Bound == Bound.Outside)
  187. Gizmos.color = Color.yellow;
  188. else
  189. Gizmos.color = Color.magenta;
  190. float radius = m_Radius * Mathf.Abs(transform.lossyScale.x);
  191. float h = m_Height * 0.5f - m_Radius;
  192. if (h <= 0)
  193. {
  194. Gizmos.DrawWireSphere(transform.TransformPoint(m_Center), radius);
  195. }
  196. else
  197. {
  198. Vector3 c0 = m_Center;
  199. Vector3 c1 = m_Center;
  200. switch (m_Direction)
  201. {
  202. case Direction.X:
  203. c0.x -= h;
  204. c1.x += h;
  205. break;
  206. case Direction.Y:
  207. c0.y -= h;
  208. c1.y += h;
  209. break;
  210. case Direction.Z:
  211. c0.z -= h;
  212. c1.z += h;
  213. break;
  214. }
  215. Gizmos.DrawWireSphere(transform.TransformPoint(c0), radius);
  216. Gizmos.DrawWireSphere(transform.TransformPoint(c1), radius);
  217. }
  218. }
  219. }