From ad71f38061fac375d2afa1c00ee7aa31ba402391 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 29 Aug 2016 12:13:05 +0300 Subject: [PATCH] Deleted old version of algorithm from RoomBypassingAlgorithm; Refactored new algo from RoomTest into RoomBypassingAlgorithm --- server/src/main/java/DebugClInterface.kt | 7 +- .../java/algorithm/RoomBypassingAlgorithm.kt | 223 ++++-------------- server/src/main/java/algorithm/RoomModel.kt | 2 +- server/src/main/java/algorithm/RoomTest.kt | 96 -------- 4 files changed, 50 insertions(+), 278 deletions(-) delete mode 100644 server/src/main/java/algorithm/RoomTest.kt diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 90b5cdf4cd8..8b11685e694 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -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-- diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 0108641dd47..c551c1e4aaa 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -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) : AbstractAlgorithm(thisCar, exchanger) { - var wallAngleWithOX = 0.0//in radian - var wallLength = 0.0//sm - var errorCount = 0 + val points = arrayListOf() + 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): CarState { + return CarState.WALL + } - override fun getCarState(anglesDistances: Map): 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, 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) : Abs } } - override fun getCommand(anglesDistances: Map, 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() - 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): 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): 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 - } - - } \ No newline at end of file diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 0a8ffeccfa2..79295132abe 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -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() diff --git a/server/src/main/java/algorithm/RoomTest.kt b/server/src/main/java/algorithm/RoomTest.kt deleted file mode 100644 index 5d1d3a93ad2..00000000000 --- a/server/src/main/java/algorithm/RoomTest.kt +++ /dev/null @@ -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) : AbstractAlgorithm(thisCar, exchanger) { - - val points = arrayListOf() - private val DISTANCE_TO_WALL_THRESHOLD = 60 - - var carX = 0 - var carY = 0 - var carAngle = 0 - - override fun getCarState(anglesDistances: Map): CarState { - return CarState.WALL - } - - override fun getCommand(anglesDistances: Map, 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 - } - -} \ No newline at end of file