Deleted old version of algorithm from RoomBypassingAlgorithm; Refactored new algo from RoomTest into RoomBypassingAlgorithm

2016-08-29 12:13:05 +03:00
parent e828a1e0f1
commit ad71f38061
4 changed files with 50 additions and 278 deletions
@@ -6,7 +6,6 @@ import io.netty.buffer.Unpooled
 import io.netty.handler.codec.http.*
 import objects.Car
 import java.util.concurrent.Exchanger
-import algorithm.RoomTest

 object DebugClInterface {

@@ -63,9 +62,6 @@ object DebugClInterface {
            "pos" -> {
                val tmp = algorithmImpl
                if (tmp is RoomBypassingAlgorithm) {
-                    println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}")
-                    println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
-                } else if (tmp is RoomTest) {
                    println("points: ${tmp.points}")
                    println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
                }
@@ -85,8 +81,7 @@ object DebugClInterface {
                } else 1

        if (algorithmImpl == null) {
-            algorithmImpl = RoomTest(environment.map.values.last(), exchanger)
-//            algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
+            algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
        }
        while (count > 0) {
            count--
@@ -1,37 +1,67 @@
 package algorithm

 import objects.Car
-import RouteMetricRequest
-import algorithm.geometry.*
 import java.util.concurrent.Exchanger
+import RouteMetricRequest
+import algorithm.geometry.AngleData
+import algorithm.geometry.Point

 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {

-    var wallAngleWithOX = 0.0//in radian
-    var wallLength = 0.0//sm
-    var errorCount = 0
+    val points = arrayListOf<Point>()
+    private val DISTANCE_TO_WALL_THRESHOLD = 60

    var carX = 0
    var carY = 0
    var carAngle = 0

-    private var prevPoint = Pair(0.0, 0.0)
+    override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState {
+        return CarState.WALL
+    }

-    override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState? {
-        val angleData0 = anglesDistances[0]
-        val angleData90 = anglesDistances[90]
-        if (angleData90 == null || angleData90.distance > 85) {
-            requiredAngles = getIntArray(0, 60, 90, 120, 180)
-            return CarState.OUTER
+    override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
+        val dist0 = anglesDistances[0]
+        val dist60 = anglesDistances[60]
+        val dist90 = anglesDistances[90]
+        val dist120 = anglesDistances[120]
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+        if (dist90 == null) {
+            resultBuilder.setDirections(getIntArray(RIGHT))
+            resultBuilder.setDistances(getIntArray(10))
+            return resultBuilder.build()
        }
-        val dist90 = angleData90.distance
-        if (dist90 < 20) {
-            return CarState.WALL
+        val sonarAngle = carAngle - 90
+        points.add(Point(
+                x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
+                y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
+        ))
+        if (dist120 == null || dist60 == null) {
+            resultBuilder.setDirections(getIntArray(FORWARD))
+            resultBuilder.setDistances(getIntArray(10))
+            return resultBuilder.build()
        }
-        if (angleData0 == null || angleData0.distance > 70) {
-            return CarState.WALL
+        if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) {
+            resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
+            resultBuilder.setDistances(getIntArray(20, 10))
+            return resultBuilder.build()
        }
-        return CarState.INNER
+        val abs = Math.abs(dist120.distance - dist60.distance)
+        if (abs > 10) {
+            val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT
+            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+            resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
+            return resultBuilder.build()
+        }
+        if (dist90.distance > 50 || dist90.distance < 25) {
+            val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT
+            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+            resultBuilder.setDistances(getIntArray(10, 10))
+            return resultBuilder.build()
+        }
+
+        resultBuilder.setDirections(getIntArray(FORWARD))
+        resultBuilder.setDistances(getIntArray(20))
+        return resultBuilder.build()
    }

    private fun getIntArray(vararg args: Int): IntArray {
@@ -59,165 +89,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
        }
    }

-    override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
-        val angleData0 = anglesDistances[0]
-        val angleData60 = anglesDistances[60]
-        val angleData90 = anglesDistances[90]
-        val angleData120 = anglesDistances[120]
-        val angleData180 = anglesDistances[180]
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        if (angleData120 == null || angleData90 == null || angleData60 == null) {
-            println("null distance!")
-            if (errorCount >= 3) {
-                errorCount = 0
-                resultBuilder.setDirections(getIntArray(BACKWARD))
-                resultBuilder.setDistances(getIntArray(15))
-                return resultBuilder.build()
-            }
-            errorCount++
-            return resultBuilder.build()
-        }
-        if (getPrevState() == null) {
-            //its first run, save right wall
-            RoomModel.lines.add(Line(0.0, 1.0, angleData90.distance.toDouble()))
-            prevPoint = Pair(0.0, -angleData90.distance.toDouble())
-            if (angleData0 != null) {
-                wallLength = angleData0.distance.toDouble()
-            }
-        }
-        when (state) {
-            CarState.WALL -> {
-                if (wallLength.toInt() == 0 && (angleData0 != null && angleData180 != null)) {
-                    wallLength = angleData0.distance.toDouble() + angleData180.distance.toDouble()
-                }
-                if (angleData90.distance > 40 || angleData90.distance < 20) {
-                    val rotationDirection = if (angleData90.distance > 40) RIGHT else LEFT
-                    resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-                    resultBuilder.setDistances(getIntArray(10, 35))
-                    return resultBuilder.build()
-                }
-
-                if (Math.abs(angleData120.distance - angleData60.distance) > 10) {
-                    val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT
-                    resultBuilder.setDirections(getIntArray(rotationDirection))
-                    resultBuilder.setDistances(getIntArray(15))
-                    return resultBuilder.build()
-                }
-
-                resultBuilder.setDirections(getIntArray(FORWARD))
-                resultBuilder.setDistances(getIntArray(35))
-                return resultBuilder.build()
-            }
-            CarState.INNER -> {
-
-                //в угле первое действие - встать максимально паралельно стене.
-                if (Math.abs(angleData120.distance - angleData60.distance) > 10) {
-                    val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT
-                    resultBuilder.setDirections(getIntArray(rotationDirection))
-                    resultBuilder.setDistances(getIntArray(12))
-                    return resultBuilder.build()
-                }
-
-                //если стоим паралельно - можно мерить угол
-                val pointsNextLine = arrayListOf<Point>()
-                for (angleData in anglesDistances.values) {
-                    if (angleData.angle <= 15) {
-                        pointsNextLine.add(angleData.toPoint(Util.radianToDegrees(wallAngleWithOX)))
-                    }
-                }
-                if (pointsNextLine.size < 3) {
-                    requiredAngles = IntArray(37, { it * 5 })
-                    return resultBuilder.build()
-                }
-                val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
-                val currentLine = RoomModel.lines.last()
-
-                val xIntersection = prevPoint.first + wallLength * Math.cos(wallAngleWithOX)
-                val yIntersection = prevPoint.second + wallLength * Math.sin(wallAngleWithOX)
-                nextLine.C = -nextLine.A * xIntersection - nextLine.B * yIntersection
-
-                var xOnNextLine = xIntersection + 1
-                var yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
-
-                //точка должна находится слева от нашей правой стены
-                if (currentLine.A * xOnNextLine + currentLine.B * yOnNextLine + currentLine.C < 0) {
-                    xOnNextLine = xIntersection - 1
-                    yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
-                }
-
-                val vectorNextLine = Vector(xIntersection, yIntersection, xOnNextLine, yOnNextLine)
-                val vectorCurrentLine = Vector(xIntersection, yIntersection, prevPoint.first, prevPoint.second)
-
-                val scalarProduct = vectorCurrentLine.scalarProduct(vectorNextLine)
-//                val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length()))
-                //todo
-                val angle = Math.PI / 2
-                val wallsAngleInDegrees = radiansToDegrees(angle)
-                wallAngleWithOX += degreesToRadian(180 - wallsAngleInDegrees.toInt())
-                prevPoint = Pair(xIntersection, yIntersection)
-                wallLength = 0.0
-                RoomModel.lines.add(nextLine)
-                resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
-                resultBuilder.setDistances(getIntArray((angle / 2).toInt(), 15, (angle / 2).toInt()))
-                return resultBuilder.build()
-            }
-            CarState.OUTER -> {
-                //todo calculate target
-                resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
-                resultBuilder.setDistances(getIntArray(45, 40, 45))
-                return resultBuilder.build()
-            }
-        }
-    }
-
    private fun degreesToRadian(angle: Int): Double {
        return Math.PI * angle / 180
    }

-    private fun radiansToDegrees(angle: Double): Double {
-        return angle * 180 / Math.PI
-    }
-
-
-    private fun approximatePointsByLine(points: Array<Point>): Line {
-
-//        val p1 = points.first()
-//        val p2 = points[1]
-//        return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first)
-
-        val sumX = points.sumByDouble { it.x }
-        val sumXQuad = points.sumByDouble { it.x * it.x }
-        val sumY = points.sumByDouble { it.y }
-        val sumXY = points.sumByDouble { it.y * it.x }
-
-        val pointsCount = points.size
-        val den = pointsCount * sumXQuad - sumX * sumX
-        if (Math.abs(den) < 0.001) {
-            return Line(1.0, 0.0, -points.first().x)
-        }
-        val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
-        val b = (sumY - sumX * k) / pointsCount
-
-        return Line(-k, 1.0, -b)
-    }
-
-
-    private fun calculateAngleWithWall(anglesDistances: MutableMap<Int, Double>): Double {
-        val dist60 = anglesDistances[60]
-        val dist120 = anglesDistances[120]
-
-        //Math.cos(60) = 1/2
-//        val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle
-//
-//        val hOnWall = getRangeToWall(wallLength, dist60, dist120)
-        return 0.0
-    }
-
-    //return height in triangle on side a
-    private fun getRangeToWall(a: Double, b: Double, c: Double): Double {
-        val halfPerimeter = (a + b + c) / 2
-        return Math.sqrt(halfPerimeter * (halfPerimeter - a) * (halfPerimeter - b) * (halfPerimeter - c)) * 2 / a
-    }
-
-
 }
@@ -24,7 +24,7 @@ object RoomModel {
    fun getUpdate(): Waypoints {

        val algorithm = DebugClInterface.algorithmImpl
-        if (algorithm == null || !(algorithm is RoomTest)) {
+        if (algorithm == null || !(algorithm is RoomBypassingAlgorithm)) {
            val emptyArr = IntArray(0)
            return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build()

@@ -1,96 +0,0 @@
-package algorithm
-
-import objects.Car
-import java.util.concurrent.Exchanger
-import RouteMetricRequest
-import algorithm.geometry.AngleData
-import algorithm.geometry.Point
-
-class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
-
-    val points = arrayListOf<Point>()
-    private val DISTANCE_TO_WALL_THRESHOLD = 60
-
-    var carX = 0
-    var carY = 0
-    var carAngle = 0
-
-    override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState {
-        return CarState.WALL
-    }
-
-    override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
-        val dist0 = anglesDistances[0]
-        val dist60 = anglesDistances[60]
-        val dist90 = anglesDistances[90]
-        val dist120 = anglesDistances[120]
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        if (dist90 == null) {
-            resultBuilder.setDirections(getIntArray(RIGHT))
-            resultBuilder.setDistances(getIntArray(10))
-            return resultBuilder.build()
-        }
-        val sonarAngle = carAngle - 90
-        points.add(Point(
-                x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
-                y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
-        ))
-        if (dist120 == null || dist60 == null) {
-            resultBuilder.setDirections(getIntArray(FORWARD))
-            resultBuilder.setDistances(getIntArray(10))
-            return resultBuilder.build()
-        }
-        if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) {
-            resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
-            resultBuilder.setDistances(getIntArray(20, 10))
-            return resultBuilder.build()
-        }
-        val abs = Math.abs(dist120.distance - dist60.distance)
-        if (abs > 10) {
-            val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT
-            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-            resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
-            return resultBuilder.build()
-        }
-        if (dist90.distance > 50 || dist90.distance < 25) {
-            val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT
-            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-            resultBuilder.setDistances(getIntArray(10, 10))
-            return resultBuilder.build()
-        }
-
-        resultBuilder.setDirections(getIntArray(FORWARD))
-        resultBuilder.setDistances(getIntArray(20))
-        return resultBuilder.build()
-    }
-
-    private fun getIntArray(vararg args: Int): IntArray {
-        return args
-    }
-
-    override fun afterGetCommand(route: RouteMetricRequest) {
-        route.directions.forEachIndexed { idx, direction ->
-            when (direction) {
-                FORWARD -> {
-                    carX += (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
-                    carY += (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
-                }
-                BACKWARD -> {
-                    carX -= (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
-                    carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
-                }
-                LEFT -> {
-                    carAngle += route.distances[idx]
-                }
-                RIGHT -> {
-                    carAngle -= route.distances[idx]
-                }
-            }
-        }
-    }
-
-    private fun degreesToRadian(angle: Int): Double {
-        return Math.PI * angle / 180
-    }
-
-}