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 55d33b6041becfffe41a2268738b41b0f1c449b1..b4c7e72e5ab3a7678b17822bb67b073dd5a1a112 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
@@ -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);
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 6d23eb7999ad789670f1cb363d582299e24d2185..3d1727deb044c56314a59755b5243978822bdc8c 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
@@ -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
diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java
index 76f0e62b62986fc79c049e85b2b14d0c1eda2ee3..5cf1977e9494c3019c599fdd44e334b48c5af2ea 100644
--- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Vector3.java
+++ b/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);
diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/QuaternionTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/QuaternionTest.java
index e0e83fdbac3c24d71c5c15be45d0e877a4a82cc2..5a2717c6437dc02a5ded54ad2a507636ea0140f1 100644
--- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/QuaternionTest.java
+++ b/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);
diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java
index 8373829cd7abe099483a645636d584c6d1f09a23..f1c73e374c9678533f90e09c94dd188f34df2708 100644
--- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java
+++ b/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());
+ }
}