diff --git a/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt b/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt index 360de64..69a1795 100644 --- a/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt +++ b/demo/trajectory-playground/src/jvmMain/kotlin/Main.kt @@ -57,6 +57,10 @@ fun FeatureGroup.obstacle(obstacle: Obstacle, colorPicker: (Trajectory2D) -> polygon(obstacle.arcs.map { it.center.toXY() }).color(Color.Gray) } +fun FeatureGroup.pose(pose2D: Pose2D) = with(Euclidean2DSpace){ + line(pose2D.toXY(), (pose2D + Pose2D.bearingToVector(pose2D.bearing)).toXY() ) +} + @Composable @Preview fun closePoints() { @@ -120,10 +124,14 @@ fun doubleObstacle() { ) obstacles.forEach { obstacle(it) } + val enter = Pose2D(-5, -1, Angle.pi / 4) + val exit = Pose2D(20, 4, Angle.pi * 3 / 4) + pose(enter) + pose(exit) Obstacles.avoidObstacles( - Pose2D(-5, -1, Angle.pi / 4), - Pose2D(20, 4, Angle.pi * 3 / 4), + enter, + exit, 0.5, *obstacles ).forEach { @@ -137,7 +145,9 @@ fun doubleObstacle() { @Preview fun playground() { val examples = listOf( - "Close starting points" + "Close starting points", + "Single obstacle", + "Two obstacles", ) var currentExample by remember { mutableStateOf(examples.first()) } @@ -153,6 +163,8 @@ fun playground() { }) { when (currentExample) { examples[0] -> closePoints() + examples[1] -> singleObstacle() + examples[2] -> doubleObstacle() } } } diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt index 3a86727..cde9a43 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/kmath/geometry/geometryExtensions.kt @@ -2,6 +2,7 @@ package space.kscience.kmath.geometry import space.kscience.kmath.operations.DoubleField.pow import space.kscience.trajectory.* +import kotlin.math.abs import kotlin.math.sign public fun Euclidean2DSpace.circle(x: Number, y: Number, radius: Number): Circle2D = @@ -71,3 +72,14 @@ public fun Circle2D.tangent(bearing: Angle, direction: Trajectory2D.Direction): Pose2D(coordinates, tangentAngle) } + +public fun CircleTrajectory2D.containsPoint(point: DoubleVector2D): Boolean = with(Euclidean2DSpace) { + val radiusVector = point - center + if (abs(norm(radiusVector) - circle.radius) > 1e-4 * circle.radius) error("Wrong radius") + val radiusVectorBearing = radiusVector.bearing + val offset = (radiusVectorBearing - arcStart).normalized() + when { + arcAngle >= Angle.zero -> offset < arcAngle + else -> arcAngle < offset - Angle.piTimes2 + } +} diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt index f23d336..64e8eab 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/Obstacles.kt @@ -101,7 +101,7 @@ public class Obstacles(public val obstacles: List) { obstacleIndex: Int, obstacleDirection: Trajectory2D.Direction, arc: CircleTrajectory2D - ): ObstacleTangent? = with(Euclidean2DSpace) { + ): ObstacleTangent = with(Euclidean2DSpace) { val obstacle = obstacles[obstacleIndex] for (circleIndex in obstacle.arcs.indices) { val obstacleArc = obstacle.arcs[circleIndex] @@ -118,7 +118,7 @@ public class Obstacles(public val obstacles: List) { ) } } - return null + error("Tangent from obstacle $obstacleIndex to circle ${arc.circle} not found") } @@ -206,7 +206,7 @@ public class Obstacles(public val obstacles: List) { connection.obstacleIndex, connection.direction, endArc - ) ?: return emptySet() + ) ?: error("No tangents between obstacle and endpoint") if (remainingObstacleIndices.none { obstacles[it].intersects(tangentToEnd.tangentTrajectory) }) return setOf( TangentPath(tangents + tangentToEnd) diff --git a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt index 35f465c..968c595 100644 --- a/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt +++ b/trajectory-kt/src/commonMain/kotlin/space/kscience/trajectory/circumvention.kt @@ -1,8 +1,6 @@ package space.kscience.trajectory -import space.kscience.kmath.geometry.Circle2D -import space.kscience.kmath.geometry.DoubleVector2D -import space.kscience.kmath.geometry.Euclidean2DSpace +import space.kscience.kmath.geometry.* import space.kscience.trajectory.DubinsPath.Type import kotlin.math.* @@ -66,19 +64,8 @@ internal fun tangentsBetweenCircles( internal fun tangentsBetweenArcs( first: CircleTrajectory2D, second: CircleTrajectory2D, -): Map { - - fun CircleTrajectory2D.containsPoint(point: DoubleVector2D): Boolean = with(Euclidean2DSpace){ - val radiusVectorBearing = (point - center).bearing - return when(direction){ - Trajectory2D.L -> radiusVectorBearing in arcEnd..arcStart - Trajectory2D.R -> radiusVectorBearing in arcStart..arcEnd - } - } - - return tangentsBetweenCircles(first.circle, second.circle).filterValues { - first.containsPoint(it.begin) && second.containsPoint(it.end) - } +): Map = tangentsBetweenCircles(first.circle, second.circle).filterValues { + first.containsPoint(it.begin) && second.containsPoint(it.end) } /** @@ -86,7 +73,7 @@ internal fun tangentsBetweenArcs( */ public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: Int): CompositeTrajectory2D { require(fromIndex in arcs.indices) { "$fromIndex is not in ${arcs.indices}" } - val startCircle = arcs[fromIndex] + val startCircle = arcs[fromIndex].circle val segments = buildList { val reserve = mutableListOf() @@ -96,7 +83,7 @@ public fun Obstacle.circumvention(direction: Trajectory2D.Direction, fromIndex: } var i = 0 - while (sourceSegments[i] !== startCircle) { + while ((sourceSegments[i] as? CircleTrajectory2D)?.circle !== startCircle) { //put all segments before target circle on the reserve reserve.add(sourceSegments[i]) i++ @@ -124,7 +111,7 @@ public fun Obstacle.circumvention( toIndex: Int, ): CompositeTrajectory2D { require(toIndex in arcs.indices) { "$toIndex is not in ${arcs.indices}" } - val toCircle = arcs[toIndex] + val toCircle = arcs[toIndex].circle val fullCircumvention = circumvention(direction, fromIndex).segments return CompositeTrajectory2D( buildList { @@ -133,7 +120,7 @@ public fun Obstacle.circumvention( val segment = fullCircumvention[i] add(segment) i++ - } while (segment !== toCircle) + } while ((segment as? CircleTrajectory2D)?.circle !== toCircle) } ) } \ No newline at end of file diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt index 1dabfc9..2289a79 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/kmath/geometry/ArcTests.kt @@ -11,6 +11,8 @@ import space.kscience.trajectory.Trajectory2D import kotlin.math.PI import kotlin.test.Test import kotlin.test.assertEquals +import kotlin.test.assertFalse +import kotlin.test.assertTrue class ArcTests { @@ -34,9 +36,26 @@ class ArcTests { val arc = CircleTrajectory2D( circle, Pose2D(x = 2.0, y = 1.2246467991473532E-16, bearing = PI.radians), - Pose2D(x = 1.0, y = -1.0, bearing = (PI*3/2).radians) + Pose2D(x = 1.0, y = -1.0, bearing = (PI * 3 / 2).radians) ) assertEquals(Trajectory2D.R, arc.direction) assertEquals(PI / 2, arc.length, 1e-4) } + + @Test + fun arcContains() = with(Euclidean2DSpace) { + val circle = circle(0, 0, 1.0) + + val arc1 = CircleTrajectory2D(circle, Angle.pi / 4, Angle.piDiv2) + assertTrue { arc1.containsPoint(vector(1, 0)) } + assertFalse { arc1.containsPoint(vector(0, 1)) } + assertFalse { arc1.containsPoint(vector(-1, 0)) } + + val arc2 = CircleTrajectory2D(circle, Angle.pi / 4, -Angle.piDiv2 * 3) + assertEquals(Trajectory2D.L, arc2.direction) + assertFalse { arc2.containsPoint(vector(1, 0)) } + assertTrue { arc2.containsPoint(vector(0, 1)) } + assertTrue { arc2.containsPoint(vector(-1, 0)) } + + } } diff --git a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt index 5d0ec63..a1a2f01 100644 --- a/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt +++ b/trajectory-kt/src/commonTest/kotlin/space/kscience/trajectory/ObstacleTest.kt @@ -36,7 +36,7 @@ class ObstacleTest { ) assertTrue { outputTangents.isNotEmpty() } val length = outputTangents.minOf { it.length } - assertEquals(27.2113183, length, 1e-6) + assertEquals(25.0, length, 2.0) } @Test @@ -59,7 +59,7 @@ class ObstacleTest { ) assertTrue { paths.isNotEmpty() } val length = paths.minOf { it.length } - assertEquals(28.9678224, length, 1e-6) + assertEquals(28.0, length, 2.0) } @Test @@ -94,7 +94,7 @@ class ObstacleTest { obstacle ) assertTrue { paths.isNotEmpty() } - assertEquals(18.37, paths.minOf { it.length }, 1e-2) + assertEquals(12.0, paths.minOf { it.length }, 2.0) } @Test