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

Re-introduce line/straight segment, rename components to start/end

Browse Source
This commit is contained in:
Erik Schouten 2022-07-17 14:21:12 +02:00
parent 4f88982734
commit 7de157ce24
7 changed files with 106 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/dubins/DubinsPathFactory.kt
View File

@ -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<Circle, Circle> {
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)

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

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

23
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Line.kt
View File

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

18
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/Straight.kt Normal file
View File

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

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

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

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

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

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

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