Skip to content

Commit

Permalink
Fix final compile errors in robot project
Browse files Browse the repository at this point in the history
- Add new Pose2d(Double, Double, Double) constructor
- Add @JvmStatic on Pose2dWithCurvature.identity()
- Make Twist2d members public
- Add Translation2d.fromPolar()
  • Loading branch information
richiksc committed Jun 18, 2021
1 parent b65fd7b commit ffbd90a
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 3 deletions.
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ plugins {
}

group 'com.team1816'
version '0.1.2'
version '0.1.3'

repositories {
mavenCentral()
Expand Down
4 changes: 4 additions & 0 deletions src/commonMain/kotlin/com/team254/lib/geometry/Pose2d.kt
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,10 @@ class Pose2d : IPose2d<Pose2d> {
rotation_ = rotation
}

constructor(x: Double, y: Double, rotation: Double): this(
x, y, Rotation2d.fromDegrees(rotation)
)

constructor(translation: Translation2d, rotation: Rotation2d) {
translation_ = translation
rotation_ = rotation
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<Pose2dWithCurvature>,
ICurvature<Pose2dWithCurvature> {
Expand Down Expand Up @@ -101,6 +102,8 @@ class Pose2dWithCurvature : IPose2d<Pose2dWithCurvature>,

companion object {
private val kIdentity = Pose2dWithCurvature()

@JvmStatic
fun identity(): Pose2dWithCurvature {
return kIdentity
}
Expand Down
11 changes: 11 additions & 0 deletions src/commonMain/kotlin/com/team254/lib/geometry/Translation2d.kt
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,17 @@ class Translation2d : ITranslation2d<Translation2d> {
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_
Expand Down
5 changes: 3 additions & 2 deletions src/commonMain/kotlin/com/team254/lib/geometry/Twist2d.kt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit ffbd90a

Please sign in to comment.