forked from kscience/kmath
Re-introduce line/straight segment, rename components to start/end
This commit is contained in:
parent
4f88982734
commit
7de157ce24
@ -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,17 +142,17 @@ 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) {
|
||||
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)
|
||||
@ -166,8 +163,7 @@ private fun innerTangent(base: Circle, direction: Circle, side: SIDE): Line2D? {
|
||||
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
|
||||
}
|
||||
return Straight(p1, p2)
|
||||
}
|
||||
|
||||
internal fun theta(theta: Double) = (theta + (2 * PI)) % (2 * PI)
|
||||
|
@ -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)
|
||||
|
@ -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)
|
@ -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))
|
||||
}
|
@ -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)
|
||||
)
|
||||
}
|
||||
|
@ -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)))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -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())
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user