refactoring car emulator

car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt

@@ -3,14 +3,15 @@ package control.emulator
 import CarState
 import RouteRequest
 import RouteResponse
+import SonarRequest
 import SonarResponse
 import control.Controller
 import encodeProtoBuf
+import geometry.Line
+import geometry.Vector
 import room.Room
-import room.Line
 import setTimeout
 import kotlin.Pair
-import SonarRequest
 
 class ControllerEmulator : Controller {
 
@@ -55,12 +56,7 @@ class ControllerEmulator : Controller {
 
         val distances = arrayListOf<Int>()
         angles.forEach { angle ->
-            //need angle in [0,360)
+            val angleFinal = getSensorAngle(angle, carAngle)
-            var angleTmp = carAngle - angle
-            while (angleTmp < 0) {
-                angleTmp += 360
-            }
-            val angleFinal = angleTmp % 360
             val xSensor1: Int
             val ySensor1: Double
             //tg can be equal to inf if angle = 90 or 270. it vertical line. x1 = x0, y1 = y0+-[any number. eg 1]
@@ -78,42 +74,13 @@ class ControllerEmulator : Controller {
                     ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180)
                 }
             }
-            val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0, xSensor0 * ySensor1 - ySensor0 * xSensor1)
+            val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0,
-            val sensorVectorX = xSensor1 - xSensor0
+                    xSensor0 * ySensor1 - ySensor0 * xSensor1)
-            val sensorVectorY = ySensor1 - ySensor0
-            var distance = Int.MAX_VALUE
-            for (wall in Room.walls) {
-                val wallLine = wall.line
-                val coef = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A
-                if (Math.abs(coef) < 0.01) {
-                    //line is parallel.
-                    continue
-                }
-                val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / coef
-                val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / coef
 
-                //filters by direction and intersection position
+            val sensorVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xSensor1.toDouble(), ySensor1)
-                val IntersectionVectorX = xIntersection - xSensor0
+            val distance = getDistance(xSensor0, ySensor0, sensorLine, sensorVector)
-                val IntersectionVectorY = yIntersection - ySensor0
+            if (distance == -1) {
-                if (IntersectionVectorX * sensorVectorX + IntersectionVectorY * sensorVectorY < 0) {
+                distances.add(distance)
-                    continue
-                }
-                val wallVectorXTo = wall.xTo - xIntersection
-                val wallVectorYTo = wall.yTo - yIntersection
-                val wallVectorXFrom = wall.xFrom - xIntersection
-                val wallVectorYFrom = wall.yFrom - yIntersection
-                if (wallVectorXTo * wallVectorXFrom + wallVectorYTo * wallVectorYFrom > 0) {
-                    continue
-                }
-                val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
-                        + Math.pow(yIntersection - ySensor0, 2.0)))
-                if (currentDistance < distance) {
-                    distance = currentDistance
-                }
-            }
-            println("dist $distance")
-            if (distance < 5 || distance > 500) {
-                distances.add(-1)
             } else {
                 val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2
                 distances.add(distance + delta)
@@ -124,6 +91,49 @@ class ControllerEmulator : Controller {
         callBack.invoke(bytesMessage)
     }
 
+    private fun getDistance(xSensor0: Int, ySensor0: Int, sensorLine: Line, sensorVector: Vector): Int {
+        var result = Int.MAX_VALUE
+        for (wall in Room.walls) {
+            val wallLine = wall.line
+            val slope = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A
+            if (Math.abs(slope) < 0.01) {
+                //line is parallel.
+                continue
+            }
+            val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / slope
+            val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / slope
+
+            //filters by direction and intersection position
+            val intersectionVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xIntersection, yIntersection)
+            if (intersectionVector.scalarProduct(sensorVector) < 0) {
+                continue
+            }
+            val wallVector1 = Vector(xIntersection, yIntersection, wall.xTo.toDouble(), wall.yTo.toDouble())
+            val wallVector2 = Vector(xIntersection, yIntersection, wall.xFrom.toDouble(), wall.yFrom.toDouble())
+            if (wallVector1.scalarProduct(wallVector2) > 0) {
+                continue
+            }
+            val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+                    + Math.pow(yIntersection - ySensor0, 2.0)))
+            if (currentDistance < result) {
+                result = currentDistance
+            }
+        }
+        if (result < 5 || result > 500) {
+            return -1
+        }
+        return result
+    }
+
+    //return sensor angle in [0, 360) with considering car rotation
+    private fun getSensorAngle(requestAngle: Int, carAngle: Int): Int {
+        var angleTmp = carAngle - requestAngle
+        while (angleTmp < 0) {
+            angleTmp += 360
+        }
+        return angleTmp % 360
+    }
+
     //return random int from array
     private fun getRandomIntFrom(values: IntArray): Int {
         val randomValue = (Math.random() * 1000).toInt()//value in [0,999]

car_srv/kotlinSrv/src/geometry/Line.kt

@@ -1,4 +1,4 @@
-package room
+package geometry
 
 class Line(val A: Double, val B: Double, val C: Double) {
 }

car_srv/kotlinSrv/src/geometry/Vector.kt

@@ -0,0 +1,12 @@
+package geometry
+
+class Vector constructor(val x: Double, val y: Double) {
+
+    constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1)
+
+
+    fun scalarProduct(vector: Vector): Double {
+        return this.x * vector.x + this.y * vector.y
+    }
+
+}

car_srv/kotlinSrv/src/room/Room.kt

@@ -1,5 +1,7 @@
 package room
 
+import geometry.Line
+
 object Room {
 
     val upLine = Line(0.0, 1.0, -300.0)

car_srv/kotlinSrv/src/room/Wall.kt

@@ -1,5 +1,7 @@
 package room
 
+import geometry.Line
+
 class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) {
 
 }