From a124ad32bdb24b5d5d0ae3fa35930788ac84dd1f Mon Sep 17 00:00:00 2001 From: Ben Scholar Date: Sun, 3 Feb 2019 00:26:34 -0800 Subject: [PATCH] Added mirrored paths (#30) * added mirrored paths * documentation * Update build.gradle --- build.gradle | 2 +- .../math/geometry/Pose2dWithCurvature.kt | 4 ++ .../team5499/monkeyLib/path/MirroredPath.kt | 22 ++++++++++ .../org/team5499/monkeyLib/path/Path.kt | 40 +++++++++---------- 4 files changed, 47 insertions(+), 21 deletions(-) create mode 100644 src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt diff --git a/build.gradle b/build.gradle index a8651d8..c9a8b3c 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt b/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt index d971618..16e6995 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/math/geometry/Pose2dWithCurvature.kt +++ b/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), diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt b/src/main/kotlin/org/team5499/monkeyLib/path/MirroredPath.kt new file mode 100644 index 0000000..9c8de64 --- /dev/null +++ b/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 = 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) + } +} diff --git a/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt b/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt index 49a5946..cc032b8 100644 --- a/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt +++ b/src/main/kotlin/org/team5499/monkeyLib/path/Path.kt @@ -12,20 +12,20 @@ class Path( reversed: Boolean = false ) : CSVWritable { - private val mPoints: MutableList - private val mVelocities: MutableList + internal val points: MutableList + internal val velocities: MutableList 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,8 +70,8 @@ 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() @@ -79,10 +79,10 @@ class Path( 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()