forked from kscience/kmath
Arc contains circle, circle direction is computed from poses, factory function can create Arc based on Vector points and provided direction
This commit is contained in:
parent
8faa312424
commit
86fce7ec68
@ -65,9 +65,9 @@ public class DubinsPath(
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
|
||||
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, end, Arc.Direction.RIGHT)
|
||||
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
|
||||
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
|
||||
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
|
||||
return DubinsPath(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -87,9 +87,9 @@ public class DubinsPath(
|
||||
dX = turningRadius * sin(theta)
|
||||
dY = turningRadius * cos(theta)
|
||||
val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
|
||||
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, end, Arc.Direction.LEFT)
|
||||
val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
|
||||
val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
|
||||
val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
|
||||
return DubinsPath(a1, a2, a3)
|
||||
}
|
||||
|
||||
@ -97,8 +97,8 @@ public class DubinsPath(
|
||||
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)
|
||||
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
|
||||
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
|
||||
return DubinsPath(a1, s, a3)
|
||||
}
|
||||
|
||||
@ -106,8 +106,8 @@ public class DubinsPath(
|
||||
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)
|
||||
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
|
||||
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
|
||||
return DubinsPath(a1, s, a3)
|
||||
}
|
||||
|
||||
@ -117,8 +117,8 @@ public class DubinsPath(
|
||||
val s = rightInnerTangent(c1, c2)
|
||||
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
|
||||
|
||||
val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT)
|
||||
val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT)
|
||||
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
|
||||
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
|
||||
return DubinsPath(a1, s, a3)
|
||||
}
|
||||
|
||||
@ -128,8 +128,8 @@ public class DubinsPath(
|
||||
val s = leftInnerTangent(c1, c2)
|
||||
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
|
||||
|
||||
val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT)
|
||||
val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT)
|
||||
val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
|
||||
val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
|
||||
return DubinsPath(a1, s, a3)
|
||||
}
|
||||
}
|
||||
|
@ -1,44 +1,56 @@
|
||||
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 space.kscience.kmath.trajectory.segments.components.Circle
|
||||
import space.kscience.kmath.trajectory.segments.components.Pose2D
|
||||
import kotlin.math.PI
|
||||
|
||||
public class Arc(
|
||||
center: Vector2D,
|
||||
a: Vector2D,
|
||||
b: Vector2D,
|
||||
internal val direction: Direction
|
||||
) : Circle(center, center.distanceTo(a)), Segment {
|
||||
public data class Arc(
|
||||
public val circle: Circle,
|
||||
public val start: Pose2D,
|
||||
public val end: Pose2D
|
||||
) : Segment {
|
||||
|
||||
private val s1 = Straight(center, a)
|
||||
private val s2 = Straight(center, b)
|
||||
internal companion object {
|
||||
fun of(center: Vector2D, start: Vector2D, end: Vector2D, direction: Direction): Arc {
|
||||
val s1 = Straight(center, start)
|
||||
val s2 = Straight(center, end)
|
||||
val pose1 = calculatePose(start, s1.theta, direction)
|
||||
val pose2 = calculatePose(end, s2.theta, direction)
|
||||
return Arc(Circle(center, s1.length), pose1, pose2)
|
||||
}
|
||||
|
||||
internal val pose1 = calculatePose(a, s1.theta)
|
||||
internal val pose2 = calculatePose(b, s2.theta)
|
||||
private val angle = calculateAngle()
|
||||
override val length: Double = calculateLength()
|
||||
private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
|
||||
Pose2D.of(
|
||||
vector,
|
||||
when (direction) {
|
||||
Direction.LEFT -> theta(theta - PI / 2)
|
||||
Direction.RIGHT -> theta(theta + PI / 2)
|
||||
}
|
||||
)
|
||||
}
|
||||
|
||||
public enum class Direction {
|
||||
internal enum class Direction {
|
||||
LEFT, RIGHT
|
||||
}
|
||||
|
||||
private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta)
|
||||
|
||||
private fun calculateLength(): Double {
|
||||
override val length: Double get() {
|
||||
val angle: Double = theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
|
||||
val proportion = angle / (2 * PI)
|
||||
return circumference * proportion
|
||||
return circle.circumference * proportion
|
||||
}
|
||||
|
||||
internal val direction: Direction = if (start.y < circle.center.y) {
|
||||
if (start.theta > PI) Direction.RIGHT else Direction.LEFT
|
||||
} else if (start.y > circle.center.y) {
|
||||
if (start.theta < PI) Direction.RIGHT else Direction.LEFT
|
||||
} else {
|
||||
if (start.theta == 0.0) {
|
||||
if (start.x < circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||
} else {
|
||||
if (start.x > circle.center.x) Direction.RIGHT else Direction.LEFT
|
||||
}
|
||||
}
|
||||
|
||||
private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
|
||||
Pose2D.of(
|
||||
vector,
|
||||
when (direction) {
|
||||
Direction.LEFT -> theta(theta - PI / 2)
|
||||
Direction.RIGHT -> theta(theta + PI / 2)
|
||||
}
|
||||
)
|
||||
}
|
||||
|
@ -48,18 +48,18 @@ class DubinsTests {
|
||||
println("${it.key}: ${path.length}")
|
||||
assertTrue(it.value.equalFloat(path.length))
|
||||
|
||||
assertTrue(start.equalsFloat(path.a.pose1))
|
||||
assertTrue(end.equalsFloat(path.c.pose2))
|
||||
assertTrue(start.equalsFloat(path.a.start))
|
||||
assertTrue(end.equalsFloat(path.c.end))
|
||||
|
||||
// Not working, theta double precision inaccuracy
|
||||
if (path.b is Arc) {
|
||||
val b = path.b as Arc
|
||||
assertTrue(path.a.pose2.equalsFloat(b.pose1))
|
||||
assertTrue(path.c.pose1.equalsFloat(b.pose2))
|
||||
assertTrue(path.a.end.equalsFloat(b.start))
|
||||
assertTrue(path.c.start.equalsFloat(b.end))
|
||||
} else if (path.b is Straight) {
|
||||
val b = path.b as Straight
|
||||
assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta)))
|
||||
assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta)))
|
||||
assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
|
||||
assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -11,9 +11,9 @@ class ArcTests {
|
||||
@Test
|
||||
fun arcTest() {
|
||||
val circle = Circle(Vector2D(0.0, 0.0), 2.0)
|
||||
val arc = Arc(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
|
||||
val arc = Arc.of(circle.center, Vector2D(-2.0, 0.0), Vector2D(0.0, 2.0), Arc.Direction.RIGHT)
|
||||
assertEquals(circle.circumference / 4, arc.length, 1.0)
|
||||
assertEquals(0.0, arc.pose1.theta.radiansToDegrees())
|
||||
assertEquals(90.0, arc.pose2.theta.radiansToDegrees())
|
||||
assertEquals(0.0, arc.start.theta.radiansToDegrees())
|
||||
assertEquals(90.0, arc.end.theta.radiansToDegrees())
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user