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
No related branches found
No related tags found
No related merge requests found
......@@ -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
......@@ -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) {
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment