Adds additional tests and methods to aid testing.

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java

@@ -17,6 +17,9 @@
 package org.ros.rosjava_geometry;
 
 import com.google.common.base.Preconditions;
+import com.google.common.collect.Lists;
+
+import java.util.List;
 
 /**
  * A quaternion.
@@ -94,9 +97,9 @@ public class Quaternion {
         * other.x, w * other.w - x * other.x - y * other.y - z * other.z);
   }
 
-  public Vector3 rotateVector(Vector3 vector) {
+  public Vector3 rotateAndScaleVector(Vector3 vector) {
     Quaternion vectorQuaternion = new Quaternion(vector.getX(), vector.getY(), vector.getZ(), 0);
-    Quaternion rotatedQuaternion = multiply(vectorQuaternion.multiply(invert()));
+    Quaternion rotatedQuaternion = multiply(vectorQuaternion.multiply(conjugate()));
     return new Vector3(rotatedQuaternion.getX(), rotatedQuaternion.getY(), rotatedQuaternion.getZ());
   }
 
@@ -136,6 +139,20 @@ public class Quaternion {
     return result;
   }
 
+  public boolean almostEquals(Quaternion other, double epsilon) {
+    List<Double> epsilons = Lists.newArrayList();
+    epsilons.add(x - other.x);
+    epsilons.add(y - other.y);
+    epsilons.add(z - other.z);
+    epsilons.add(w - other.w);
+    for (double e : epsilons) {
+      if (Math.abs(e) > epsilon) {
+        return false;
+      }
+    }
+    return true;
+  }
+
   @Override
   public String toString() {
     return String.format("Quaternion<x: %.4f, y: %.4f, z: %.4f, w: %.4f>", x, y, z, w);

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java

@@ -17,13 +17,10 @@
 package org.ros.rosjava_geometry;
 
 import com.google.common.annotations.VisibleForTesting;
-import com.google.common.collect.Lists;
 
 import org.ros.message.Time;
 import org.ros.namespace.GraphName;
 
-import java.util.List;
-
 /**
  * A transformation in terms of translation, rotation, and scale.
  * 
@@ -87,12 +84,12 @@ public class Transform {
 
   public Transform invert() {
     Quaternion inverseRotationAndScale = rotationAndScale.invert();
-    return new Transform(inverseRotationAndScale.rotateVector(translation.invert()),
+    return new Transform(inverseRotationAndScale.rotateAndScaleVector(translation.invert()),
         inverseRotationAndScale);
   }
 
   public Vector3 apply(Vector3 vector) {
-    return rotationAndScale.rotateVector(vector).add(translation);
+    return rotationAndScale.rotateAndScaleVector(vector).add(translation);
   }
 
   public Quaternion apply(Quaternion quaternion) {
@@ -118,12 +115,10 @@ public class Transform {
     double z = rotationAndScale.getZ();
     double w = rotationAndScale.getW();
     double mm = rotationAndScale.getMagnitudeSquared();
-    return new double[] {
-        mm - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w, 0,
-        2 * x * y - 2 * z * w, mm - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * w, 0,
+    return new double[] { mm - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w,
+        0, 2 * x * y - 2 * z * w, mm - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * w, 0,
         2 * x * z + 2 * y * w, 2 * y * z - 2 * x * w, mm - 2 * x * x - 2 * y * y, 0,
-        translation.getX(), translation.getY(), translation.getZ(), 1
-        };
+        translation.getX(), translation.getY(), translation.getZ(), 1 };
   }
 
   public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) {
@@ -147,20 +142,8 @@ public class Transform {
   }
 
   public boolean almostEquals(Transform other, double epsilon) {
-    List<Double> epsilons = Lists.newArrayList();
-    epsilons.add(translation.getX() - other.getTranslation().getX());
-    epsilons.add(translation.getY() - other.getTranslation().getY());
-    epsilons.add(translation.getZ() - other.getTranslation().getZ());
-    epsilons.add(rotationAndScale.getX() - other.getRotationAndScale().getX());
-    epsilons.add(rotationAndScale.getY() - other.getRotationAndScale().getY());
-    epsilons.add(rotationAndScale.getZ() - other.getRotationAndScale().getZ());
-    epsilons.add(rotationAndScale.getW() - other.getRotationAndScale().getW());
-    for (double e : epsilons) {
-      if (Math.abs(e) > epsilon) {
-        return false;
-      }
-    }
-    return true;
+    return translation.almostEquals(other.translation, epsilon)
+        && rotationAndScale.almostEquals(other.rotationAndScale, epsilon);
   }
 
   @VisibleForTesting

rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java

@@ -16,6 +16,10 @@
 
 package org.ros.rosjava_geometry;
 
+import com.google.common.collect.Lists;
+
+import java.util.List;
+
 /**
  * A three dimensional vector.
  * 
@@ -78,6 +82,10 @@ public class Vector3 {
     return new Vector3(x / getMagnitude(), y / getMagnitude(), z / getMagnitude());
   }
 
+  public Vector3 scale(double factor) {
+    return new Vector3(x * factor, y * factor, z * factor);
+  }
+
   public double getX() {
     return x;
   }
@@ -90,8 +98,12 @@ public class Vector3 {
     return z;
   }
 
+  private double getMagnitudeSquared() {
+    return x * x + y * y + z * z;
+  }
+
   public double getMagnitude() {
-    return Math.sqrt(x * x + y * y + z * z);
+    return Math.sqrt(getMagnitudeSquared());
   }
 
   public geometry_msgs.Vector3 toVector3Message(geometry_msgs.Vector3 result) {
@@ -108,6 +120,19 @@ public class Vector3 {
     return result;
   }
 
+  public boolean almostEquals(Vector3 other, double epsilon) {
+    List<Double> epsilons = Lists.newArrayList();
+    epsilons.add(x - other.x);
+    epsilons.add(y - other.y);
+    epsilons.add(z - other.z);
+    for (double e : epsilons) {
+      if (Math.abs(e) > epsilon) {
+        return false;
+      }
+    }
+    return true;
+  }
+
   @Override
   public String toString() {
     return String.format("Vector3<x: %.4f, y: %.4f, z: %.4f>", x, y, z);

rosjava_geometry/src/test/java/org/ros/rosjava_geometry/QuaternionTest.java

@@ -93,7 +93,7 @@ public class QuaternionTest {
   public void testRotateVector() {
     Quaternion quaternion = Quaternion.fromAxisAngle(Vector3.zAxis(), Math.PI / 2);
     Vector3 vector = new Vector3(1, 0, 0);
-    Vector3 rotated = quaternion.rotateVector(vector);
+    Vector3 rotated = quaternion.rotateAndScaleVector(vector);
     assertEquals(0, rotated.getX(), 1e-9);
     assertEquals(1, rotated.getY(), 1e-9);
     assertEquals(0, rotated.getZ(), 1e-9);

rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java

@@ -76,14 +76,45 @@ public class TransformTest {
     Random random = new Random();
     random.setSeed(42);
     for (int i = 0; i < 10000; i++) {
-      Vector3 vector = new Vector3(random.nextDouble(), random.nextDouble(), random.nextDouble());
-      Quaternion quaternion =
-          new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(),
-              random.nextDouble());
+      Vector3 vector = randomVector(random);
+      Quaternion quaternion = randomQuaternion(random);
       Transform transform = new Transform(vector, quaternion);
       Transform inverse = transform.invert();
       Transform neutral = transform.multiply(inverse);
       assertTrue(neutral.almostEquals(Transform.identity(), 1e-9));
     }
   }
+
+  @Test
+  public void testMultiplyRandom() {
+    Random random = new Random();
+    random.setSeed(42);
+    for (int i = 0; i < 10000; i++) {
+      Vector3 data = randomVector(random);
+      Vector3 vector1 = randomVector(random);
+      Vector3 vector2 = randomVector(random);
+      Quaternion quaternion1 = randomQuaternion(random);
+      Quaternion quaternion2 = randomQuaternion(random);
+      Transform transform1 = new Transform(vector1, quaternion1);
+      Transform transform2 = new Transform(vector2, quaternion2);
+      Vector3 result1 = transform1.apply(transform2.apply(data));
+      Vector3 result2 = transform1.multiply(transform2).apply(data);
+      assertTrue(result1.almostEquals(result2, 1e-9));
+    }
+  }
+
+  @Test
+  public void testScale() {
+    assertTrue(Vector3.xAxis().scale(2)
+        .almostEquals(Transform.identity().scale(2).apply(Vector3.xAxis()), 1e-9));
+  }
+
+  private Quaternion randomQuaternion(Random random) {
+    return new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(),
+        random.nextDouble());
+  }
+
+  private Vector3 randomVector(Random random) {
+    return new Vector3(random.nextDouble(), random.nextDouble(), random.nextDouble());
+  }
 }