From 00ce7d5a48e4736ec6d80a480327a2c550787425 Mon Sep 17 00:00:00 2001 From: Alexander Nozik Date: Wed, 5 Apr 2023 13:30:13 +0300 Subject: [PATCH] Obstacle avoidance finished --- .../space/kscience/kmath/geometry/Polygon.kt | 14 +++++++++++++ .../kmath/geometry/ProjectionOntoLineTest.kt | 4 ++-- .../kscience/kmath/trajectory/DubinsPath.kt | 10 +++++----- .../kscience/kmath/trajectory/Obstacle.kt | 20 +++++++++++++++---- .../kscience/kmath/trajectory/Trajectory2D.kt | 12 +++++------ .../kscience/kmath/trajectory/DubinsTests.kt | 4 ++-- .../space/kscience/kmath/trajectory/math.kt | 4 ++-- 7 files changed, 46 insertions(+), 22 deletions(-) create mode 100644 kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt diff --git a/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt new file mode 100644 index 000000000..20f4a031e --- /dev/null +++ b/kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt @@ -0,0 +1,14 @@ +/* + * Copyright 2018-2023 KMath contributors. + * Use of this source code is governed by the Apache 2.0 license that can be found in the license/LICENSE.txt file. + */ + +package space.kscience.kmath.geometry + + +/** + * A closed polygon in 2D space + */ +public interface Polygon { + public val points: List> +} \ No newline at end of file diff --git a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt index cdb8ea870..7c6c105cf 100644 --- a/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt +++ b/kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt @@ -63,7 +63,7 @@ internal class ProjectionOntoLineTest { @Test fun projectionOntoLine3d() = with(Euclidean3DSpace) { - val line = Line3D( + val line = Line( base = vector(1.0, 3.5, 0.07), direction = vector(2.0, -0.0037, 11.1111) ) @@ -77,7 +77,7 @@ internal class ProjectionOntoLineTest { val result = projectToLine(v, line) // assert that result is on the line - assertTrue(isCollinear(result - line.base, line.direction)) + assertTrue(isCollinear(result - line.start, line.direction)) // assert that PV vector is orthogonal to direction vector assertTrue(isOrthogonal(v - result, line.direction)) } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt index 87ea52a69..a1563b29c 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt @@ -36,7 +36,7 @@ private fun outerTangent(from: Circle2D, to: Circle2D, direction: Direction): St } return StraightTrajectory2D( p1, - vector(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) + vector(p1.x + (centers.end.x - centers.begin.x), p1.y + (centers.end.y - centers.begin.y)) ) } @@ -207,7 +207,7 @@ public object DubinsPath { val c1 = start.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius) val s = outerTangent(c1, c2, L) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } @@ -216,7 +216,7 @@ public object DubinsPath { val c1 = start.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius) val s = outerTangent(c1, c2, R) - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -227,7 +227,7 @@ public object DubinsPath { val s = innerTangent(c1, c2, R) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, R) + val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, R) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, L) return CompositeTrajectory2D(a1, s, a3) } @@ -238,7 +238,7 @@ public object DubinsPath { val s = innerTangent(c1, c2, L) if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null - val a1 = CircleTrajectory2D.of(c1.center, start, s.start, L) + val a1 = CircleTrajectory2D.of(c1.center, start, s.begin, L) val a3 = CircleTrajectory2D.of(c2.center, s.end, end, R) return CompositeTrajectory2D(a1, s, a3) } diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt index 3819afb54..32061efea 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt @@ -23,7 +23,7 @@ internal data class Tangent( val lineSegment: LineSegment2D, val startDirection: Trajectory2D.Direction, val endDirection: Trajectory2D.Direction = startDirection, -): LineSegment2D by lineSegment +) : LineSegment2D by lineSegment private class TangentPath(val tangents: List) { fun last() = tangents.last() @@ -143,7 +143,7 @@ private fun dubinsTangentsToCircles( } } -public class Obstacle( +internal class Obstacle( public val circles: List, ) { internal val tangents: List = boundaryTangents().first @@ -281,7 +281,7 @@ public class Obstacle( } } -public fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) +internal fun Obstacle(vararg circles: Circle2D): Obstacle = Obstacle(listOf(*circles)) private fun LineSegment2D.intersectSegment(other: LineSegment2D): Boolean { fun crossProduct(v1: DoubleVector2D, v2: DoubleVector2D): Double { @@ -594,7 +594,19 @@ internal fun findAllPaths( } - +public object Obstacles { + public fun allPathsAvoiding( + start: DubinsPose2D, + finish: DubinsPose2D, + trajectoryRadius: Double, + obstaclePolygons: List>, + ): List { + val obstacles: List = obstaclePolygons.map { polygon -> + Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) }) + } + return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles) + } +} diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt index 8df0de237..59a8e613a 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt @@ -41,17 +41,15 @@ public sealed interface Trajectory2D { @Serializable @SerialName("straight") public data class StraightTrajectory2D( - public val start: DoubleVector2D, - public val end: DoubleVector2D, -) : Trajectory2D { + override val begin: DoubleVector2D, + override val end: DoubleVector2D, +) : Trajectory2D, LineSegment2D { - override val length: Double get() = start.distanceTo(end) + override val length: Double get() = begin.distanceTo(end) - public val bearing: Angle get() = (atan2(end.x - start.x, end.y - start.y).radians).normalized() + public val bearing: Angle get() = (atan2(end.x - begin.x, end.y - begin.y).radians).normalized() } -public fun StraightTrajectory2D.toSegment(): LineSegment> = LineSegment(start, end) - /** * An arc segment */ diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt index f5bfb884e..80f7173f1 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt @@ -20,7 +20,7 @@ class DubinsTests { val lineP1 = straight.shift(1, 10.0).inverse() val start = DubinsPose2D(straight.end, straight.bearing) - val end = DubinsPose2D(lineP1.start, lineP1.bearing) + val end = DubinsPose2D(lineP1.begin, lineP1.bearing) val radius = 2.0 val dubins = DubinsPath.all(start, end, radius) @@ -53,7 +53,7 @@ class DubinsTests { assertTrue(a.end.equalsFloat(b.start)) assertTrue(c.start.equalsFloat(b.end)) } else if (b is StraightTrajectory2D) { - assertTrue(a.end.equalsFloat(DubinsPose2D(b.start, b.bearing))) + assertTrue(a.end.equalsFloat(DubinsPose2D(b.begin, b.bearing))) assertTrue(c.start.equalsFloat(DubinsPose2D(b.end, b.bearing))) } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt index 24685f528..8b8ccf95e 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt @@ -14,14 +14,14 @@ import space.kscience.kmath.geometry.sin fun DubinsPose2D.equalsFloat(other: DubinsPose2D) = x.equalsFloat(other.x) && y.equalsFloat(other.y) && bearing.radians.equalsFloat(other.bearing.radians) -fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, start) +fun StraightTrajectory2D.inverse() = StraightTrajectory2D(end, begin) fun StraightTrajectory2D.shift(shift: Int, width: Double): StraightTrajectory2D = with(Euclidean2DSpace) { val dX = width * sin(inverse().bearing) val dY = width * sin(bearing) return StraightTrajectory2D( - vector(start.x - dX * shift, start.y - dY * shift), + vector(begin.x - dX * shift, begin.y - dY * shift), vector(end.x - dX * shift, end.y - dY * shift) ) }