diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java
index 41a4a981abb9638205a24c029e66c09996bb49a1..acf378e0ed16503164c534bdd8b1094d51009aad 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/LazyFrameTransform.java
+++ b/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,13 +50,11 @@ public class LazyFrameTransform {
* {@link geometry_msgs.TransformStamped} message
*/
public FrameTransform get() {
- if (frameTransform != null) {
- return frameTransform;
- }
synchronized (mutex) {
- if (frameTransform == null) {
- frameTransform = FrameTransform.fromTransformStampedMessage(message);
+ if (frameTransform != null) {
+ return frameTransform;
}
+ frameTransform = FrameTransform.fromTransformStampedMessage(message);
}
return frameTransform;
}
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
index 3217f8e90c0736b991d44d896c952dbe4a3bb7a6..f0d399ce726ed32d474f63e0382519a96c078b50 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java
+++ b/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) {
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
index f75a680d65f0af2c6e6b4a0bf7e3239872fab2c2..ba2439cc94e7161085f176741e3b4a4e36dab9fc 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java
+++ b/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);