diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt index 91287b952..04c639576 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt @@ -6,21 +6,18 @@ package space.kscience.kmath.trajectory.dubins import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D -import space.kscience.kmath.trajectory.segments.Arc -import space.kscience.kmath.trajectory.segments.LineSegment +import space.kscience.kmath.trajectory.segments.* import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Pose2D -import space.kscience.kmath.trajectory.segments.length -import space.kscience.kmath.trajectory.segments.theta +import kotlin.math.PI import kotlin.math.acos import kotlin.math.cos import kotlin.math.sin public class DubinsPathFactory( - private val base: Pose2D, - private val direction: Pose2D, + private val start: Pose2D, + private val end: Pose2D, private val turningRadius: Double ) { @@ -30,9 +27,9 @@ public class DubinsPathFactory( public val rlr: DubinsPath? get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val centers = Straight(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta - acos(centers.length / (turningRadius * 4))) @@ -45,17 +42,17 @@ public class DubinsPathFactory( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, base, p1, Arc.Direction.RIGHT) + val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) - val a3 = Arc(c2.center, p2, direction, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) return DubinsPath(a1, a2, a3) } private val lrl: DubinsPath? get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val centers = Line2D(c1.center, c2.center) + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val centers = Straight(c1.center, c2.center) if (centers.length > turningRadius * 4) return null var theta = theta(centers.theta + acos(centers.length / (turningRadius * 4))) @@ -68,54 +65,54 @@ public class DubinsPathFactory( dX = turningRadius * sin(theta) dY = turningRadius * cos(theta) val p2 = Vector2D(e.center.x + dX, e.center.y + dY) - val a1 = Arc(c1.center, base, p1, Arc.Direction.LEFT) + val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, p2, direction, Arc.Direction.LEFT) + val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) return DubinsPath(a1, a2, a3) } public val rsr: DubinsPath get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftOuterTangent(c1, c2) - val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT) - return DubinsPath(a1, LineSegment(l), a3) + val c1 = start.getRightCircle(turningRadius) + val c2 = end.getRightCircle(turningRadius) + val s = leftOuterTangent(c1, c2) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + return DubinsPath(a1, s, a3) } public val lsl: DubinsPath get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightOuterTangent(c1, c2) - val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT) - val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT) - return DubinsPath(a1, LineSegment(l), a3) + val c1 = start.getLeftCircle(turningRadius) + val c2 = end.getLeftCircle(turningRadius) + val s = rightOuterTangent(c1, c2) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + return DubinsPath(a1, s, a3) } public val rsl: DubinsPath? get() { - val c1 = base.getRightCircle(turningRadius) - val c2 = direction.getLeftCircle(turningRadius) - val l = rightInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + 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 - val a1 = Arc(c1.center, base, l.base, Arc.Direction.RIGHT) - val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.LEFT) - return DubinsPath(a1, LineSegment(l), a3) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) + return DubinsPath(a1, s, a3) } public val lsr: DubinsPath? get() { - val c1 = base.getLeftCircle(turningRadius) - val c2 = direction.getRightCircle(turningRadius) - val l = leftInnerTangent(c1, c2) - if (c1.center.distanceTo(c2.center) < turningRadius * 2 || l == null) return null + 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 - val a1 = Arc(c1.center, base, l.base, Arc.Direction.LEFT) - val a3 = Arc(c2.center, l.direction, direction, Arc.Direction.RIGHT) - return DubinsPath(a1, LineSegment(l), a3) + val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) + val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) + return DubinsPath(a1, s, a3) } } @@ -133,8 +130,8 @@ private fun Pose2D.getTangentCircles(radius: Double): Pair { private fun leftOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.LEFT) private fun rightOuterTangent(a: Circle, b: Circle) = outerTangent(a, b, SIDE.RIGHT) -private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { - val centers = Line2D(a.center, b.center) +private fun outerTangent(a: Circle, b: Circle, side: SIDE): Straight { + val centers = Straight(a.center, b.center) val p1 = when (side) { SIDE.LEFT -> Vector2D( a.center.x - a.radius * cos(centers.theta), @@ -145,29 +142,28 @@ private fun outerTangent(a: Circle, b: Circle, side: SIDE): Line2D { a.center.y - a.radius * sin(centers.theta) ) } - return Line2D( + return Straight( p1, - Vector2D(p1.x + (centers.direction.x - centers.base.x), p1.y + (centers.direction.y - centers.base.y)) + Vector2D(p1.x + (centers.end.x - centers.start.x), p1.y + (centers.end.y - centers.start.y)) ) } private fun leftInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.LEFT) private fun rightInnerTangent(base: Circle, direction: Circle) = innerTangent(base, direction, SIDE.RIGHT) -private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? { - val centers = Line2D(base.center, direction.center) - return if (centers.length > base.radius * 2) { - 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) - Line2D(p1, p2) - } else { - null - } +private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Straight? { + val centers = Straight(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 Straight(p1, p2) } + +internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt index b8a81a070..b5e091db7 100644 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Arc.kt @@ -1,8 +1,8 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.dubins.theta import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Pose2D import kotlin.math.PI @@ -14,11 +14,11 @@ public class Arc( internal val direction: Direction ) : Circle(center, center.distanceTo(a)), Segment { - private val l1 = Line2D(center, a) - private val l2 = Line2D(center, b) + private val s1 = Straight(center, a) + private val s2 = Straight(center, b) - internal val pose1 = calculatePose(a, l1.theta) - internal val pose2 = calculatePose(b, l2.theta) + internal val pose1 = calculatePose(a, s1.theta) + internal val pose2 = calculatePose(b, s2.theta) private val angle = calculateAngle() override val length: Double = calculateLength() @@ -26,7 +26,7 @@ public class Arc( LEFT, RIGHT } - private fun calculateAngle() = theta(if (direction == Direction.LEFT) l1.theta - l2.theta else l2.theta - l1.theta) + private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta) private fun calculateLength(): Double { val proportion = angle / (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt deleted file mode 100644 index 0e23b27f1..000000000 --- a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt +++ /dev/null @@ -1,23 +0,0 @@ -package space.kscience.kmath.trajectory.segments - -import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Line2D -import space.kscience.kmath.operations.DoubleField.pow -import kotlin.math.PI -import kotlin.math.atan2 -import kotlin.math.sqrt - -public data class LineSegment( - internal val line: Line2D -) : Segment { - override val length: Double - get() = line.length -} - -internal val Line2D.theta: Double - get() = theta(atan2(direction.x - base.x, direction.y - base.y)) - -internal val Line2D.length: Double - get() = base.distanceTo(direction) - -internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI) diff --git a/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt new file mode 100644 index 000000000..444025d83 --- /dev/null +++ b/kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt @@ -0,0 +1,18 @@ +package space.kscience.kmath.trajectory.segments + +import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo +import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.dubins.theta +import kotlin.math.PI +import kotlin.math.atan2 + +public data class Straight( + internal val start: Vector2D, + internal val end: Vector2D +) : Segment { + override val length: Double + get() = start.distanceTo(end) + + internal val theta: Double + get() = theta(atan2(end.x - start.x, end.y - start.y)) +} 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 f52bb56f2..92b2f1df9 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 @@ -1,9 +1,8 @@ package space.kscience.kmath.trajectory -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D +import space.kscience.kmath.trajectory.segments.Straight import space.kscience.kmath.trajectory.segments.components.Pose2D -import space.kscience.kmath.trajectory.segments.theta import kotlin.math.PI import kotlin.math.abs import kotlin.math.sin @@ -15,13 +14,13 @@ fun Double.radiansToDegrees() = this * 180 / PI fun Double.equalFloat(other: Double) = abs(this - other) < maxFloatDelta fun Pose2D.equalsFloat(other: Pose2D) = x.equalFloat(other.x) && y.equalFloat(other.y) && theta.equalFloat(other.theta) -fun Line2D.inverse() = Line2D(direction, base) -fun Line2D.shift(shift: Int, width: Double): Line2D { +fun Straight.inverse() = Straight(end, start) +fun Straight.shift(shift: Int, width: Double): Straight { val dX = width * sin(inverse().theta) val dY = width * sin(theta) - return Line2D( - Vector2D(base.x - dX * shift, base.y - dY * shift), - Vector2D(direction.x - dX * shift, direction.y - dY * shift) + return Straight( + Vector2D(start.x - dX * shift, start.y - dY * shift), + Vector2D(end.x - dX * shift, end.y - dY * shift) ) } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt index 583e7a4e0..efe35e5d8 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/dubins/DubinsTests.kt @@ -6,15 +6,13 @@ package space.kscience.kmath.trajectory.dubins import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.segments.Arc -import space.kscience.kmath.trajectory.segments.LineSegment import space.kscience.kmath.trajectory.equalFloat import space.kscience.kmath.trajectory.equalsFloat import space.kscience.kmath.trajectory.inverse +import space.kscience.kmath.trajectory.segments.Straight import space.kscience.kmath.trajectory.segments.components.Pose2D -import space.kscience.kmath.trajectory.segments.theta import space.kscience.kmath.trajectory.shift import kotlin.test.Test import kotlin.test.assertNotNull @@ -25,11 +23,11 @@ class DubinsTests { @Test fun dubinsTest() { - val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) - val lineP1 = line.shift(1, 10.0).inverse() + val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + val lineP1 = straight.shift(1, 10.0).inverse() - val start = Pose2D(line.direction, line.theta) - val end = Pose2D(lineP1.base, lineP1.theta) + val start = Pose2D(straight.end, straight.theta) + val end = Pose2D(lineP1.start, lineP1.theta) val radius = 2.0 val dubins = DubinsPathFactory(start, end, radius) @@ -58,10 +56,10 @@ class DubinsTests { val b = path.b as Arc assertTrue(path.a.pose2.equalsFloat(b.pose1)) assertTrue(path.c.pose1.equalsFloat(b.pose2)) - } else if (path.b is LineSegment) { - val b = (path.b as LineSegment).line - assertTrue(path.a.pose2.equalsFloat(Pose2D(b.base, b.theta))) - assertTrue(path.c.pose1.equalsFloat(Pose2D(b.direction, b.theta))) + } else if (path.b is Straight) { + val b = path.b as Straight + assertTrue(path.a.pose2.equalsFloat(Pose2D(b.start, b.theta))) + assertTrue(path.c.pose1.equalsFloat(Pose2D(b.end, b.theta))) } } } diff --git a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt index 30f5ef6d8..e8184e178 100644 --- a/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt +++ b/kmath-trajectory/src/commonTest/kotlin/space/kscience/kmath/trajectory/segments/LineTests.kt @@ -1,7 +1,6 @@ package space.kscience.kmath.trajectory.segments import space.kscience.kmath.geometry.Euclidean2DSpace -import space.kscience.kmath.geometry.Line2D import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.trajectory.radiansToDegrees import kotlin.math.pow @@ -13,21 +12,21 @@ class LineTests { @Test fun lineTest() { - val line = Line2D(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) - assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), line.length) - assertEquals(45.0, line.theta.radiansToDegrees()) + val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0)) + assertEquals(sqrt(100.0.pow(2) + 100.0.pow(2)), straight.length) + assertEquals(45.0, straight.theta.radiansToDegrees()) } @Test fun lineAngleTest() { val zero = Vector2D(0.0, 0.0) - val north = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) + val north = Straight(Euclidean2DSpace.zero, Vector2D(0.0, 2.0)) assertEquals(0.0, north.theta.radiansToDegrees()) - val east = Line2D(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) + val east = Straight(Euclidean2DSpace.zero, Vector2D(2.0, 0.0)) assertEquals(90.0, east.theta.radiansToDegrees()) - val south = Line2D(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) + val south = Straight(Euclidean2DSpace.zero, Vector2D(0.0, -2.0)) assertEquals(180.0, south.theta.radiansToDegrees()) - val west = Line2D(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) + val west = Straight(Euclidean2DSpace.zero, Vector2D(-2.0, 0.0)) assertEquals(270.0, west.theta.radiansToDegrees()) } }