diff --git a/build.gradle b/build.gradle index 6246f3b..b8df103 100644 --- a/build.gradle +++ b/build.gradle @@ -5,7 +5,7 @@ plugins { } group 'com.team1816' -version '0.1.2' +version '0.1.3' repositories { mavenCentral() diff --git a/src/commonMain/kotlin/com/team254/lib/geometry/Pose2d.kt b/src/commonMain/kotlin/com/team254/lib/geometry/Pose2d.kt index ccfeecc..0f7eebd 100644 --- a/src/commonMain/kotlin/com/team254/lib/geometry/Pose2d.kt +++ b/src/commonMain/kotlin/com/team254/lib/geometry/Pose2d.kt @@ -23,6 +23,10 @@ class Pose2d : IPose2d { rotation_ = rotation } + constructor(x: Double, y: Double, rotation: Double): this( + x, y, Rotation2d.fromDegrees(rotation) + ) + constructor(translation: Translation2d, rotation: Rotation2d) { translation_ = translation rotation_ = rotation diff --git a/src/commonMain/kotlin/com/team254/lib/geometry/Pose2dWithCurvature.kt b/src/commonMain/kotlin/com/team254/lib/geometry/Pose2dWithCurvature.kt index fdc0e07..db63dd5 100644 --- a/src/commonMain/kotlin/com/team254/lib/geometry/Pose2dWithCurvature.kt +++ b/src/commonMain/kotlin/com/team254/lib/geometry/Pose2dWithCurvature.kt @@ -2,6 +2,7 @@ package com.team254.lib.geometry import com.team254.lib.util.Util import com.team254.lib.util.format +import kotlin.jvm.JvmStatic class Pose2dWithCurvature : IPose2d, ICurvature { @@ -101,6 +102,8 @@ class Pose2dWithCurvature : IPose2d, companion object { private val kIdentity = Pose2dWithCurvature() + + @JvmStatic fun identity(): Pose2dWithCurvature { return kIdentity } diff --git a/src/commonMain/kotlin/com/team254/lib/geometry/Translation2d.kt b/src/commonMain/kotlin/com/team254/lib/geometry/Translation2d.kt index 9111c6e..2f883c1 100644 --- a/src/commonMain/kotlin/com/team254/lib/geometry/Translation2d.kt +++ b/src/commonMain/kotlin/com/team254/lib/geometry/Translation2d.kt @@ -139,6 +139,17 @@ class Translation2d : ITranslation2d { return kIdentity } + /** + * fromPolar courtesy 1323 + * @param direction + * @param magnitude + * @return + */ + @JvmStatic + fun fromPolar(direction: Rotation2d, magnitude: Double): Translation2d { + return Translation2d(direction.cos() * magnitude, direction.sin() * magnitude) + } + @JvmStatic fun dot(a: Translation2d, b: Translation2d): Double { return a.x_ * b.x_ + a.y_ * b.y_ diff --git a/src/commonMain/kotlin/com/team254/lib/geometry/Twist2d.kt b/src/commonMain/kotlin/com/team254/lib/geometry/Twist2d.kt index 06b9d83..629d27a 100644 --- a/src/commonMain/kotlin/com/team254/lib/geometry/Twist2d.kt +++ b/src/commonMain/kotlin/com/team254/lib/geometry/Twist2d.kt @@ -13,8 +13,9 @@ import kotlin.jvm.JvmStatic * A Twist can be used to represent a difference between two poses, a velocity, an acceleration, etc. */ class Twist2d( - val dx: Double, val dy: Double, // Radians! - val dtheta: Double + public val dx: Double, + public val dy: Double, + public val dtheta: Double // Radians! ) { fun scaled(scale: Double): Twist2d { return Twist2d(dx * scale, dy * scale, dtheta * scale)