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

Pose2D facrtory function

Browse Source
This commit is contained in:
Erik Schouten 2022-07-17 14:39:43 +02:00
parent 7de157ce24
commit 3260c3d171
3 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -34,9 +34,11 @@ public class Arc(
}
private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
if (direction == Direction.LEFT) {
Pose2D(vector.x, vector.y, theta(theta - PI / 2))
} else {
Pose2D(vector.x, vector.y, theta(theta + PI / 2))
}
Pose2D.of(
vector,
when (direction) {
Direction.LEFT -> theta(theta - PI / 2)
Direction.RIGHT -> theta(theta + PI / 2)
}
)
}

6
kmath-trajectory/src/commonMain/kotlin/space/kscience/kmath/trajectory/segments/components/Pose2D.kt
View File

@ -1,13 +1,13 @@
package space.kscience.kmath.trajectory.segments.components
import space.kscience.kmath.geometry.Vector2D
import kotlin.math.cos
import kotlin.math.sin
public data class Pose2D(
override val x: Double,
override val y: Double,
public val theta: Double
) : Vector2D {
internal constructor(vector: Vector2D, theta: Double) : this(vector.x, vector.y, theta)
internal companion object {
internal fun of(vector: Vector2D, theta: Double) = Pose2D(vector.x, vector.y, theta)
}
}

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

@ -7,10 +7,10 @@ package space.kscience.kmath.trajectory.dubins
import space.kscience.kmath.geometry.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Arc
import space.kscience.kmath.trajectory.equalFloat
import space.kscience.kmath.trajectory.equalsFloat
import space.kscience.kmath.trajectory.inverse
import space.kscience.kmath.trajectory.segments.Arc
import space.kscience.kmath.trajectory.segments.Straight
import space.kscience.kmath.trajectory.segments.components.Pose2D
import space.kscience.kmath.trajectory.shift
@ -26,8 +26,8 @@ class DubinsTests {
val straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse()
val start = Pose2D(straight.end, straight.theta)
val end = Pose2D(lineP1.start, lineP1.theta)
val start = Pose2D.of(straight.end, straight.theta)
val end = Pose2D.of(lineP1.start, lineP1.theta)
val radius = 2.0
val dubins = DubinsPathFactory(start, end, radius)
@ -58,8 +58,8 @@ class DubinsTests {
assertTrue(path.c.pose1.equalsFloat(b.pose2))
} 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)))
assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta)))
}
}
}