SlopeCollider.cs 8.4 KB

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