Fixes locking anti-pattern.

Adds new quaternion methods.
Fixes Transform methods to no longer assume rotation is a unit quaternion.

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

@@ -29,19 +29,19 @@ import com.google.common.annotations.VisibleForTesting;
 public class LazyFrameTransform {
 
   private final geometry_msgs.TransformStamped message;
-  private final Object mutex;
+
+  // Avoiding constructor code duplication.
+  private final Object mutex = new Object();
 
   private FrameTransform frameTransform;
 
   public LazyFrameTransform(geometry_msgs.TransformStamped message) {
     this.message = message;
-    mutex = new Object();
   }
 
   @VisibleForTesting
   LazyFrameTransform(FrameTransform frameTransform) {
     message = null;
-    mutex = null;
     this.frameTransform = frameTransform;
   }
 
@@ -50,14 +50,12 @@ public class LazyFrameTransform {
    *         {@link geometry_msgs.TransformStamped} message
    */
   public FrameTransform get() {
+    synchronized (mutex) {
       if (frameTransform != null) {
         return frameTransform;
       }
-    synchronized (mutex) {
-      if (frameTransform == null) {
       frameTransform = FrameTransform.fromTransformStampedMessage(message);
     }
-    }
     return frameTransform;
   }
 }
\ No newline at end of file

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

@@ -51,7 +51,8 @@ public class Quaternion {
     if (vector1.normalize().equals(vector2.normalize())) {
       return identity();
     }
-    double angle = Math.acos(vector1.dotProduct(vector2) / (vector1.getMagnitude() * vector2.getMagnitude()));
+    double angle =
+        Math.acos(vector1.dotProduct(vector2) / (vector1.getMagnitude() * vector2.getMagnitude()));
     double axisX = vector1.getY() * vector2.getZ() - vector1.getZ() * vector2.getY();
     double axisY = vector1.getZ() * vector2.getX() - vector1.getX() * vector2.getZ();
     double axisZ = vector1.getX() * vector2.getY() - vector1.getY() * vector2.getX();
@@ -69,23 +70,28 @@ public class Quaternion {
     this.w = w;
   }
 
-  public Quaternion invert() {
-    return new Quaternion(-x, -y, -z, w);
+  public Quaternion scale(double factor) {
+    return new Quaternion(x * factor, y * factor, z * factor, w * factor);
   }
 
-  public Quaternion multiply(Quaternion other) {
-    return new Quaternion(w * other.x + x * other.w + y * other.z - z * other.y, w * other.y + y
-        * other.w + z * other.x - x * other.z, w * other.z + z * other.w + x * other.y - y
-        * other.x, w * other.w - x * other.x - y * other.y - z * other.z);
+  public Quaternion conjugate() {
+    return new Quaternion(-x, -y, -z, w);
   }
 
-  public Quaternion scale(double factor) {
-    return new Quaternion(x * factor, y * factor, z * factor, w * factor);
+  public Quaternion invert() {
+    double mm = getMagnitudeSquared();
+    Preconditions.checkState(mm != 0);
+    return conjugate().scale(1 / mm);
   }
 
   public Quaternion normalize() {
-    return new Quaternion(x / getMagnitude(), y / getMagnitude(), z / getMagnitude(), w
-        / getMagnitude());
+    return scale(1 / getMagnitude());
+  }
+
+  public Quaternion multiply(Quaternion other) {
+    return new Quaternion(w * other.x + x * other.w + y * other.z - z * other.y, w * other.y + y
+        * other.w + z * other.x - x * other.z, w * other.z + z * other.w + x * other.y - y
+        * other.x, w * other.w - x * other.x - y * other.y - z * other.z);
   }
 
   public Vector3 rotateVector(Vector3 vector) {
@@ -110,8 +116,12 @@ public class Quaternion {
     return w;
   }
 
+  public double getMagnitudeSquared() {
+    return x * x + y * y + z * z + w * w;
+  }
+
   public double getMagnitude() {
-    return Math.sqrt(x * x + y * y + z * z + w * w);
+    return Math.sqrt(getMagnitudeSquared());
   }
 
   public geometry_msgs.Quaternion toQuaternionMessage(geometry_msgs.Quaternion result) {

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

@@ -55,13 +55,28 @@ public class Transform {
     return new Transform(Vector3.zero(), Quaternion.fromAxisAngle(Vector3.zAxis(), angle));
   }
 
+  public static Transform translation(double x, double y, double z) {
+    return new Transform(new Vector3(x, y, z), Quaternion.identity());
+  }
+
+  public static Transform translation(Vector3 vector) {
+    return new Transform(vector, Quaternion.identity());
+  }
+
   public Transform(Vector3 translation, Quaternion rotation) {
-    this.setTranslation(translation);
-    this.setRotation(rotation);
+    this.translation = translation;
+    this.rotation = rotation;
   }
 
+  /**
+   * Apply another {@link Transform} to this {@link Transform}.
+   * 
+   * @param other
+   *          the {@link Transform} to apply to this {@link Transform}
+   * @return the resulting {@link Transform}
+   */
   public Transform multiply(Transform other) {
-    return new Transform(translate(other.getTranslation()), rotate(other.getRotation()));
+    return new Transform(apply(other.getTranslation()), apply(other.getRotation()));
   }
 
   public Transform invert() {
@@ -69,23 +84,33 @@ public class Transform {
     return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation);
   }
 
-  public Vector3 translate(Vector3 vector) {
-    return translation.add(rotation.rotateVector(vector));
+  public Vector3 apply(Vector3 vector) {
+    return rotation.rotateVector(vector).add(translation);
   }
 
-  public Quaternion rotate(Quaternion quaternion) {
+  public Quaternion apply(Quaternion quaternion) {
     return rotation.multiply(quaternion);
   }
 
+  public Transform scale(double factor) {
+    return new Transform(translation, rotation.scale(factor));
+  }
+
+  /**
+   * @see <a
+   *      href="http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion">Quaternion
+   *      rotation matrix</a>
+   */
   public double[] toMatrix() {
     double x = getRotation().getX();
     double y = getRotation().getY();
     double z = getRotation().getZ();
     double w = getRotation().getW();
+    double mm = getRotation().getMagnitudeSquared();
     return new double[] {
-        1 - 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, 1 - 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, 1 - 2 * x * x - 2 * y * y, 0,
+        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,
         getTranslation().getX(), getTranslation().getY(), getTranslation().getZ(), 1
         };
   }
@@ -114,18 +139,10 @@ public class Transform {
     return translation;
   }
 
-  public void setTranslation(Vector3 translation) {
-    this.translation = translation;
-  }
-
   public Quaternion getRotation() {
     return rotation;
   }
 
-  public void setRotation(Quaternion rotation) {
-    this.rotation = rotation;
-  }
-
   @Override
   public String toString() {
     return String.format("Transform<%s, %s>", translation, rotation);