teldufalsari/kmath
2
0
Fork 0
forked from kscience/kmath
Code Pull Requests 1 Activity

Code simplification for Dubins path

Browse Source
This commit is contained in:
Alexander Nozik 2022-07-29 14:12:44 +03:00
parent 323e8b6872
commit 137ddb3ade
No known key found for this signature in database
GPG Key ID: F7FCF2DD25C71357
11 changed files with 146 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPath.kt → kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/DubinsPath.kt
View File

@ -3,24 +3,79 @@
* 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.trajectory.dubins
package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.ArcSegment
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import space.kscience.kmath.trajectory.segments.Trajectory
import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
val dX = radius * cos(theta)
val dY = radius * sin(theta)
return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
}
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, ArcSegment.Direction.LEFT)
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b,
ArcSegment.Direction.RIGHT
)
private fun outerTangent(a: Circle2D, b: Circle2D, side: ArcSegment.Direction): StraightSegment {
val centers = StraightSegment(a.center, b.center)
val p1 = when (side) {
ArcSegment.Direction.LEFT -> Vector2D(
a.center.x - a.radius * cos(centers.theta),
a.center.y + a.radius * sin(centers.theta)
)
ArcSegment.Direction.RIGHT -> Vector2D(
a.center.x + a.radius * cos(centers.theta),
a.center.y - a.radius * sin(centers.theta)
)
}
return StraightSegment(
p1,
Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
)
}
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, ArcSegment.Direction.LEFT)
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, ArcSegment.Direction.RIGHT)
private fun innerTangent(base: Circle2D, direction: Circle2D, side: ArcSegment.Direction): StraightSegment? {
val centers = StraightSegment(base.center, direction.center)
if (centers.length < base.radius * 2) return null
val angle = theta(
when (side) {
ArcSegment.Direction.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
ArcSegment.Direction.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
}
)
val dX = base.radius * sin(angle)
val dY = base.radius * cos(angle)
val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
return StraightSegment(p1, p2)
}
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)
public class DubinsPath(
public val a: ArcSegment,
public val b: Trajectory,
public val c: ArcSegment,
) : Trajectory {
) : CompositeTrajectory(listOf(a,b,c)) {
public val type: TYPE = TYPE.valueOf(
arrayOf(
@ -30,8 +85,6 @@ public class DubinsPath(
).toCharArray().concatToString()
)
override val length: Double get() = a.length + b.length + c.length
public enum class TYPE {
RLR, LRL, RSR, LSL, RSL, LSR
}
@ -119,7 +172,7 @@ public class DubinsPath(
val c1 = start.getRightCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius)
val s = rightInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.RIGHT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.LEFT)
@ -130,11 +183,11 @@ public class DubinsPath(
val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius)
val s = leftInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
if (s == null || c1.center.distanceTo(c2.center) < turningRadius * 2) return null
val a1 = ArcSegment.of(c1.center, start, s.start, ArcSegment.Direction.LEFT)
val a3 = ArcSegment.of(c2.center, s.end, end, ArcSegment.Direction.RIGHT)
return DubinsPath(a1, s, a3)
}
}
}
}

33
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Pose2D.kt Normal file
View File

@ -0,0 +1,33 @@
/*
* Copyright 2018-2021 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.trajectory
import space.kscience.kmath.geometry.Vector
import space.kscience.kmath.geometry.Vector2D
import kotlin.math.atan2
/**
* Combination of [Vector] and its view angle
*/
public interface Pose2D: Vector2D{
public val coordinate: Vector2D
public val theta: Double
}
public class PhaseVector2D(
override val coordinate: Vector2D,
public val velocity: Vector2D
): Pose2D, Vector2D by coordinate{
override val theta: Double get() = atan2(velocity.y, velocity.x)
}
internal class Pose2DImpl(
override val coordinate: Vector2D,
override val theta: Double
) : Pose2D, Vector2D by coordinate
public fun Pose2D(coordinate: Vector2D, theta: Double): Pose2D = Pose2DImpl(coordinate, theta)

17
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Trajectory.kt → kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/Trajectory.kt
View File

@ -1,14 +1,18 @@
package space.kscience.kmath.trajectory.segments
/*
* Copyright 2018-2021 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.trajectory
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.trajectory.dubins.theta
import kotlin.math.PI
import kotlin.math.atan2
public interface Trajectory {
public sealed interface Trajectory {
public val length: Double
}
@ -69,7 +73,7 @@ public data class ArcSegment(
vector: Vector2D,
theta: Double,
direction: Direction,
): Pose2D = Pose2D.of(
): Pose2D = Pose2D(
vector,
when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
@ -84,6 +88,9 @@ public data class ArcSegment(
return ArcSegment(Circle2D(center, s1.length), pose1, pose2)
}
}
}
public open class CompositeTrajectory(public val segments: Collection<Trajectory>) : Trajectory {
override val length: Double get() = segments.sumOf { it.length }
}

14
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/TrajectoryCost.kt Normal file
View File

@ -0,0 +1,14 @@
/*
* Copyright 2018-2021 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.trajectory
public fun interface TrajectoryCost {
public fun estimate(trajectory: Trajectory): Double
public companion object{
public val length: TrajectoryCost = TrajectoryCost { it.length }
}
}

72
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/trajectoryFunctions.kt
View File

@ -1,72 +0,0 @@
/*
* Copyright 2018-2021 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.trajectory.dubins
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin
private enum class SIDE {
LEFT, RIGHT
}
internal fun Pose2D.getLeftCircle(radius: Double): Circle2D = getTangentCircles(radius).first
internal fun Pose2D.getRightCircle(radius: Double): Circle2D = getTangentCircles(radius).second
internal fun Pose2D.getTangentCircles(radius: Double): Pair<Circle2D, Circle2D> {
val dX = radius * cos(theta)
val dY = radius * sin(theta)
return Circle2D(Vector2D(x - dX, y + dY), radius) to Circle2D(Vector2D(x + dX, y - dY), radius)
}
internal fun leftOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.LEFT)
internal fun rightOuterTangent(a: Circle2D, b: Circle2D): StraightSegment = outerTangent(a, b, SIDE.RIGHT)
private fun outerTangent(a: Circle2D, b: Circle2D, side: SIDE): StraightSegment {
val centers = StraightSegment(a.center, b.center)
val p1 = when (side) {
SIDE.LEFT -> Vector2D(
a.center.x - a.radius * cos(centers.theta),
a.center.y + a.radius * sin(centers.theta)
)
SIDE.RIGHT -> Vector2D(
a.center.x + a.radius * cos(centers.theta),
a.center.y - a.radius * sin(centers.theta)
)
}
return StraightSegment(
p1,
Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y))
)
}
internal fun leftInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, SIDE.LEFT)
internal fun rightInnerTangent(base: Circle2D, direction: Circle2D): StraightSegment? =
innerTangent(base, direction, SIDE.RIGHT)
private fun innerTangent(base: Circle2D, direction: Circle2D, side: SIDE): StraightSegment? {
val centers = StraightSegment(base.center, direction.center)
if (centers.length < base.radius * 2) return null
val angle = theta(
when (side) {
SIDE.LEFT -> centers.theta + acos(base.radius * 2 / centers.length)
SIDE.RIGHT -> centers.theta - acos(base.radius * 2 / centers.length)
}
)
val dX = base.radius * sin(angle)
val dY = base.radius * cos(angle)
val p1 = Vector2D(base.center.x + dX, base.center.y + dY)
val p2 = Vector2D(direction.center.x - dX, direction.center.y - dY)
return StraightSegment(p1, p2)
}
internal fun theta(theta: Double): Double = (theta + (2 * PI)) % (2 * PI)

16
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/route.kt Normal file
View File

@ -0,0 +1,16 @@
/*
* Copyright 2018-2021 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.trajectory
public fun interface MaxCurvature {
public fun compute(startPoint: PhaseVector2D): Double
}
public fun DubinsPath.Companion.shortest(
start: PhaseVector2D,
end: PhaseVector2D,
computer: MaxCurvature,
): DubinsPath = shortest(start, end, computer.compute(start))

21
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Pose2D.kt
View File

@ -1,21 +0,0 @@
/*
* Copyright 2018-2021 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.trajectory.segments
import space.kscience.kmath.geometry.Vector2D
/**
* A [Vector2D] with view direction
*/
public data class Pose2D(
override val x: Double,
override val y: Double,
public val theta: Double
) : Vector2D {
public companion object {
public fun of(vector: Vector2D, theta: Double): Pose2D = Pose2D(vector.x, vector.y, theta)
}
}

2
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/Math.kt
View File

@ -1,8 +1,6 @@
package space.kscience.kmath.trajectory
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import kotlin.math.PI
import kotlin.math.abs
import kotlin.math.sin

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

@ -7,13 +7,7 @@ package space.kscience.kmath.trajectory.dubins
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.equalFloat
import space.kscience.kmath.trajectory.equalsFloat
import space.kscience.kmath.trajectory.inverse
import space.kscience.kmath.trajectory.segments.ArcSegment
import space.kscience.kmath.trajectory.segments.Pose2D
import space.kscience.kmath.trajectory.segments.StraightSegment
import space.kscience.kmath.trajectory.shift
import space.kscience.kmath.trajectory.*
import kotlin.test.Test
import kotlin.test.assertNotNull
import kotlin.test.assertTrue
@ -26,8 +20,8 @@ class DubinsTests {
val straight = StraightSegment(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = Pose2D.of(straight.end, straight.theta)
val end = Pose2D.of(lineP1.start, lineP1.theta)
val start = Pose2D(straight.end, straight.theta)
val end = Pose2D(lineP1.start, lineP1.theta)
val radius = 2.0
val dubins = DubinsPath.all(start, end, radius)
@ -58,8 +52,8 @@ class DubinsTests {
assertTrue(path.c.start.equalsFloat(b.end))
} else if (path.b is StraightSegment) {
val b = path.b as StraightSegment
assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
assertTrue(path.a.end.equalsFloat(Pose2D(b.start, b.theta)))
assertTrue(path.c.start.equalsFloat(Pose2D(b.end, b.theta)))
}
}
}

1
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/ArcTests.kt
View File

@ -3,6 +3,7 @@ package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Circle2D
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.geometry.circumference
import space.kscience.kmath.trajectory.ArcSegment
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.test.Test
import kotlin.test.assertEquals

1
kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt
View File

@ -2,6 +2,7 @@ package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.StraightSegment
import space.kscience.kmath.trajectory.radiansToDegrees
import kotlin.math.pow
import kotlin.math.sqrt