From 4c43c3c411a9f7c9b90120fd95b990ba2032a685 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Wed, 24 Aug 2016 18:01:07 +0300 Subject: [PATCH] upgrade algorithm --- .../main/java/algorithm/AbstractAlgorithm.kt | 14 +- .../java/algorithm/RoomBypassingAlgorithm.kt | 123 +++++++++++++++--- server/src/main/java/algorithm/Vector.kt | 16 +++ 3 files changed, 135 insertions(+), 18 deletions(-) create mode 100644 server/src/main/java/algorithm/Vector.kt diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 355eac93f05..dbfb5bf5f5a 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -51,7 +51,16 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() for (i in 0..angles.size - 1) { + if (Math.abs(distances[i]) < 0.01) { + continue + } anglesDistances.put(angles[i], distances[i]) } diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 2c37973b0e2..6f51b092456 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -2,6 +2,7 @@ package algorithm import RouteRequest import objects.Car +import java.util.* import java.util.concurrent.Exchanger class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { @@ -10,9 +11,22 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs private val ROTATION_VELOCITY = 11.0//degrees/s + private var wallLength = 0.0//in sm + private var wallAngle = 0.0//in radian + private var xPos = 0.0 + private var yPos = 0.0 + + override fun getCarState(anglesDistances: Map): CarState { val dist0 = anglesDistances[0]!! val dist90 = anglesDistances[90]!! + val dist60 = anglesDistances[60]!! + val oldDist90 = getPrevSonarDistances()[90] + println(dist90) + println(oldDist90) + if (oldDist90 != null && oldDist90 + 60 < dist90) { + return CarState.OUTER + } if (dist90 < 20) { return CarState.WALL } @@ -36,33 +50,108 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val dist90 = anglesDistances[90]!! val dist120 = anglesDistances[120]!! val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) - if (state == CarState.WALL) { - if (dist90 > 40 || dist90 < 20) { - val rotationDirection = if (dist90 > 40) RIGHT else LEFT - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setTimes(getIntArray(2000, 1000)) - return resultBuilder.build() - } + when (state) { + CarState.WALL -> { + if (dist90 > 40 || dist90 < 20) { + val rotationDirection = if (dist90 > 40) RIGHT else LEFT + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + wallLength += 1 * MOVE_VELOCITY + resultBuilder.setTimes(getIntArray(2000, 1000)) + return resultBuilder.build() + } - if (Math.abs(dist120 - dist60) > 10) { - val rotationDirection = if (dist120 > dist60) LEFT else RIGHT - resultBuilder.setDirections(getIntArray(rotationDirection)) + if (Math.abs(dist120 - dist60) > 10) { + val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection)) + resultBuilder.setTimes(getIntArray(1000)) + return resultBuilder.build() + } + + resultBuilder.setDirections(getIntArray(FORWARD)) + wallLength += 1 * MOVE_VELOCITY resultBuilder.setTimes(getIntArray(1000)) return resultBuilder.build() } + CarState.INNER -> { - resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setTimes(getIntArray(1000)) - return resultBuilder.build() + if (getPrevState() != state) { + val pointsNextLine = arrayListOf>() + val pointsCurrentLine = arrayListOf>() + for (angleDist in anglesDistances) { + if (angleDist.key <= 40) { + pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair())) + } else if (angleDist.key <= 120) { + pointsCurrentLine.add(sonarAngleDistToPoint(angleDist.toPair())) + } + } + val innerAnglePoint = getInnerAnglePoint(pointsNextLine, pointsCurrentLine) + val vectorNextLine = Vector(innerAnglePoint.first, innerAnglePoint.second, + pointsNextLine[0].first, pointsNextLine[0].second) - } else { - resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT)) - resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) - return resultBuilder.build() + val vectorCurrentLine = Vector(innerAnglePoint.first, innerAnglePoint.second, + pointsCurrentLine.last().first, pointsCurrentLine.last().second) + + val scalarMult = vectorCurrentLine.scalarProduct(vectorNextLine) + val angle = Math.acos(scalarMult / (vectorCurrentLine.length() * vectorNextLine.length())) + wallAngle += angle + //todo add length and save this point + } + + resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT)) + resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() + } + CarState.OUTER -> { + resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT)) + resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() + } } } + private fun getInnerAnglePoint(pointsNextLine: ArrayList>, + pointsCurrentLine: ArrayList>): Pair { + + val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray()) + val currentLine = approximatePointsByLine(pointsCurrentLine.toTypedArray()) + val b1 = currentLine.second + val k1 = currentLine.first + val b2 = nextLine.second + val k2 = nextLine.first + val intersectionPoint = Pair((b1 - b2) / (k2 - k1), (k2 * b1 - b2 * k1) / (k2 - k1)) + return intersectionPoint + } + + private fun degreesToRadian(angle: Int): Double { + return Math.PI * angle / 180 + } + + private fun sonarAngleDistToPoint(angleDist: Pair): Pair { + + val angle = wallAngle - degreesToRadian(angleDist.first) + val dist = angleDist.second + + val result = Pair(xPos + Math.cos(angle) * dist, yPos + Math.sin(angle) * dist) + return result + } + + //returns coef in y=kx+b (first is k, second is b) + private fun approximatePointsByLine(points: Array>): Pair { + + val sumX = points.sumByDouble { it.first } + val sumXQuad = points.sumByDouble { it.first * it.first } + val sumY = points.sumByDouble { it.second } + val sumXY = points.sumByDouble { it.second * it.first } + + val pointsCount = points.size + val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) + val b = (sumY - sumX * k) / pointsCount + + return Pair(k, b) + } + + private fun calculateAngleWithWall(anglesDistances: MutableMap): Double { val dist60 = anglesDistances[60]!! val dist120 = anglesDistances[120]!! diff --git a/server/src/main/java/algorithm/Vector.kt b/server/src/main/java/algorithm/Vector.kt new file mode 100644 index 00000000000..c1c8e141b75 --- /dev/null +++ b/server/src/main/java/algorithm/Vector.kt @@ -0,0 +1,16 @@ +package algorithm + +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 + } + + fun length(): Double { + return Math.sqrt(x * x + y * y) + } + +}