Initial commit

src/org/usfirst/frc4388/utility/RigidTransform2d.java

@@ -0,0 +1,131 @@
+package org.usfirst.frc4388.utility;
+
+/**
+ * Represents a 2d pose (rigid transform) containing translational and
+ * rotational elements.
+ * 
+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
+ */
+public class RigidTransform2d implements Interpolable<RigidTransform2d> {
+    private final static double kEps = 1E-9;
+
+    // Movement along an arc at constant curvature and velocity. We can use
+    // ideas from "differential calculus" to create new RigidTransform2d's from
+    // a Delta.
+    public static class Delta {
+        public final double dx;
+        public final double dy;
+        public final double dtheta;
+
+        public Delta(double dx, double dy, double dtheta) {
+            this.dx = dx;
+            this.dy = dy;
+            this.dtheta = dtheta;
+        }
+    }
+
+    protected Translation2d translation_;
+    protected Rotation2d rotation_;
+
+    public RigidTransform2d() {
+        translation_ = new Translation2d();
+        rotation_ = new Rotation2d();
+    }
+
+    public RigidTransform2d(Translation2d translation, Rotation2d rotation) {
+        translation_ = translation;
+        rotation_ = rotation;
+    }
+
+    public RigidTransform2d(RigidTransform2d other) {
+        translation_ = new Translation2d(other.translation_);
+        rotation_ = new Rotation2d(other.rotation_);
+    }
+
+    public static RigidTransform2d fromTranslation(Translation2d translation) {
+        return new RigidTransform2d(translation, new Rotation2d());
+    }
+
+    public static RigidTransform2d fromRotation(Rotation2d rotation) {
+        return new RigidTransform2d(new Translation2d(), rotation);
+    }
+
+    /**
+     * Obtain a new RigidTransform2d from a (constant curvature) velocity. See:
+     * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp
+     */
+    public static RigidTransform2d fromVelocity(Delta delta) {
+        double sin_theta = Math.sin(delta.dtheta);
+        double cos_theta = Math.cos(delta.dtheta);
+        double s, c;
+        if (Math.abs(delta.dtheta) < kEps) {
+            s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta;
+            c = .5 * delta.dtheta;
+        } else {
+            s = sin_theta / delta.dtheta;
+            c = (1.0 - cos_theta) / delta.dtheta;
+        }
+        return new RigidTransform2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s),
+                new Rotation2d(cos_theta, sin_theta, false));
+    }
+
+    public Translation2d getTranslation() {
+        return translation_;
+    }
+
+    public void setTranslation(Translation2d translation) {
+        translation_ = translation;
+    }
+
+    public Rotation2d getRotation() {
+        return rotation_;
+    }
+
+    public void setRotation(Rotation2d rotation) {
+        rotation_ = rotation;
+    }
+
+    /**
+     * Transforming this RigidTransform2d means first translating by
+     * other.translation and then rotating by other.rotation
+     * 
+     * @param other
+     *            The other transform.
+     * @return This transform * other
+     */
+    public RigidTransform2d transformBy(RigidTransform2d other) {
+        return new RigidTransform2d(translation_.translateBy(other.translation_.rotateBy(rotation_)),
+                rotation_.rotateBy(other.rotation_));
+    }
+
+    /**
+     * The inverse of this transform "undoes" the effect of translating by this
+     * transform.
+     * 
+     * @return The opposite of this transform.
+     */
+    public RigidTransform2d inverse() {
+        Rotation2d rotation_inverted = rotation_.inverse();
+        return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted);
+    }
+
+    /**
+     * Do linear interpolation of this transform (there are more accurate ways
+     * using constant curvature, but this is good enough).
+     */
+    @Override
+    public RigidTransform2d interpolate(RigidTransform2d other, double x) {
+        if (x <= 0) {
+            return new RigidTransform2d(this);
+        } else if (x >= 1) {
+            return new RigidTransform2d(other);
+        }
+        return new RigidTransform2d(translation_.interpolate(other.translation_, x),
+                rotation_.interpolate(other.rotation_, x));
+    }
+
+    @Override
+    public String toString() {
+        return "T:" + translation_.toString() + ", R:" + rotation_.toString();
+    }
+}