Skip to content
Snippets Groups Projects
Commit 0df76bb1 authored by Damon Kohler's avatar Damon Kohler
Browse files

Fixes locking anti-pattern.

Adds new quaternion methods.
Fixes Transform methods to no longer assume rotation is a unit quaternion.
parent 9628f68d
Branches
No related tags found
No related merge requests found
...@@ -29,19 +29,19 @@ import com.google.common.annotations.VisibleForTesting; ...@@ -29,19 +29,19 @@ import com.google.common.annotations.VisibleForTesting;
public class LazyFrameTransform { public class LazyFrameTransform {
private final geometry_msgs.TransformStamped message; private final geometry_msgs.TransformStamped message;
private final Object mutex;
// Avoiding constructor code duplication.
private final Object mutex = new Object();
private FrameTransform frameTransform; private FrameTransform frameTransform;
public LazyFrameTransform(geometry_msgs.TransformStamped message) { public LazyFrameTransform(geometry_msgs.TransformStamped message) {
this.message = message; this.message = message;
mutex = new Object();
} }
@VisibleForTesting @VisibleForTesting
LazyFrameTransform(FrameTransform frameTransform) { LazyFrameTransform(FrameTransform frameTransform) {
message = null; message = null;
mutex = null;
this.frameTransform = frameTransform; this.frameTransform = frameTransform;
} }
...@@ -50,14 +50,12 @@ public class LazyFrameTransform { ...@@ -50,14 +50,12 @@ public class LazyFrameTransform {
* {@link geometry_msgs.TransformStamped} message * {@link geometry_msgs.TransformStamped} message
*/ */
public FrameTransform get() { public FrameTransform get() {
synchronized (mutex) {
if (frameTransform != null) { if (frameTransform != null) {
return frameTransform; return frameTransform;
} }
synchronized (mutex) {
if (frameTransform == null) {
frameTransform = FrameTransform.fromTransformStampedMessage(message); frameTransform = FrameTransform.fromTransformStampedMessage(message);
} }
}
return frameTransform; return frameTransform;
} }
} }
\ No newline at end of file
...@@ -51,7 +51,8 @@ public class Quaternion { ...@@ -51,7 +51,8 @@ public class Quaternion {
if (vector1.normalize().equals(vector2.normalize())) { if (vector1.normalize().equals(vector2.normalize())) {
return identity(); 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 axisX = vector1.getY() * vector2.getZ() - vector1.getZ() * vector2.getY();
double axisY = vector1.getZ() * vector2.getX() - vector1.getX() * vector2.getZ(); double axisY = vector1.getZ() * vector2.getX() - vector1.getX() * vector2.getZ();
double axisZ = vector1.getX() * vector2.getY() - vector1.getY() * vector2.getX(); double axisZ = vector1.getX() * vector2.getY() - vector1.getY() * vector2.getX();
...@@ -69,23 +70,28 @@ public class Quaternion { ...@@ -69,23 +70,28 @@ public class Quaternion {
this.w = w; this.w = w;
} }
public Quaternion invert() { public Quaternion scale(double factor) {
return new Quaternion(-x, -y, -z, w); return new Quaternion(x * factor, y * factor, z * factor, w * factor);
} }
public Quaternion multiply(Quaternion other) { public Quaternion conjugate() {
return new Quaternion(w * other.x + x * other.w + y * other.z - z * other.y, w * other.y + y return new Quaternion(-x, -y, -z, w);
* 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 scale(double factor) { public Quaternion invert() {
return new Quaternion(x * factor, y * factor, z * factor, w * factor); double mm = getMagnitudeSquared();
Preconditions.checkState(mm != 0);
return conjugate().scale(1 / mm);
} }
public Quaternion normalize() { public Quaternion normalize() {
return new Quaternion(x / getMagnitude(), y / getMagnitude(), z / getMagnitude(), w return scale(1 / getMagnitude());
/ 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) { public Vector3 rotateVector(Vector3 vector) {
...@@ -110,8 +116,12 @@ public class Quaternion { ...@@ -110,8 +116,12 @@ public class Quaternion {
return w; return w;
} }
public double getMagnitudeSquared() {
return x * x + y * y + z * z + w * w;
}
public double getMagnitude() { 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) { public geometry_msgs.Quaternion toQuaternionMessage(geometry_msgs.Quaternion result) {
......
...@@ -55,13 +55,28 @@ public class Transform { ...@@ -55,13 +55,28 @@ public class Transform {
return new Transform(Vector3.zero(), Quaternion.fromAxisAngle(Vector3.zAxis(), angle)); 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) { public Transform(Vector3 translation, Quaternion rotation) {
this.setTranslation(translation); this.translation = translation;
this.setRotation(rotation); 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) { 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() { public Transform invert() {
...@@ -69,23 +84,33 @@ public class Transform { ...@@ -69,23 +84,33 @@ public class Transform {
return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation); return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation);
} }
public Vector3 translate(Vector3 vector) { public Vector3 apply(Vector3 vector) {
return translation.add(rotation.rotateVector(vector)); return rotation.rotateVector(vector).add(translation);
} }
public Quaternion rotate(Quaternion quaternion) { public Quaternion apply(Quaternion quaternion) {
return rotation.multiply(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() { public double[] toMatrix() {
double x = getRotation().getX(); double x = getRotation().getX();
double y = getRotation().getY(); double y = getRotation().getY();
double z = getRotation().getZ(); double z = getRotation().getZ();
double w = getRotation().getW(); double w = getRotation().getW();
double mm = getRotation().getMagnitudeSquared();
return new double[] { return new double[] {
1 - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w, 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, 1 - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * 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, 1 - 2 * x * x - 2 * y * y, 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 getTranslation().getX(), getTranslation().getY(), getTranslation().getZ(), 1
}; };
} }
...@@ -114,18 +139,10 @@ public class Transform { ...@@ -114,18 +139,10 @@ public class Transform {
return translation; return translation;
} }
public void setTranslation(Vector3 translation) {
this.translation = translation;
}
public Quaternion getRotation() { public Quaternion getRotation() {
return rotation; return rotation;
} }
public void setRotation(Quaternion rotation) {
this.rotation = rotation;
}
@Override @Override
public String toString() { public String toString() {
return String.format("Transform<%s, %s>", translation, rotation); return String.format("Transform<%s, %s>", translation, rotation);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment