From 97ed19358729596bde3d9004d6bf9238a1f6303c Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 22 Aug 2016 11:18:06 +0300 Subject: [PATCH] room datas --- car_srv/kotlinSrv/src/control/Controller.kt | 2 +- .../src/control/car/ControllerToUsb.kt | 2 +- .../control/emulator/ControllerEmulator.kt | 56 +++++++++++++++++-- car_srv/kotlinSrv/src/room/Data.kt | 16 ++++++ car_srv/kotlinSrv/src/room/Line.kt | 4 ++ car_srv/kotlinSrv/src/room/Wall.kt | 5 ++ 6 files changed, 79 insertions(+), 6 deletions(-) create mode 100644 car_srv/kotlinSrv/src/room/Data.kt create mode 100644 car_srv/kotlinSrv/src/room/Line.kt create mode 100644 car_srv/kotlinSrv/src/room/Wall.kt diff --git a/car_srv/kotlinSrv/src/control/Controller.kt b/car_srv/kotlinSrv/src/control/Controller.kt index 37fb05912d0..d0e3c22cf37 100644 --- a/car_srv/kotlinSrv/src/control/Controller.kt +++ b/car_srv/kotlinSrv/src/control/Controller.kt @@ -6,6 +6,6 @@ interface Controller { fun executeRoute(route: RouteRequest, callBack: (ByteArray) -> Unit) - fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) + fun executeRequestSensorData(angles: IntArray, callBack: (ByteArray) -> Unit) } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt b/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt index ccc8aefa5c4..db08536896b 100644 --- a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt +++ b/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt @@ -16,7 +16,7 @@ class ControllerToUsb : Controller { mcTransport.sendProtoBuf(route) } - override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) { + override fun executeRequestSensorData(angles: IntArray, callBack: (ByteArray) -> Unit) { //todo make after connect sensor to car } } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index ee3292d7797..1ade6950565 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -5,6 +5,8 @@ import RouteRequest import RouteResponse import control.Controller import encodeProtoBuf +import room.Data +import room.Line import setTimeout import kotlin.Pair @@ -42,11 +44,58 @@ class ControllerEmulator : Controller { executeCommand(commands, 0, callBack) } - override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) { - -// ByteArray + override fun executeRequestSensorData(angles: IntArray, callBack: (ByteArray) -> Unit) { //calculate distance + + val xSensor0 = CarState.instance.x + val ySensor0 = CarState.instance.y + val carAngle = CarState.instance.angle + + val distances = arrayListOf() + angles.forEach { angle -> + val angleFinal = ((carAngle - angle) % 360) + //tg can be equal to inf if angle = 90. it vertical line and x1 = x0, y1 = y0+[any number. eg 1] + + val xSensor1: Int + val ySensor1: Double + if (angleFinal == 90) { + xSensor1 = xSensor0 + ySensor1 = (ySensor0 + 1).toDouble() + } else { + xSensor1 = xSensor0 + 1 + ySensor1 = xSensor1 * Math.tan(angleFinal.toDouble()) + } + val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0, xSensor0 * ySensor1 - ySensor0 * xSensor1) + + for (wall in Data.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 + } + //sensor line is A1 + //x=(B1*C2 - B2*C1)/coef y = (C1A2-C2A1)/coef + val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / coef + val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / coef + + //filter by direction + if (angle > 90 && angle < 270) { + //negative direction for OX + if (xIntersection >= xSensor0) { + continue + } + } else { + if (xIntersection < xSensor0) { + continue + } + } + + } + + } + } fun executeCommand(commands: List>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { @@ -74,5 +123,4 @@ class ControllerEmulator : Controller { } - } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/room/Data.kt b/car_srv/kotlinSrv/src/room/Data.kt new file mode 100644 index 00000000000..927e4b4bb80 --- /dev/null +++ b/car_srv/kotlinSrv/src/room/Data.kt @@ -0,0 +1,16 @@ +package room + +object Data { + + 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 walls = listOf(Wall(upLine, 200, -150, 300, 300), + Wall(leftLine, -150, -150, 300, -20), + Wall(bottomLine, -150, 200, -20, -20), + Wall(rightLine, 200, 200, -20, 300) + ) + +} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/room/Line.kt b/car_srv/kotlinSrv/src/room/Line.kt new file mode 100644 index 00000000000..5bd98a8af73 --- /dev/null +++ b/car_srv/kotlinSrv/src/room/Line.kt @@ -0,0 +1,4 @@ +package room + +class Line(val A: Double, val B: Double, val C: Double) { +} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/room/Wall.kt b/car_srv/kotlinSrv/src/room/Wall.kt new file mode 100644 index 00000000000..b0367d5229d --- /dev/null +++ b/car_srv/kotlinSrv/src/room/Wall.kt @@ -0,0 +1,5 @@ +package room + +class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, yFrom: Int, yTo: Int) { + +} \ No newline at end of file