Added mirrored paths (#30)

* added mirrored paths

* documentation

* Update build.gradle

build.gradle

@@ -32,7 +32,7 @@ dependencies {
 }
 
 group = 'org.team5499'
-version = '2.4.0'
+version = '2.5.0'
 
 task sourcesJar(type: Jar) {
     from sourceSets.main.allJava

src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt

@@ -31,6 +31,10 @@ class Pose2dWithCurvature(
     constructor(): this(Vector2(), Rotation2d(), 0.0, 0.0)
     constructor(other: Pose2dWithCurvature): this(other.translation, other.rotation, other.curvature, other.dCurvature)
 
+    public fun mirror(): Pose2dWithCurvature {
+        return Pose2dWithCurvature(pose.mirror(), -curvature, -dCurvature)
+    }
+
     override fun interpolate(other: Pose2dWithCurvature, x: Double): Pose2dWithCurvature {
         return Pose2dWithCurvature(pose.interpolate(other.pose, x),
             Utils.interpolate(curvature, other.curvature, x),

src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt

@@ -0,0 +1,22 @@
+package org.team5499.monkeyLib.path
+
+import org.team5499.monkeyLib.math.geometry.Pose2dWithCurvature
+
+/**
+* this class represents a path that can be mirrored over the centerline of the field
+* @property left is path on the left side of the field
+*/
+public class MirroredPath(left: Path) {
+
+    public val left: Path
+    public val right: Path
+
+    init {
+        this.left = left
+        val newPoints: MutableList<Pose2dWithCurvature> = left.points.toMutableList()
+        for (i in 0..newPoints.size - 1) {
+            newPoints.set(i, newPoints.get(i).mirror())
+        }
+        this.right = Path(newPoints, left.velocities, left.reversed)
+    }
+}

src/main/kotlin/org/team5499/monkeyLib/path/Path.kt

@@ -12,20 +12,20 @@ class Path(
     reversed: Boolean = false
 ) : CSVWritable {
 
-    private val mPoints: MutableList<Pose2dWithCurvature>
-    private val mVelocities: MutableList<Double>
+    internal val points: MutableList<Pose2dWithCurvature>
+    internal val velocities: MutableList<Double>
 
     val pathLength: Int
-        get() = mPoints.size
+        get() = points.size
 
     val reversed: Boolean
         get() = field
 
     val startPose: Pose2dWithCurvature
-        get() = Pose2dWithCurvature(mPoints.get(0))
+        get() = Pose2dWithCurvature(points.get(0))
 
     val endPose: Pose2dWithCurvature
-        get() = Pose2dWithCurvature(mPoints.get(pathLength - 1))
+        get() = Pose2dWithCurvature(points.get(pathLength - 1))
 
     init {
         if (points.size != velocities.size) {
@@ -34,32 +34,32 @@ class Path(
         }
         if (points.size < 2) throw IllegalArgumentException("Needs to be more than 2 points for a path")
         this.reversed = reversed
-        mPoints = points.toMutableList()
-        mVelocities = velocities.toMutableList()
+        this.points = points.toMutableList()
+        this.velocities = velocities.toMutableList()
     }
 
-    constructor(other: Path): this(other.mPoints.toMutableList(), other.mVelocities, other.reversed)
+    constructor(other: Path): this(other.points.toMutableList(), other.velocities, other.reversed)
 
     fun getPose(index: Int): Pose2dWithCurvature {
-        if (index >= mPoints.size || index < 0) {
+        if (index >= points.size || index < 0) {
             throw IndexOutOfBoundsException("Point $index not in path")
         }
-        return mPoints.get(index)
+        return points.get(index)
     }
 
     fun getVelocity(index: Int): Double {
-        if (index >= mPoints.size || index < 0) {
+        if (index >= points.size || index < 0) {
             throw IndexOutOfBoundsException("Point $index not in velocities")
         }
-        return mVelocities.get(index)
+        return velocities.get(index)
     }
 
     fun findClosestPointIndex(point: Pose2d, lastIndex: Int): Int {
-        val lastPose: Vector2 = mPoints.get(lastIndex).translation
+        val lastPose: Vector2 = points.get(lastIndex).translation
         var minDistance: Double = Vector2.distanceBetween(point.translation, lastPose)
         var index: Int = lastIndex
-        for (i in lastIndex..mPoints.size - 1) {
-            val tempDistance: Double = Vector2.distanceBetween(point.translation, mPoints.get(i).translation)
+        for (i in lastIndex..points.size - 1) {
+            val tempDistance: Double = Vector2.distanceBetween(point.translation, points.get(i).translation)
             if (tempDistance < minDistance) {
                 index = i
                 minDistance = tempDistance
@@ -70,19 +70,19 @@ class Path(
 
     override fun toString(): String {
         val buffer: StringBuilder = StringBuilder()
-        for (i in 0..mPoints.size - 1) {
-            buffer.append(mPoints.get(i).toString())
+        for (i in 0..points.size - 1) {
+            buffer.append(points.get(i).toString())
             buffer.append(System.lineSeparator())
         }
         return buffer.toString()
     }
 
     override fun toCSV(): String {
         val buffer: StringBuilder = StringBuilder()
-        for (i in 0..mPoints.size - 1) {
-            buffer.append(mPoints.get(i).pose.toCSV())
+        for (i in 0..points.size - 1) {
+            buffer.append(points.get(i).pose.toCSV())
             buffer.append(", ")
-            buffer.append(mVelocities.get(i))
+            buffer.append(velocities.get(i))
             buffer.append(System.lineSeparator())
         }
         return buffer.toString()