kscience/kmath
9
0
Fork 1
Code Issues Pull Requests 6 Packages Projects Releases 16 Wiki 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 = private fun calculatePose(vector: Vector2D, theta: Double): Pose2D =
if (direction == Direction.LEFT) { Pose2D.of(
Pose2D(vector.x, vector.y, theta(theta - PI / 2)) vector,
} else { when (direction) {
Pose2D(vector.x, vector.y, theta(theta + PI / 2)) 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 package space.kscience.kmath.trajectory.segments.components
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Vector2D
import kotlin.math.cos
import kotlin.math.sin
public data class Pose2D( public data class Pose2D(
override val x: Double, override val x: Double,
override val y: Double, override val y: Double,
public val theta: Double public val theta: Double
) : Vector2D { ) : 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.Euclidean2DSpace.distanceTo
import space.kscience.kmath.geometry.Vector2D import space.kscience.kmath.geometry.Vector2D
import space.kscience.kmath.trajectory.segments.Arc
import space.kscience.kmath.trajectory.equalFloat import space.kscience.kmath.trajectory.equalFloat
import space.kscience.kmath.trajectory.equalsFloat import space.kscience.kmath.trajectory.equalsFloat
import space.kscience.kmath.trajectory.inverse 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.Straight
import space.kscience.kmath.trajectory.segments.components.Pose2D import space.kscience.kmath.trajectory.segments.components.Pose2D
import space.kscience.kmath.trajectory.shift 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 straight = Straight(Vector2D(0.0, 0.0), Vector2D(100.0, 100.0))
val lineP1 = straight.shift(1, 10.0).inverse() val lineP1 = straight.shift(1, 10.0).inverse()
val start = Pose2D(straight.end, straight.theta) val start = Pose2D.of(straight.end, straight.theta)
val end = Pose2D(lineP1.start, lineP1.theta) val end = Pose2D.of(lineP1.start, lineP1.theta)
val radius = 2.0 val radius = 2.0
val dubins = DubinsPathFactory(start, end, radius) val dubins = DubinsPathFactory(start, end, radius)
@ -58,8 +58,8 @@ class DubinsTests {
assertTrue(path.c.pose1.equalsFloat(b.pose2)) assertTrue(path.c.pose1.equalsFloat(b.pose2))
} 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(b.start, b.theta))) assertTrue(path.a.pose2.equalsFloat(Pose2D.of(b.start, b.theta)))
assertTrue(path.c.pose1.equalsFloat(Pose2D(b.end, b.theta))) assertTrue(path.c.pose1.equalsFloat(Pose2D.of(b.end, b.theta)))
} }
} }
} }