kscience/kmath
8
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki Activity

Obstacle avoidance finished

Browse Source
This commit is contained in:
Alexander Nozik 2023-04-05 13:30:13 +03:00
parent a0e2ef1afc
commit 00ce7d5a48
7 changed files with 46 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
kmath-geometry/src/commonMain/kotlin/space/kscience/kmath/geometry/Polygon.kt Normal file
View File

@ -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<T> {
public val points: List<Vector2D<T>>
}

4
kmath-geometry/src/commonTest/kotlin/space/kscience/kmath/geometry/ProjectionOntoLineTest.kt
View File

@ -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))
}

10
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
View File

@ -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)
}

20
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Obstacle.kt
View File

@ -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<Tangent>) {
fun last() = tangents.last()
@ -143,7 +143,7 @@ private fun dubinsTangentsToCircles(
}
}
public class Obstacle(
internal class Obstacle(
public val circles: List<Circle2D>,
) {
internal val tangents: List<Tangent> = 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<Polygon<Double>>,
): List<CompositeTrajectory2D> {
val obstacles: List<Obstacle> = obstaclePolygons.map { polygon ->
Obstacle(polygon.points.map { point -> Circle2D(point, trajectoryRadius) })
}
return findAllPaths(start, trajectoryRadius, finish, trajectoryRadius, obstacles)
}
}

12
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory2D.kt
View File

@ -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<Vector2D<Double>> = LineSegment(start, end)
/**
* An arc segment
*/

4
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/DubinsTests.kt
View File

@ -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)))
}
}

4
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/math.kt
View File

@ -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)
)
}