VehicleAI.cs 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379
  1. // Traffic Simulation
  2. // https://github.com/mchrbn/unity-traffic-simulation
  3. using System.Collections;
  4. using System.Collections.Generic;
  5. using UnityEngine;
  6. namespace TrafficSimulation {
  7. /*
  8. [-] Check prefab #6 issue
  9. [-] Deaccelerate when see stop in front
  10. [-] Smooth sharp turns when two segments are linked
  11. */
  12. public struct Target{
  13. public int segment;
  14. public int waypoint;
  15. }
  16. public enum Status{
  17. GO,
  18. STOP,
  19. SLOW_DOWN
  20. }
  21. public class VehicleAI : MonoBehaviour
  22. {
  23. [Header("Traffic System")]
  24. [Tooltip("Current active traffic system")]
  25. public TrafficSystem trafficSystem;
  26. [Tooltip("Determine when the vehicle has reached its target. Can be used to \"anticipate\" earlier the next waypoint (the higher this number his, the earlier it will anticipate the next waypoint)")]
  27. public float waypointThresh = 6;
  28. [Header("Radar")]
  29. [Tooltip("Empty gameobject from where the rays will be casted")]
  30. public Transform raycastAnchor;
  31. [Tooltip("Length of the casted rays")]
  32. public float raycastLength = 5;
  33. [Tooltip("Spacing between each rays")]
  34. public int raySpacing = 2;
  35. [Tooltip("Number of rays to be casted")]
  36. public int raysNumber = 6;
  37. [Tooltip("If detected vehicle is below this distance, ego vehicle will stop")]
  38. public float emergencyBrakeThresh = 2f;
  39. [Tooltip("If detected vehicle is below this distance (and above, above distance), ego vehicle will slow down")]
  40. public float slowDownThresh = 4f;
  41. [HideInInspector] public Status vehicleStatus = Status.GO;
  42. private WheelDrive wheelDrive;
  43. private float initMaxSpeed = 0;
  44. private int pastTargetSegment = -1;
  45. private Target currentTarget;
  46. private Target futureTarget;
  47. void Start()
  48. {
  49. wheelDrive = this.GetComponent<WheelDrive>();
  50. if(trafficSystem == null)
  51. return;
  52. initMaxSpeed = wheelDrive.maxSpeed;
  53. SetWaypointVehicleIsOn();
  54. }
  55. private void FixedUpdate()
  56. {
  57. if (trafficSystem == null)
  58. return;
  59. WaypointChecker();
  60. MoveVehicle();
  61. }
  62. void WaypointChecker()
  63. {
  64. GameObject waypoint = trafficSystem.segments[currentTarget.segment].waypoints[currentTarget.waypoint].gameObject;
  65. //Position of next waypoint relative to the car
  66. Vector3 wpDist = this.transform.InverseTransformPoint(new Vector3(waypoint.transform.position.x, this.transform.position.y, waypoint.transform.position.z));
  67. //Go to next waypoint if arrived to current
  68. if(wpDist.magnitude < waypointThresh)
  69. {
  70. //Get next target
  71. currentTarget.waypoint++;
  72. if(currentTarget.waypoint >= trafficSystem.segments[currentTarget.segment].waypoints.Count)
  73. {
  74. pastTargetSegment = currentTarget.segment;
  75. currentTarget.segment = futureTarget.segment;
  76. currentTarget.waypoint = 0;
  77. }
  78. //Get future target
  79. futureTarget.waypoint = currentTarget.waypoint + 1;
  80. if(futureTarget.waypoint >= trafficSystem.segments[currentTarget.segment].waypoints.Count)
  81. {
  82. futureTarget.waypoint = 0;
  83. futureTarget.segment = GetNextSegmentId();
  84. }
  85. }
  86. }
  87. void MoveVehicle()
  88. {
  89. //Default, full acceleration, no break and no steering
  90. float acc = 1;
  91. float brake = 0;
  92. float steering = 0;
  93. wheelDrive.maxSpeed = initMaxSpeed;
  94. //Calculate if there is a planned turn
  95. Transform targetTransform = trafficSystem.segments[currentTarget.segment].waypoints[currentTarget.waypoint].transform;
  96. Transform futureTargetTransform = trafficSystem.segments[futureTarget.segment].waypoints[futureTarget.waypoint].transform;
  97. Vector3 futureVel = futureTargetTransform.position - targetTransform.position;
  98. float futureSteering = Mathf.Clamp(this.transform.InverseTransformDirection(futureVel.normalized).x, -1, 1);
  99. //Check if the car has to stop
  100. if(vehicleStatus == Status.STOP)
  101. {
  102. //Debug.Log(gameObject.name + ":STOP");
  103. acc = 0;
  104. brake = 1;
  105. //wheelDrive.maxSpeed = Mathf.Min(wheelDrive.maxSpeed / 2f, 5f);
  106. wheelDrive.maxSpeed = 0f;
  107. }
  108. else{
  109. //Not full acceleration if have to slow down
  110. if(vehicleStatus == Status.SLOW_DOWN){
  111. //Debug.Log(gameObject.name + ": SLOW DOWN");
  112. acc = .3f;
  113. brake = 0f;
  114. }
  115. //If planned to steer, decrease the speed
  116. if(futureSteering > .3f || futureSteering < -.3f)
  117. {
  118. //Debug.Log( gameObject.name + " :Future Steering. Decrease Speed");
  119. wheelDrive.maxSpeed = Mathf.Min(wheelDrive.maxSpeed, wheelDrive.steeringSpeedMax);
  120. }
  121. //2. Check if there are obstacles which are detected by the radar
  122. float hitDist;
  123. GameObject obstacle = GetDetectedObstacles(out hitDist);
  124. //Check if we hit something
  125. if(obstacle != null){
  126. //Debug.Log(gameObject.name + ": Obstacle detected " + obstacle.name + "!");
  127. WheelDrive otherVehicle = null;
  128. otherVehicle = obstacle.GetComponent<WheelDrive>();
  129. ///////////////////////////////////////////////////////////////
  130. //Differenciate between other vehicles AI and generic obstacles (including controlled vehicle, if any)
  131. if(otherVehicle != null){
  132. //Check if it's front vehicle
  133. float dotFront = Vector3.Dot(this.transform.forward, otherVehicle.transform.forward);
  134. //If detected front vehicle max speed is lower than ego vehicle, then decrease ego vehicle max speed
  135. //if(otherVehicle.maxSpeed < wheelDrive.maxSpeed && otherVehicle.maxSpeed > 0 && dotFront > .8f)
  136. if (otherVehicle.maxSpeed < wheelDrive.maxSpeed && otherVehicle.maxSpeed > 0 && dotFront > .8f)
  137. {
  138. float ms = Mathf.Max(wheelDrive.GetSpeedMS(otherVehicle.maxSpeed) - .5f, .1f);
  139. //Debug.Log(obstacle.name + " slower. lower speed of " +gameObject.name + " to " + wheelDrive.GetSpeedUnit(ms) + "!");
  140. wheelDrive.maxSpeed = wheelDrive.GetSpeedUnit(ms);
  141. }
  142. else if(otherVehicle.maxSpeed == 0f)
  143. {
  144. //Debug.Log(gameObject.name + ": Stopping because of " + obstacle.name +"!");
  145. acc = 0;
  146. brake = 1;
  147. wheelDrive.maxSpeed = 0f;
  148. }
  149. //If the two vehicles are too close, and facing the same direction, brake the ego vehicle
  150. if(hitDist < emergencyBrakeThresh && dotFront > .8f)
  151. {
  152. //Debug.Log(gameObject.name + "Two close, same direction. Breaking");
  153. acc = 0;
  154. brake = 1;
  155. wheelDrive.maxSpeed = Mathf.Max(wheelDrive.maxSpeed / 2f, wheelDrive.minSpeed);
  156. }
  157. //If the two vehicles are too close, and not facing same direction, slight make the ego vehicle go backward
  158. else if(hitDist < emergencyBrakeThresh && dotFront <= .8f)
  159. {
  160. //Debug.Log(gameObject.name + "Too Close, not same direction. Go Backward");
  161. acc = -.3f;
  162. brake = 0f;
  163. wheelDrive.maxSpeed = Mathf.Max(wheelDrive.maxSpeed / 2f, wheelDrive.minSpeed);
  164. //Check if the vehicle we are close to is located on the right or left then apply according steering to try to make it move
  165. float dotRight = Vector3.Dot(this.transform.forward, otherVehicle.transform.right);
  166. if(dotRight > 0.1f)//Right
  167. {
  168. //Debug.Log("Steer left");
  169. steering = -.2f;
  170. }else if(dotRight < -0.1f) //Left
  171. {
  172. //Debug.Log("Steer right");
  173. steering = .2f;
  174. }else //Middle
  175. {
  176. //Debug.Log("Steer Middle");
  177. steering = -.7f;
  178. }
  179. }
  180. //If the two vehicles are getting close, slow down their speed
  181. else if(hitDist < slowDownThresh && acc > 0)
  182. {
  183. //Debug.Log(gameObject.name + ": Too Close, Slow Down!");
  184. acc = .5f;
  185. brake = 0f;
  186. //wheelDrive.maxSpeed = Mathf.Max(wheelDrive.maxSpeed / 1.5f, wheelDrive.minSpeed);
  187. }
  188. }
  189. ///////////////////////////////////////////////////////////////////
  190. // Generic obstacles
  191. else
  192. {
  193. //Emergency brake if getting too close
  194. if(hitDist < emergencyBrakeThresh)
  195. {
  196. acc = 0;
  197. brake = 1;
  198. wheelDrive.maxSpeed = Mathf.Max(wheelDrive.maxSpeed / 2f, wheelDrive.minSpeed);
  199. }
  200. //Otherwise if getting relatively close decrease speed
  201. else if(hitDist < slowDownThresh)
  202. {
  203. acc = .5f;
  204. brake = 0f;
  205. }
  206. }
  207. }
  208. //Check if we need to steer to follow path
  209. if(acc > 0f)
  210. {
  211. Vector3 desiredVel = trafficSystem.segments[currentTarget.segment].waypoints[currentTarget.waypoint].transform.position - this.transform.position;
  212. steering = Mathf.Clamp(this.transform.InverseTransformDirection(desiredVel.normalized).x, -1f, 1f);
  213. }
  214. }
  215. //Move the car
  216. wheelDrive.Move(acc, steering, brake);
  217. }
  218. GameObject GetDetectedObstacles(out float _hitDist)
  219. {
  220. GameObject detectedObstacle = null;
  221. float minDist = 1000f;
  222. float initRay = (raysNumber / 2f) * raySpacing;
  223. float hitDist = -1f;
  224. for(float a=-initRay; a<=initRay; a+=raySpacing)
  225. {
  226. CastRay(raycastAnchor.transform.position, a, this.transform.forward, raycastLength, out detectedObstacle, out hitDist);
  227. if(detectedObstacle == null){
  228. continue;
  229. }
  230. float dist = Vector3.Distance(this.transform.position, detectedObstacle.transform.position);
  231. if(dist < minDist)
  232. {
  233. minDist = dist;
  234. break;
  235. }
  236. }
  237. _hitDist = hitDist;
  238. return detectedObstacle;
  239. }
  240. void CastRay(Vector3 _anchor, float _angle, Vector3 _dir, float _length, out GameObject _outObstacle, out float _outHitDistance){
  241. _outObstacle = null;
  242. _outHitDistance = -1f;
  243. //Draw raycast
  244. Debug.DrawRay(_anchor, Quaternion.Euler(0, _angle, 0) * _dir * _length, new Color(1, 0, 0, 0.5f));
  245. //Detect hit only on the autonomous vehicle layer
  246. int layer = 1 << LayerMask.NameToLayer("AutonomousVehicle");
  247. int finalMask = layer;
  248. foreach(string layerName in trafficSystem.collisionLayers)
  249. {
  250. int id = 1 << LayerMask.NameToLayer(layerName);
  251. finalMask = finalMask | id;
  252. }
  253. RaycastHit hit;
  254. if(Physics.Raycast(_anchor, Quaternion.Euler(0, _angle, 0) * _dir, out hit, _length, finalMask))
  255. {
  256. _outObstacle = hit.collider.gameObject;
  257. _outHitDistance = hit.distance;
  258. }
  259. }
  260. int GetNextSegmentId()
  261. {
  262. if(trafficSystem.segments[currentTarget.segment].nextSegments.Count == 0)
  263. {
  264. return 0;
  265. }
  266. int c = Random.Range(0, trafficSystem.segments[currentTarget.segment].nextSegments.Count);
  267. return trafficSystem.segments[currentTarget.segment].nextSegments[c].id;
  268. }
  269. void SetWaypointVehicleIsOn()
  270. {
  271. //Find current target
  272. foreach(Segment segment in trafficSystem.segments)
  273. {
  274. if(segment.IsOnSegment(this.transform.position))
  275. {
  276. currentTarget.segment = segment.id;
  277. //Find nearest waypoint to start within the segment
  278. float minDist = float.MaxValue;
  279. for(int j=0; j<trafficSystem.segments[currentTarget.segment].waypoints.Count; j++)
  280. {
  281. float d = Vector3.Distance(this.transform.position, trafficSystem.segments[currentTarget.segment].waypoints[j].transform.position);
  282. //Only take in front points
  283. Vector3 lSpace = this.transform.InverseTransformPoint(trafficSystem.segments[currentTarget.segment].waypoints[j].transform.position);
  284. if(d < minDist && lSpace.z > 0)
  285. {
  286. minDist = d;
  287. currentTarget.waypoint = j;
  288. }
  289. }
  290. break;
  291. }
  292. }
  293. //Get future target
  294. futureTarget.waypoint = currentTarget.waypoint + 1;
  295. futureTarget.segment = currentTarget.segment;
  296. if(futureTarget.waypoint >= trafficSystem.segments[currentTarget.segment].waypoints.Count)
  297. {
  298. futureTarget.waypoint = 0;
  299. futureTarget.segment = GetNextSegmentId();
  300. }
  301. }
  302. public int GetSegmentVehicleIsIn()
  303. {
  304. int vehicleSegment = currentTarget.segment;
  305. bool isOnSegment = trafficSystem.segments[vehicleSegment].IsOnSegment(this.transform.position);
  306. if(!isOnSegment)
  307. {
  308. bool isOnPSegement = trafficSystem.segments[pastTargetSegment].IsOnSegment(this.transform.position);
  309. if(isOnPSegement)
  310. vehicleSegment = pastTargetSegment;
  311. }
  312. return vehicleSegment;
  313. }
  314. }
  315. }