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

Arc contains circle, circle direction is computed from poses, factory function can create Arc based on Vector points and provided direction

Browse Source
This commit is contained in:
Erik Schouten 2022-07-17 15:47:05 +02:00
parent 8faa312424
commit 86fce7ec68
4 changed files with 61 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,9 +65,9 @@ public class DubinsPath(
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, start, p1, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, p1, Arc.Direction.RIGHT)
val a2 = Arc(e.center, p1, p2, Arc.Direction.LEFT) val a2 = Arc.of(e.center, p1, p2, Arc.Direction.LEFT)
val a3 = Arc(c2.center, p2, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, p2, end, Arc.Direction.RIGHT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -87,9 +87,9 @@ public class DubinsPath(
dX = turningRadius * sin(theta) dX = turningRadius * sin(theta)
dY = turningRadius * cos(theta) dY = turningRadius * cos(theta)
val p2 = Vector2D(e.center.x + dX, e.center.y + dY) val p2 = Vector2D(e.center.x + dX, e.center.y + dY)
val a1 = Arc(c1.center, start, p1, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, p1, Arc.Direction.LEFT)
val a2 = Arc(e.center, p1, p2, Arc.Direction.RIGHT) val a2 = Arc.of(e.center, p1, p2, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, p2, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, p2, end, Arc.Direction.LEFT)
return DubinsPath(a1, a2, a3) return DubinsPath(a1, a2, a3)
} }
@ -97,8 +97,8 @@ public class DubinsPath(
val c1 = start.getRightCircle(turningRadius) val c1 = start.getRightCircle(turningRadius)
val c2 = end.getRightCircle(turningRadius) val c2 = end.getRightCircle(turningRadius)
val s = leftOuterTangent(c1, c2) val s = leftOuterTangent(c1, c2)
val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -106,8 +106,8 @@ public class DubinsPath(
val c1 = start.getLeftCircle(turningRadius) val c1 = start.getLeftCircle(turningRadius)
val c2 = end.getLeftCircle(turningRadius) val c2 = end.getLeftCircle(turningRadius)
val s = rightOuterTangent(c1, c2) val s = rightOuterTangent(c1, c2)
val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -117,8 +117,8 @@ public class DubinsPath(
val s = rightInnerTangent(c1, c2) val s = rightInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc(c1.center, start, s.start, Arc.Direction.RIGHT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.RIGHT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.LEFT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.LEFT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
@ -128,8 +128,8 @@ public class DubinsPath(
val s = leftInnerTangent(c1, c2) val s = leftInnerTangent(c1, c2)
if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null if (c1.center.distanceTo(c2.center) < turningRadius * 2 || s == null) return null
val a1 = Arc(c1.center, start, s.start, Arc.Direction.LEFT) val a1 = Arc.of(c1.center, start, s.start, Arc.Direction.LEFT)
val a3 = Arc(c2.center, s.end, end, Arc.Direction.RIGHT) val a3 = Arc.of(c2.center, s.end, end, Arc.Direction.RIGHT)
return DubinsPath(a1, s, a3) return DubinsPath(a1, s, a3)
} }
} }

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

@ -1,44 +1,56 @@
package space.kscience.kmath.trajectory.segments package space.kscience.kmath.trajectory.segments
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.dubins.theta import space.kscience.kmath.trajectory.dubins.theta
import space.kscience.kmath.trajectory.segments.components.Circle import space.kscience.kmath.trajectory.segments.components.Circle
import space.kscience.kmath.trajectory.segments.components.Pose2D import space.kscience.kmath.trajectory.segments.components.Pose2D
import kotlin.math.PI import kotlin.math.PI
public class Arc( public data class Arc(
center: Vector2D, public val circle: Circle,
a: Vector2D, public val start: Pose2D,
b: Vector2D, public val end: Pose2D
internal val direction: Direction ) : Segment {
) : Circle(center, center.distanceTo(a)), Segment {
private val s1 = Straight(center, a) internal companion object {
private val s2 = Straight(center, b) 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) private fun calculatePose(vector: Vector2D, theta: Double, direction: Direction): Pose2D =
internal val pose2 = calculatePose(b, s2.theta) Pose2D.of(
private val angle = calculateAngle() vector,
override val length: Double = calculateLength() when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
Direction.RIGHT -> theta(theta + PI / 2)
}
)
}
public enum class Direction { internal enum class Direction {
LEFT, RIGHT LEFT, RIGHT
} }
private fun calculateAngle() = theta(if (direction == Direction.LEFT) s1.theta - s2.theta else s2.theta - s1.theta) override val length: Double get() {
val angle: Double = theta(if (direction == Direction.LEFT) start.theta - end.theta else end.theta - start.theta)
private fun calculateLength(): Double {
val proportion = angle / (2 * PI) 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)
}
)
} }

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

@ -48,18 +48,18 @@ class DubinsTests {
println("${it.key}: ${path.length}") println("${it.key}: ${path.length}")
assertTrue(it.value.equalFloat(path.length)) assertTrue(it.value.equalFloat(path.length))
assertTrue(start.equalsFloat(path.a.pose1)) assertTrue(start.equalsFloat(path.a.start))
assertTrue(end.equalsFloat(path.c.pose2)) assertTrue(end.equalsFloat(path.c.end))
// Not working, theta double precision inaccuracy // Not working, theta double precision inaccuracy
if (path.b is Arc) { if (path.b is Arc) {
val b = path.b as Arc val b = path.b as Arc
assertTrue(path.a.pose2.equalsFloat(b.pose1)) assertTrue(path.a.end.equalsFloat(b.start))
assertTrue(path.c.pose1.equalsFloat(b.pose2)) assertTrue(path.c.start.equalsFloat(b.end))
} else if (path.b is Straight) { } else if (path.b is Straight) {
val b = path.b as Straight val b = path.b as Straight
assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta))) assertTrue(path.a.end.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta))) assertTrue(path.c.start.equalsFloat(Pose2D.of(b.end, b.theta)))
} }
} }
} }

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

@ -11,9 +11,9 @@ class ArcTests {
@Test @Test
fun arcTest() { fun arcTest() {
val circle = Circle(Vector2D(0.0, 0.0), 2.0) 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(circle.circumference / 4, arc.length, 1.0)
assertEquals(0.0, arc.pose1.theta.radiansToDegrees()) assertEquals(0.0, arc.start.theta.radiansToDegrees())
assertEquals(90.0, arc.pose2.theta.radiansToDegrees()) assertEquals(90.0, arc.end.theta.radiansToDegrees())
} }
} }