From 78d51b6ac006c40746abfeb61d8aa19915b48c10 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Thu, 25 Aug 2016 19:28:05 +0300 Subject: [PATCH] working algorithm on very simple room --- car_srv/kotlinSrv/src/Test.kt | 12 + .../control/emulator/ControllerEmulator.kt | 28 ++- car_srv/kotlinSrv/src/room/Room.kt | 61 +++-- server/src/main/java/DebugClInterface.kt | 31 ++- .../main/java/algorithm/AbstractAlgorithm.kt | 12 +- .../java/algorithm/RoomBypassingAlgorithm.kt | 210 +++++++++++------- server/src/main/java/algorithm/RoomModel.kt | 8 +- .../src/main/java/algorithm/geometry/Line.kt | 21 ++ .../java/algorithm/{ => geometry}/Vector.kt | 2 +- .../src/main/java/car/client/ClientHandler.kt | 5 +- 10 files changed, 252 insertions(+), 138 deletions(-) create mode 100644 server/src/main/java/algorithm/geometry/Line.kt rename server/src/main/java/algorithm/{ => geometry}/Vector.kt (92%) diff --git a/car_srv/kotlinSrv/src/Test.kt b/car_srv/kotlinSrv/src/Test.kt index 2ba16f1275b..6ffa3ce9d8b 100644 --- a/car_srv/kotlinSrv/src/Test.kt +++ b/car_srv/kotlinSrv/src/Test.kt @@ -55,6 +55,18 @@ private fun testCarEmulator() { val dist90 = sonarResponse.distances[0] eq(dist90.toDouble(), 98.9949, "check angle 90", 2.5) }) + + println("--------------------") + CarState.instance.x = 150 + CarState.instance.y = 50 + CarState.instance.angle = 0 + sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(7, { it*5 })).build() + sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() + controller.executeRequestSensorData(sonarRequest, { bytes -> + sonarResponse.mergeFrom(CodedInputStream(bytes)) + val dist30 = sonarResponse.distances[7] + eq(dist30.toDouble(), 98.9949, "check angle 30", 2.5) + }) } private var testNum = 1 diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index 679d83dfb07..9e9b7df9b88 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -11,10 +11,14 @@ import encodeProtoBuf import geometry.Line import geometry.Vector import room.Room +import kotlin.Pair class ControllerEmulator : Controller { - private val MOVE_VELOCITY = 32.78//sm/s - private val ROTATION_VELOCITY = 12.3//degrees/s + + private val MOVE_VELOCITY = 0.05//sm/ms + private val ROTATION_VELOCITY = 0.05//degrees/ms + + private val ADD_RANDOM = false enum class MoveDirection { LEFT, @@ -87,7 +91,7 @@ class ControllerEmulator : Controller { if (distance == -1) { distances.add(distance) } else { - val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2 + val delta = if (ADD_RANDOM) getRandomIntFrom(IntArray(5, { x -> x - 2 })) else 0//return one of -2 -1 0 1 or 2 distances.add(distance + delta) } } @@ -119,14 +123,14 @@ class ControllerEmulator : Controller { continue } val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0) - + Math.pow(yIntersection - ySensor0, 2.0))) + + Math.pow(yIntersection - ySensor0, 2.0))*100000) if (currentDistance < result) { result = currentDistance } } - if (result < 5 || result > 500) { - return -1 - } +// if (result < 5 || result > 500) { +// return -1 +// } return result } @@ -158,13 +162,13 @@ class ControllerEmulator : Controller { //refresh car state val carInstance = CarState.instance val commandTime = currentCommand.second - val delta = Math.random() * 0.2 + 0.9// delta in [0.9, 1.1) + val delta = if (ADD_RANDOM) Math.random() * 0.2 + 0.9 else 1.0// delta in [0.9, 1.1) val commandTimeIncludeRandom = (commandTime * delta).toInt() when (currentCommand.first) { - MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000) - MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000) - MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000) - MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000) + MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt()) + MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt()) + MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt()) + MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt()) else -> { } } diff --git a/car_srv/kotlinSrv/src/room/Room.kt b/car_srv/kotlinSrv/src/room/Room.kt index 69b543f03a1..8c61465fe87 100644 --- a/car_srv/kotlinSrv/src/room/Room.kt +++ b/car_srv/kotlinSrv/src/room/Room.kt @@ -32,45 +32,42 @@ object Room { |___| */ - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 20.0) - val rightLine = Line(1.0, 0.0, -200.0) - - val bottomLine2 = Line(0.0, 1.0, -100.0) - val rightLine2 = Line(1.0, 0.0, -300.0) - - val walls = listOf(Wall(upLine, 300, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -20), - Wall(bottomLine, -150, 200, -20, -20), - Wall(rightLine, 200, 200, -20, 100), - Wall(bottomLine2, 200, 300, 100, 100), - Wall(rightLine2, 300, 300, 100, 300) - ) +// val upLine = Line(0.0, 1.0, -300.0) +// val leftLine = Line(1.0, 0.0, 150.0) +// val bottomLine = Line(0.0, 1.0, 20.0) +// val rightLine = Line(1.0, 0.0, -200.0) +// +// val bottomLine2 = Line(0.0, 1.0, -100.0) +// val rightLine2 = Line(1.0, 0.0, -300.0) +// +// val walls = listOf(Wall(upLine, 300, -150, 300, 300), +// Wall(leftLine, -150, -150, 300, -20), +// Wall(bottomLine, -150, 200, -20, -20), +// Wall(rightLine, 200, 200, -20, 100), +// Wall(bottomLine2, 200, 300, 100, 100), +// Wall(rightLine2, 300, 300, 100, 300) +// ) /* - ________ - | __| + _______ + | | |____/ */ -// val upLine = Line(0.0, 1.0, -300.0) -// val leftLine = Line(1.0, 0.0, 150.0) -// val bottomLine = Line(0.0, 1.0, 20.0) -// val rightLine = Line(-1.2, 1.0, 140.0) -// -// val bottomLine2 = Line(0.0, 1.0, -100.0) -// val rightLine2 = Line(1.0, 1.0, -350.0) -// -// val walls = listOf(Wall(upLine, 350, -150, 300, 300), -// Wall(leftLine, -150, -150, 300, -20), -// Wall(bottomLine, -150, 100, -20, -20), -// Wall(rightLine, 100, 200, -20, 100), -// Wall(bottomLine2, 200, 350, 100, 100), -// Wall(rightLine2, 350, 350, 100, 300) -// ) + val upLine = Line(0.0, 1.0, -300.0) + val leftLine = Line(1.0, 0.0, 150.0) + val bottomLine = Line(0.0, 1.0, 20.0) + val rightLine = Line(-1.2, 1.0, 140.0) + val rightLine2 = Line(1.0, 1.0, -350.0) + + val walls = listOf(Wall(upLine, 200, -150, 300, 300), + Wall(leftLine, -150, -150, 300, -20), + Wall(bottomLine, -150, 100, -20, -20), + Wall(rightLine, 100, 200, -20, 100), + Wall(rightLine2, 200, 200, 100, 300) + ) } \ No newline at end of file diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 75880b6e3d9..e4aac6d71d2 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -1,6 +1,7 @@ import Exceptions.InactiveCarException import algorithm.AbstractAlgorithm import algorithm.RoomBypassingAlgorithm +import algorithm.RoomModel import car.client.Client import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* @@ -10,7 +11,7 @@ import java.util.concurrent.Exchanger object DebugClInterface { val exchanger = Exchanger() - var algorithm: AbstractAlgorithm? = null + var algorithmImpl: AbstractAlgorithm? = null private val routeRegex = Regex("route [0-9]{1,10}") private val sonarRegex = Regex("sonar [0-9]{1,10}") @@ -56,16 +57,36 @@ object DebugClInterface { "refloc" -> executeRefreshLocationCommand() "sonar" -> executeSonarCommand(readString) "dbinfo" -> executeDebugInfoCommand(readString) - "alg" -> { - if (algorithm == null) { - algorithm = RoomBypassingAlgorithm(environment.map.values.last(), exchanger) + "points" -> println(algorithm.RoomModel.lines) + "pos" -> { + val tmp = algorithmImpl + if (tmp is RoomBypassingAlgorithm) { + println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}") } - algorithm!!.iterate() } + "alg" -> executeAlg(readString) else -> printNotSupportedCommand(readString) } } + private fun executeAlg(readString: String) { + val params = readString.split(" ") + var count = + if (params.size == 2) try { + params[1].toInt() + } catch (e: Exception) { + 1 + } else 1 + + if (algorithmImpl == null) { + algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger) + } + while (count > 0) { + count-- + algorithmImpl!!.iterate() + } + } + private fun executeDebugInfoCommand(readString: String) { val params = readString.split(" ") if (params.size != 3) { diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index dbfb5bf5f5a..6cacab6012d 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -21,7 +21,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() @@ -33,7 +33,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): CarState protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteRequest protected abstract fun getAngles(): IntArray diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 6f51b092456..7bb72a864e5 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -1,36 +1,37 @@ package algorithm import RouteRequest +import algorithm.geometry.Line +import algorithm.geometry.Vector import objects.Car import java.util.* import java.util.concurrent.Exchanger class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { - private val MOVE_VELOCITY = 30.3//sm/s - private val ROTATION_VELOCITY = 11.0//degrees/s + private val MOVE_VELOCITY = 0.05//sm/ms + private val ROTATION_VELOCITY = 0.05//degrees/ms + public var wallAngleWithOX = 0.0//in radian + public var wallLength = 0.0//sm +// var xPos = 0.0 +// var yPos = 0.0 +// var carAngle = 0.0 - private var wallLength = 0.0//in sm - private var wallAngle = 0.0//in radian - private var xPos = 0.0 - private var yPos = 0.0 + private var prevPoint = Pair(0.0, 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) { + val dist0 = anglesDistances[0] + val dist90 = anglesDistances[90] + if (dist90 == null || dist90 > 85) { + //best analysis of outer angle is check minimum in values 60,70,80,90,100,... return CarState.OUTER } if (dist90 < 20) { return CarState.WALL } - if (dist0 > 60) { + if (dist0 == null || dist0 > 70) { return CarState.WALL } return CarState.INNER @@ -41,125 +42,180 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } override fun getAngles(): IntArray { - return IntArray(19, { it * 10 }) + return IntArray(37, { it * 5 }) +// return IntArray(181, { it * 1 }) + } + + override fun calculateCarPosition(route: RouteRequest, state: CarState) { + //todo for best values make calculate carAngle from right wall + +// for (i in 0..route.directions.size - 1) { +// when (route.directions[i]) { +// FORWARD -> { +// xPos += route.times[i] * MOVE_VELOCITY * Math.cos(carAngle) +// yPos += route.times[i] * MOVE_VELOCITY * Math.sin(carAngle) +// } +// LEFT -> { +// carAngle += degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt()) +// } +// RIGHT -> { +// carAngle -= degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt()) +// } +// } +// } +// if (state == CarState.INNER) { +// carAngle = wallAngleWithOX +// } } override fun getCommand(anglesDistances: Map, state: CarState): RouteRequest { - val dist0 = anglesDistances[0]!! - val dist60 = anglesDistances[60]!! - val dist90 = anglesDistances[90]!! - val dist120 = anglesDistances[120]!! + val dist0 = anglesDistances[0] + val dist60 = anglesDistances[60] + val dist90 = anglesDistances[90] + val dist120 = anglesDistances[120] + val dist180 = anglesDistances[180] val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) + if (dist120 == null || dist90 == null || dist60 == null) { + println("null distance!") + return resultBuilder.build() + } + if (getPrevState() == null) { + //its first run, save right wall + RoomModel.lines.add(Line(0.0, 1.0, dist90)) + prevPoint = Pair(0.0, -dist90) + if (dist0 != null) { + wallLength = dist0 + } + } when (state) { CarState.WALL -> { + if (wallLength.toInt() == 0 && (dist0 != null && dist180 != null)) { + wallLength = dist0 + dist180 + } 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)) + resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(), + (25.0 / MOVE_VELOCITY).toInt())) return resultBuilder.build() } if (Math.abs(dist120 - dist60) > 10) { val rotationDirection = if (dist120 > dist60) LEFT else RIGHT resultBuilder.setDirections(getIntArray(rotationDirection)) - resultBuilder.setTimes(getIntArray(1000)) + resultBuilder.setTimes(getIntArray((15.0 / ROTATION_VELOCITY).toInt())) return resultBuilder.build() } resultBuilder.setDirections(getIntArray(FORWARD)) - wallLength += 1 * MOVE_VELOCITY - resultBuilder.setTimes(getIntArray(1000)) + resultBuilder.setTimes(getIntArray((25.0 / MOVE_VELOCITY).toInt())) return resultBuilder.build() } CarState.INNER -> { - 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) - - - 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 + //в угле первое действие - встать максимально паралельно стене. + if (Math.abs(dist120 - dist60) > 10) { + val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection)) + resultBuilder.setTimes(getIntArray((3.0 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() } + //если стоим паралельно - можно мерить угол + val pointsNextLine = arrayListOf>() + for (angleDist in anglesDistances) { + if (angleDist.key <= 15) { + pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair())) + } + } + 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())) + 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.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) + resultBuilder.setTimes(getIntArray((radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt(), + (40 / MOVE_VELOCITY).toInt(), (radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt())) return resultBuilder.build() } CarState.OUTER -> { + //todo calculate angle resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT)) - resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt())) + resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(), + (40 / MOVE_VELOCITY).toInt(), (45 / 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 radiansToDegrees(angle: Double): Double { + return angle * 180 / Math.PI + } + private fun sonarAngleDistToPoint(angleDist: Pair): Pair { - val angle = wallAngle - degreesToRadian(angleDist.first) + val angle = wallAngleWithOX - degreesToRadian(angleDist.first) val dist = angleDist.second - val result = Pair(xPos + Math.cos(angle) * dist, yPos + Math.sin(angle) * dist) + val result = Pair(Math.cos(angle) * dist, Math.sin(angle) * dist) return result } //returns coef in y=kx+b (first is k, second is b) - private fun approximatePointsByLine(points: Array>): Pair { + private fun approximatePointsByLine(points: Array>): Line { - 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 p1 = points.first() + val p2 = points[1] - val pointsCount = points.size - val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) - val b = (sumY - sumX * k) / pointsCount + return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first) - return Pair(k, b) +// +// 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 Line(-k, 1.0, -b) } private fun calculateAngleWithWall(anglesDistances: MutableMap): Double { - val dist60 = anglesDistances[60]!! - val dist120 = anglesDistances[120]!! + 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) +// 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 } diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 7364e18e8a3..dab531f851b 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -1,11 +1,11 @@ package algorithm import Waypoints -/** - * Created by user on 8/23/16. - */ +import algorithm.geometry.Line + object RoomModel { - val point = arrayListOf>() + val point = arrayListOf>() + val lines = arrayListOf() // DEBUG BELOW diff --git a/server/src/main/java/algorithm/geometry/Line.kt b/server/src/main/java/algorithm/geometry/Line.kt new file mode 100644 index 00000000000..8e835642885 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/Line.kt @@ -0,0 +1,21 @@ +package algorithm.geometry + +class Line(val A: Double, val B: Double, var C: Double) { + + + fun getIntersectionPoint(lineTwo: Line): Pair { + val slope = this.A * lineTwo.B - this.B * lineTwo.A + if (Math.abs(slope) < 0.001) { + throw ArithmeticException("lines is parallel") + } + val xIntersection = (this.B * lineTwo.C - lineTwo.B * this.C) / slope + val yIntersection = (this.C * lineTwo.A - lineTwo.C * this.A) / slope + return Pair(xIntersection, yIntersection) + } + + override fun toString(): String{ + return "Line(A=$A, B=$B, C=$C)" + } + + +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/Vector.kt b/server/src/main/java/algorithm/geometry/Vector.kt similarity index 92% rename from server/src/main/java/algorithm/Vector.kt rename to server/src/main/java/algorithm/geometry/Vector.kt index c1c8e141b75..4759fa1ce6a 100644 --- a/server/src/main/java/algorithm/Vector.kt +++ b/server/src/main/java/algorithm/geometry/Vector.kt @@ -1,4 +1,4 @@ -package algorithm +package algorithm.geometry class Vector constructor(val x: Double, val y: Double) { diff --git a/server/src/main/java/car/client/ClientHandler.kt b/server/src/main/java/car/client/ClientHandler.kt index 576482e26eb..05d21ff6fca 100644 --- a/server/src/main/java/car/client/ClientHandler.kt +++ b/server/src/main/java/car/client/ClientHandler.kt @@ -14,6 +14,7 @@ import io.netty.util.AttributeKey import objects.Environment import setRouteUrl import sonarUrl +import java.util.* import java.util.concurrent.TimeUnit import java.util.concurrent.TimeoutException @@ -84,8 +85,8 @@ class ClientHandler : SimpleChannelInboundHandler { } private fun handlerSonarResponse(message: SonarResponse, angles: IntArray) { -// println("request angles: ${Arrays.toString(angles)}") -// println("distances from sonar: ${Arrays.toString(message.distances)}") + println("request angles: ${Arrays.toString(angles)}") + println("distances from sonar: ${Arrays.toString(message.distances)}") try { DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS) } catch (e: InterruptedException) {