SlopeCollider.cs 7.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227
  1. using System;
  2. using UnityEngine;
  3. public enum RotateDirection
  4. {
  5. Unknown,
  6. None,
  7. Forward,
  8. Backward
  9. }
  10. internal struct SlopeHit
  11. {
  12. internal float distance;
  13. internal bool even;
  14. internal float angle;
  15. internal Vector3 hitPoint;
  16. internal SlopeHit(float distance, bool even, float angle, Vector3 hitPoint)
  17. {
  18. this.distance = distance;
  19. this.even = even;
  20. this.angle = angle;
  21. this.hitPoint = hitPoint;
  22. }
  23. public override string ToString()
  24. {
  25. return $"SlopeHit(distance = {distance}, even = {even}, angle = {angle})";
  26. }
  27. }
  28. public class SlopeCollider : MonoBehaviour
  29. {
  30. public int collisionLayer = 1;
  31. public SphereCollider rearWheelCollider;
  32. public SphereCollider frontWheelCollider;
  33. public Transform bike;
  34. private const float THRESHOLD = 0.003f;
  35. private float distRwFw;
  36. private RotateDirection currentRotateDirection = RotateDirection.Unknown;
  37. private Transform frontWheelContactPoint;
  38. private Transform rearWheelContactPoint;
  39. private void Start()
  40. {
  41. frontWheelContactPoint = frontWheelCollider.transform;
  42. rearWheelContactPoint = rearWheelCollider.transform;
  43. distRwFw = Mathf.Abs(frontWheelContactPoint.localPosition.z - rearWheelContactPoint.localPosition.z);
  44. }
  45. private void FixedUpdate()
  46. {
  47. var fwContact = frontWheelContactPoint.position;
  48. var rwContact = rearWheelContactPoint.position;
  49. var fwHit = DrawRay(fwContact);
  50. var rwHit = DrawRay(rwContact);
  51. if (!fwHit.HasValue || !rwHit.HasValue)
  52. {
  53. return;
  54. }
  55. Debug.Log("----Slope Collider----");
  56. Debug.Log($"\tfwHit: {fwHit}");
  57. Debug.Log($"\trwHit: {rwHit}");
  58. var fw = fwHit.Value;
  59. var rw = rwHit.Value;
  60. var fwDist = fw.distance;
  61. var rwDist = rw.distance;
  62. var distDif = fwDist - rwDist;
  63. if (fwDist >= THRESHOLD && rwDist >= THRESHOLD)
  64. {
  65. var deltaY = fwDist <= rwDist ? fwDist : rwDist;
  66. bike.Translate(0,-deltaY,0);
  67. Debug.Log($"Translated {-deltaY} on y");
  68. }
  69. else if (fw.distance >= THRESHOLD)
  70. {
  71. var angle = Mathf.Atan(fw.distance / distRwFw) * Mathf.Rad2Deg;
  72. Debug.Log($"Rotating {angle} deg");
  73. bike.RotateAround(rearWheelContactPoint.position, rearWheelContactPoint.right, angle);
  74. }
  75. else if (rw.distance >= THRESHOLD)
  76. {
  77. var angle = Mathf.Atan(rw.distance / distRwFw) * Mathf.Rad2Deg;
  78. bike.RotateAround(frontWheelContactPoint.position, frontWheelContactPoint.right, -angle);
  79. Debug.Log($"Rotating {-angle} deg");
  80. }
  81. //if ((fwHit?.even ?? true) && (rwHit?.even ?? true)) return;
  82. /*var distFw = fwHit?.distance ?? 0f;
  83. var distRw = rwHit?.distance ?? 0f;
  84. var distDif = distFw - distRw;
  85. if (fwHit.HasValue && rwHit.HasValue)
  86. {
  87. var fw = fwHit.Value;
  88. var rw = rwHit.Value;
  89. if (!fw.even && rw.even)
  90. {
  91. if (fw.hitPoint.y > rw.hitPoint.y && currentRotateDirection != RotateDirection.Forward)
  92. {
  93. currentRotateDirection = RotateDirection.Backward;
  94. }else if (currentRotateDirection != RotateDirection.Backward)
  95. {
  96. currentRotateDirection = RotateDirection.Forward;
  97. }
  98. }else if (fw.even && !rw.even)
  99. {
  100. if (fw.hitPoint.y > rw.hitPoint.y && currentRotateDirection != RotateDirection.Backward)
  101. {
  102. currentRotateDirection = RotateDirection.Forward;
  103. }
  104. else if(currentRotateDirection != RotateDirection.Forward)
  105. {
  106. currentRotateDirection = RotateDirection.Backward;
  107. }
  108. }
  109. else
  110. {
  111. currentRotateDirection = RotateDirection.None; //TODO: maybe not
  112. }
  113. }
  114. else
  115. {
  116. if (distDif < -THRESHOLD && currentRotateDirection != RotateDirection.Forward)
  117. {
  118. currentRotateDirection = RotateDirection.Backward;
  119. }else if (distDif > THRESHOLD && currentRotateDirection != RotateDirection.Backward)
  120. {
  121. currentRotateDirection = RotateDirection.Forward;
  122. }
  123. else
  124. {
  125. currentRotateDirection = RotateDirection.None;
  126. }
  127. }
  128. Debug.Log("CurrentRotateDirection = " + currentRotateDirection);
  129. Debug.Log("----Slope Collider----");
  130. Debug.Log($"\tfwHit: {fwHit}");
  131. Debug.Log($"\trwHit: {rwHit}");
  132. if (currentRotateDirection == RotateDirection.Backward) //rwDist > fwDist
  133. {
  134. //rear wheel in the air -> rotate around front wheel -> make it go uphill
  135. var angle = Mathf.Atan(distRw / distRwFw) * Mathf.Rad2Deg;
  136. bike.RotateAround(fwContact, frontWheelContactPoint.right, -angle);
  137. }
  138. else if (currentRotateDirection == RotateDirection.Forward)
  139. {
  140. //front wheel in the air -> rotate around rear wheel
  141. var angle = Mathf.Atan(distFw / distRwFw) * Mathf.Rad2Deg;
  142. bike.RotateAround(rwContact, rearWheelContactPoint.right, angle);
  143. }
  144. /*if (rwHit.HasValue)
  145. {
  146. var rwHitVal = rwHit.Value;
  147. if (rwHitVal.angle > 0.01f)
  148. {
  149. t.RotateAround(fwContact, frontWheelContactPoint.right, -rwHitVal.angle);
  150. }
  151. /*if (rwHitVal.even)
  152. {
  153. //begin of slope
  154. Rotate(rwHitVal.distance, rwHitVal.distance - (fwHit?.distance ?? 0f), fwContact, rwContact);
  155. }
  156. }
  157. if (fwHit.HasValue)
  158. {
  159. var fwHitVal = fwHit.Value;
  160. if (fwHitVal.angle > 0.01f)
  161. {
  162. t.RotateAround(rwContact, rearWheelContactPoint.right, -fwHitVal.angle);
  163. }
  164. /*if (fwHitVal.even)
  165. {
  166. //end of slope
  167. Rotate(fwHitVal.distance, fwHitVal.distance - (rwHit?.distance ?? 0f), fwContact, rwContact);
  168. }
  169. }*/
  170. }
  171. private SlopeHit? DrawRay(Vector3 start)
  172. {
  173. var layerMask = 1 << collisionLayer;
  174. RaycastHit hit;
  175. // Does the ray intersect any objects excluding the player layer
  176. if (Physics.Raycast(start, -bike.up, out hit, 20f, layerMask))
  177. {
  178. Debug.DrawRay(start, -bike.up * hit.distance, Color.green);
  179. //Debug.DrawRay(hit.point, hit.normal, Color.blue);
  180. var isUneven = hit.collider.gameObject.CompareTag(StreetPartMetaTag.TAG_UNEVEN);
  181. var first = -(-bike.up * hit.distance);
  182. var second = (hit.normal);
  183. var angle = Mathf.Acos(Vector3.Dot(first, second) / first.magnitude * second.magnitude) * Mathf.Rad2Deg;
  184. //Debug.Log("Dot Product: " + Vector3.Dot(first, second));
  185. //Debug.Log("Angle: " + angle);
  186. return new SlopeHit(hit.distance, !isUneven, angle, hit.point);
  187. }
  188. if (Physics.Raycast(start, bike.up, out hit, 20f, layerMask))
  189. {
  190. Debug.DrawRay(start, -bike.up * hit.distance, Color.green);
  191. //Debug.DrawRay(hit.point, hit.normal, Color.blue);
  192. var isUneven = hit.collider.gameObject.CompareTag(StreetPartMetaTag.TAG_UNEVEN);
  193. var first = -(-bike.up * hit.distance);
  194. var second = (hit.normal);
  195. var angle = Mathf.Acos(Vector3.Dot(first, second) / first.magnitude * second.magnitude) * Mathf.Rad2Deg;
  196. //Debug.Log("Dot Product: " + Vector3.Dot(first, second));
  197. //Debug.Log("Angle: " + angle);
  198. return new SlopeHit(-hit.distance, !isUneven, angle, hit.point);
  199. }
  200. //Debug.DrawRay(start, -t.up * 20f, Color.red);
  201. return null;
  202. }
  203. }