AutoPilotController.js 7.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195
  1. /*
  2. * AutoPilotController.js
  3. *
  4. * @author realor
  5. */
  6. import { Controller } from './Controller.js'
  7. import { Solid } from '../core/Solid.js'
  8. import * as THREE from '../lib/three.module.js'
  9. class AutoPilotController extends Controller {
  10. constructor(object, name) {
  11. super(object, name)
  12. this.distance = 0
  13. this.maxVelocity = 1
  14. this.maxAcceleration = 1
  15. this.vehiclesGroup = 'vehicles'
  16. this.trafficLightsGroup = 'trafficLights'
  17. this._animate = this.animate.bind(this)
  18. this._position0 = this.object.position.clone()
  19. this._direction = new THREE.Vector3()
  20. this.object.updateMatrix()
  21. this.object.updateMatrixWorld()
  22. var te = this.object.matrix.elements
  23. this._direction.set(te[4], te[5], te[6]).normalize() // relative to parent
  24. this._vector1 = new THREE.Vector3()
  25. this._vector2 = new THREE.Vector3(0, 1, 0) // y axis
  26. this.object.localToWorld(this._vector1)
  27. this.object.localToWorld(this._vector2)
  28. this._directionWorld = new THREE.Vector3()
  29. this._directionWorld.subVectors(this._vector2, this._vector1).normalize()
  30. this._velocity = 0
  31. this._acceleration = 0
  32. this._sleep = 0
  33. }
  34. onStart() {
  35. const application = this.application
  36. this._vehicles = application.scene.getObjectByName(this.vehiclesGroup)
  37. this._trafficLights = application.scene.getObjectByName(this.trafficLightsGroup)
  38. application.updateVisibility(this.object, true)
  39. application.addEventListener('animation', this._animate)
  40. }
  41. onStop() {
  42. const application = this.application
  43. application.removeEventListener('animation', this._animate)
  44. }
  45. animate(event) {
  46. if (this._sleep > 0) {
  47. this._sleep -= event.delta
  48. if (this._sleep <= 0) {
  49. // reset position, velocity and acceleration
  50. this.object.position.copy(this._position0)
  51. this._velocity = 0
  52. this._acceleration = 0
  53. this.object.visible = true
  54. }
  55. } else {
  56. var safetyDistance = 8
  57. var reductionDistance = 15
  58. var vehicle = this.getClosestVehicle()
  59. var trafficLight = this.getClosestTrafficLight()
  60. var obstacle = vehicle.distance < trafficLight.distance ? vehicle : trafficLight
  61. console.info(obstacle)
  62. if (obstacle.distance >= reductionDistance || this._velocity < obstacle.velocity) {
  63. if (obstacle.distance > safetyDistance) {
  64. this._acceleration = parseFloat(this.maxAcceleration)
  65. }
  66. } // match velocities
  67. else {
  68. var vdelta = this._velocity - obstacle.velocity
  69. this._acceleration = -(vdelta * vdelta) / Math.max(obstacle.distance - safetyDistance, 0.01)
  70. }
  71. this._velocity += this._acceleration * event.delta
  72. if (this._velocity > this.maxVelocity) {
  73. this._velocity = parseFloat(this.maxVelocity)
  74. } else if (this._velocity < 0) {
  75. this._velocity = 0
  76. }
  77. var delta = this._velocity * event.delta
  78. // TODO: add v-axis vector
  79. this.object.position.x += delta * this._direction.x
  80. this.object.position.y += delta * this._direction.y
  81. this.object.position.z += delta * this._direction.z
  82. if (this.object.position.distanceTo(this._position0) > this.distance) {
  83. this._sleep = Math.random() * 10
  84. this.object.visible = false
  85. }
  86. var changeEvent = { type: 'nodeChanged', objects: [this.object], source: this }
  87. this.application.notifyEventListeners('scene', changeEvent)
  88. }
  89. }
  90. getClosestVehicle() {
  91. var minDistance = 1000
  92. var velocity = 0
  93. var closestVehicle = null
  94. if (this._vehicles) {
  95. var position = new THREE.Vector3()
  96. this.object.localToWorld(position)
  97. var vehiclePosition = new THREE.Vector3()
  98. var vector = new THREE.Vector3()
  99. var children = this._vehicles.children
  100. for (var i = 0; i < children.length; i++) {
  101. var vehicle = children[i]
  102. if (vehicle !== this.object && vehicle.visible && vehicle instanceof Solid) {
  103. vehiclePosition.set(0, 0, 0)
  104. vehicle.localToWorld(vehiclePosition)
  105. vector.subVectors(vehiclePosition, position)
  106. var vehicleDistance = vector.length()
  107. if (vehicleDistance < minDistance) {
  108. var linearDistance = vector.dot(this._directionWorld)
  109. if (linearDistance > 0) {
  110. // is not behind
  111. var margin = Math.sqrt(vehicleDistance * vehicleDistance - linearDistance * linearDistance)
  112. if (margin < 2) {
  113. // car width
  114. closestVehicle = vehicle
  115. minDistance = vehicleDistance
  116. if (vehicle.controllers && vehicle.controllers[0] && vehicle.controllers[0]._velocity) {
  117. velocity = vehicle.controllers[0]._velocity
  118. } else {
  119. velocity = 0
  120. }
  121. }
  122. }
  123. }
  124. }
  125. }
  126. }
  127. return { distance: minDistance, velocity: velocity, object: closestVehicle }
  128. }
  129. getClosestTrafficLight() {
  130. var minDistance = 1000
  131. var closestLight = null
  132. if (this._trafficLights) {
  133. var position = new THREE.Vector3()
  134. this.object.localToWorld(position)
  135. var lightPosition = new THREE.Vector3()
  136. var lightDirection = new THREE.Vector3()
  137. var vector = new THREE.Vector3()
  138. var children = this._trafficLights.children
  139. for (var i = 0; i < children.length; i++) {
  140. var light = children[i]
  141. if (light.userData['Dynamic'] && light.userData['Dynamic']['state'] > 1) {
  142. // not green
  143. lightPosition.set(0, 0, 0)
  144. light.localToWorld(lightPosition)
  145. vector.subVectors(lightPosition, position)
  146. var lightDistance = vector.length()
  147. if (lightDistance < minDistance) {
  148. vector.normalize()
  149. if (vector.dot(this._directionWorld) > 0.8) {
  150. // light is in front of us
  151. this._vector1.set(0, 0, 0)
  152. this._vector2.set(0, 1, 0) // y axis
  153. light.localToWorld(this._vector1)
  154. light.localToWorld(this._vector2)
  155. lightDirection.subVectors(this._vector2, this._vector1).normalize()
  156. if (lightDirection.dot(this._directionWorld) < -0.8) {
  157. // light looks at car
  158. minDistance = lightDistance
  159. closestLight = light
  160. }
  161. }
  162. }
  163. }
  164. }
  165. }
  166. return { distance: minDistance, velocity: 0, object: closestLight }
  167. }
  168. }
  169. Controller.addClass(AutoPilotController)
  170. export { AutoPilotController }