You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@commons.apache.org by lu...@apache.org on 2012/06/05 20:26:02 UTC
svn commit: r1346513 - in /commons/proper/math/trunk/src: changes/changes.xml
main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java
test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java
Author: luc
Date: Tue Jun 5 18:26:02 2012
New Revision: 1346513
URL: http://svn.apache.org/viewvc?rev=1346513&view=rev
Log:
Fixed a problem when building rotations from two pairs of vectors.
In very rare cases, due to numerical inaccuracies the computed quaternion
was not normalized (some examples went as high as 1.0e8) and even after
normalization, the quaternion was plain wrong.
JIRA: MATH-801
Modified:
commons/proper/math/trunk/src/changes/changes.xml
commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java
commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java
Modified: commons/proper/math/trunk/src/changes/changes.xml
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/changes/changes.xml?rev=1346513&r1=1346512&r2=1346513&view=diff
==============================================================================
--- commons/proper/math/trunk/src/changes/changes.xml (original)
+++ commons/proper/math/trunk/src/changes/changes.xml Tue Jun 5 18:26:02 2012
@@ -52,6 +52,11 @@ If the output is not quite correct, chec
<body>
<release version="3.1" date="TBD" description="
">
+ <action dev="luc" type="fix" issue="MATH-801">
+ Fixed a problem when building rotations from two pairs of vectors. In very rare cases,
+ due to numerical inaccuracies the computed quaternion was not normalized (some examples
+ went as high as 1.0e8) and even after normalization, the quaternion was plain wrong.
+ </action>
<action dev="celestin" type="remove" issue="MATH-796">
Removed unused fields LocalizedFormats.ALPHA and LocalizedFormats.BETA. This is
an acceptable compatibility break, as these fields are only meant for internal
Modified: commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java?rev=1346513&r1=1346512&r2=1346513&view=diff
==============================================================================
--- commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java (original)
+++ commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java Tue Jun 5 18:26:02 2012
@@ -22,6 +22,7 @@ import java.io.Serializable;
import org.apache.commons.math3.exception.MathIllegalArgumentException;
import org.apache.commons.math3.exception.util.LocalizedFormats;
import org.apache.commons.math3.util.FastMath;
+import org.apache.commons.math3.util.MathArrays;
/**
* This class implements rotations in a three-dimensional space.
@@ -241,54 +242,72 @@ public class Rotation implements Seriali
det);
}
- // There are different ways to compute the quaternions elements
- // from the matrix. They all involve computing one element from
- // the diagonal of the matrix, and computing the three other ones
- // using a formula involving a division by the first element,
- // which unfortunately can be zero. Since the norm of the
- // quaternion is 1, we know at least one element has an absolute
- // value greater or equal to 0.5, so it is always possible to
- // select the right formula and avoid division by zero and even
- // numerical inaccuracy. Checking the elements in turn and using
- // the first one greater than 0.45 is safe (this leads to a simple
- // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19)
- double s = ort[0][0] + ort[1][1] + ort[2][2];
- if (s > -0.19) {
- // compute q0 and deduce q1, q2 and q3
- q0 = 0.5 * FastMath.sqrt(s + 1.0);
- double inv = 0.25 / q0;
- q1 = inv * (ort[1][2] - ort[2][1]);
- q2 = inv * (ort[2][0] - ort[0][2]);
- q3 = inv * (ort[0][1] - ort[1][0]);
- } else {
- s = ort[0][0] - ort[1][1] - ort[2][2];
+ double[] quat = mat2quat(ort);
+ q0 = quat[0];
+ q1 = quat[1];
+ q2 = quat[2];
+ q3 = quat[3];
+
+ }
+
+ /** Convert an orthogonal rotation matrix to a quaternion.
+ * @param ort orthogonal rotation matrix
+ * @return quaternion corresponding to the matrix
+ */
+ private static double[] mat2quat(final double[][] ort) {
+
+ final double[] quat = new double[4];
+
+ // There are different ways to compute the quaternions elements
+ // from the matrix. They all involve computing one element from
+ // the diagonal of the matrix, and computing the three other ones
+ // using a formula involving a division by the first element,
+ // which unfortunately can be zero. Since the norm of the
+ // quaternion is 1, we know at least one element has an absolute
+ // value greater or equal to 0.5, so it is always possible to
+ // select the right formula and avoid division by zero and even
+ // numerical inaccuracy. Checking the elements in turn and using
+ // the first one greater than 0.45 is safe (this leads to a simple
+ // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19)
+ double s = ort[0][0] + ort[1][1] + ort[2][2];
if (s > -0.19) {
- // compute q1 and deduce q0, q2 and q3
- q1 = 0.5 * FastMath.sqrt(s + 1.0);
- double inv = 0.25 / q1;
- q0 = inv * (ort[1][2] - ort[2][1]);
- q2 = inv * (ort[0][1] + ort[1][0]);
- q3 = inv * (ort[0][2] + ort[2][0]);
+ // compute q0 and deduce q1, q2 and q3
+ quat[0] = 0.5 * FastMath.sqrt(s + 1.0);
+ double inv = 0.25 / quat[0];
+ quat[1] = inv * (ort[1][2] - ort[2][1]);
+ quat[2] = inv * (ort[2][0] - ort[0][2]);
+ quat[3] = inv * (ort[0][1] - ort[1][0]);
} else {
- s = ort[1][1] - ort[0][0] - ort[2][2];
- if (s > -0.19) {
- // compute q2 and deduce q0, q1 and q3
- q2 = 0.5 * FastMath.sqrt(s + 1.0);
- double inv = 0.25 / q2;
- q0 = inv * (ort[2][0] - ort[0][2]);
- q1 = inv * (ort[0][1] + ort[1][0]);
- q3 = inv * (ort[2][1] + ort[1][2]);
- } else {
- // compute q3 and deduce q0, q1 and q2
- s = ort[2][2] - ort[0][0] - ort[1][1];
- q3 = 0.5 * FastMath.sqrt(s + 1.0);
- double inv = 0.25 / q3;
- q0 = inv * (ort[0][1] - ort[1][0]);
- q1 = inv * (ort[0][2] + ort[2][0]);
- q2 = inv * (ort[2][1] + ort[1][2]);
- }
+ s = ort[0][0] - ort[1][1] - ort[2][2];
+ if (s > -0.19) {
+ // compute q1 and deduce q0, q2 and q3
+ quat[1] = 0.5 * FastMath.sqrt(s + 1.0);
+ double inv = 0.25 / quat[1];
+ quat[0] = inv * (ort[1][2] - ort[2][1]);
+ quat[2] = inv * (ort[0][1] + ort[1][0]);
+ quat[3] = inv * (ort[0][2] + ort[2][0]);
+ } else {
+ s = ort[1][1] - ort[0][0] - ort[2][2];
+ if (s > -0.19) {
+ // compute q2 and deduce q0, q1 and q3
+ quat[2] = 0.5 * FastMath.sqrt(s + 1.0);
+ double inv = 0.25 / quat[2];
+ quat[0] = inv * (ort[2][0] - ort[0][2]);
+ quat[1] = inv * (ort[0][1] + ort[1][0]);
+ quat[3] = inv * (ort[2][1] + ort[1][2]);
+ } else {
+ // compute q3 and deduce q0, q1 and q2
+ s = ort[2][2] - ort[0][0] - ort[1][1];
+ quat[3] = 0.5 * FastMath.sqrt(s + 1.0);
+ double inv = 0.25 / quat[3];
+ quat[0] = inv * (ort[0][1] - ort[1][0]);
+ quat[1] = inv * (ort[0][2] + ort[2][0]);
+ quat[2] = inv * (ort[2][1] + ort[1][2]);
+ }
+ }
}
- }
+
+ return quat;
}
@@ -308,85 +327,48 @@ public class Rotation implements Seriali
* @param u2 second vector of the origin pair
* @param v1 desired image of u1 by the rotation
* @param v2 desired image of u2 by the rotation
- * @exception MathIllegalArgumentException if the norm of one of the vectors is zero
+ * @exception MathIllegalArgumentException if the norm of one of the vectors is zero,
+ * or if one of the pair is degenerated (i.e. the vectors of the pair are colinear)
*/
- public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) {
+ public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2)
+ throws MathIllegalArgumentException {
- // norms computation
- double u1u1 = u1.getNormSq();
- double u2u2 = u2.getNormSq();
- double v1v1 = v1.getNormSq();
- double v2v2 = v2.getNormSq();
- if ((u1u1 == 0) || (u2u2 == 0) || (v1v1 == 0) || (v2v2 == 0)) {
- throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
- }
-
- // normalize v1 in order to have (v1'|v1') = (u1|u1)
- v1 = new Vector3D(FastMath.sqrt(u1u1 / v1v1), v1);
-
- // adjust v2 in order to have (u1|u2) = (v1'|v2') and (v2'|v2') = (u2|u2)
- double u1u2 = u1.dotProduct(u2);
- double v1v2 = v1.dotProduct(v2);
- double coeffU = u1u2 / u1u1;
- double coeffV = v1v2 / u1u1;
- double beta = FastMath.sqrt((u2u2 - u1u2 * coeffU) / (v2v2 - v1v2 * coeffV));
- double alpha = coeffU - beta * coeffV;
- v2 = new Vector3D(alpha, v1, beta, v2);
-
- // preliminary computation
- Vector3D uRef = u1;
- Vector3D vRef = v1;
- Vector3D v1Su1 = v1.subtract(u1);
- Vector3D v2Su2 = v2.subtract(u2);
- Vector3D k = v1Su1.crossProduct(v2Su2);
- Vector3D u3 = u1.crossProduct(u2);
- double c = k.dotProduct(u3);
- final double inPlaneThreshold = 0.001;
- if (c <= inPlaneThreshold * k.getNorm() * u3.getNorm()) {
- // the (q1, q2, q3) vector is close to the (u1, u2) plane
- // we try other vectors
- Vector3D v3 = Vector3D.crossProduct(v1, v2);
- Vector3D v3Su3 = v3.subtract(u3);
- k = v1Su1.crossProduct(v3Su3);
- Vector3D u2Prime = u1.crossProduct(u3);
- c = k.dotProduct(u2Prime);
-
- if (c <= inPlaneThreshold * k.getNorm() * u2Prime.getNorm()) {
- // the (q1, q2, q3) vector is also close to the (u1, u3) plane,
- // it is almost aligned with u1: we try (u2, u3) and (v2, v3)
- k = v2Su2.crossProduct(v3Su3);
- c = k.dotProduct(u2.crossProduct(u3));
-
- if (c <= 0) {
- // the (q1, q2, q3) vector is aligned with everything
- // this is really the identity rotation
- q0 = 1.0;
- q1 = 0.0;
- q2 = 0.0;
- q3 = 0.0;
- return;
- }
-
- // we will have to use u2 and v2 to compute the scalar part
- uRef = u2;
- vRef = v2;
-
- }
-
- }
+ // build orthonormalized base from u1, u2
+ // this fails when vectors are null or colinear, which is forbidden to define a rotation
+ final Vector3D u3 = u1.crossProduct(u2).normalize();
+ u2 = u3.crossProduct(u1).normalize();
+ u1 = u1.normalize();
+
+ // build an orthonormalized base from v1, v2
+ // this fails when vectors are null or colinear, which is forbidden to define a rotation
+ final Vector3D v3 = v1.crossProduct(v2).normalize();
+ v2 = v3.crossProduct(v1).normalize();
+ v1 = v1.normalize();
+
+ // buid a matrix transforming the first base into the second one
+ final double[][] m = new double[][] {
+ {
+ MathArrays.linearCombination(u1.getX(), v1.getX(), u2.getX(), v2.getX(), u3.getX(), v3.getX()),
+ MathArrays.linearCombination(u1.getY(), v1.getX(), u2.getY(), v2.getX(), u3.getY(), v3.getX()),
+ MathArrays.linearCombination(u1.getZ(), v1.getX(), u2.getZ(), v2.getX(), u3.getZ(), v3.getX())
+ },
+ {
+ MathArrays.linearCombination(u1.getX(), v1.getY(), u2.getX(), v2.getY(), u3.getX(), v3.getY()),
+ MathArrays.linearCombination(u1.getY(), v1.getY(), u2.getY(), v2.getY(), u3.getY(), v3.getY()),
+ MathArrays.linearCombination(u1.getZ(), v1.getY(), u2.getZ(), v2.getY(), u3.getZ(), v3.getY())
+ },
+ {
+ MathArrays.linearCombination(u1.getX(), v1.getZ(), u2.getX(), v2.getZ(), u3.getX(), v3.getZ()),
+ MathArrays.linearCombination(u1.getY(), v1.getZ(), u2.getY(), v2.getZ(), u3.getY(), v3.getZ()),
+ MathArrays.linearCombination(u1.getZ(), v1.getZ(), u2.getZ(), v2.getZ(), u3.getZ(), v3.getZ())
+ }
+ };
- // compute the vectorial part
- c = FastMath.sqrt(c);
- double inv = 1.0 / (c + c);
- q1 = inv * k.getX();
- q2 = inv * k.getY();
- q3 = inv * k.getZ();
-
- // compute the scalar part
- k = new Vector3D(uRef.getY() * q3 - uRef.getZ() * q2,
- uRef.getZ() * q1 - uRef.getX() * q3,
- uRef.getX() * q2 - uRef.getY() * q1);
- q0 = vRef.dotProduct(k) / (2 * k.getNormSq());
+ double[] quat = mat2quat(m);
+ q0 = quat[0];
+ q1 = quat[1];
+ q2 = quat[2];
+ q3 = quat[3];
}
Modified: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java?rev=1346513&r1=1346512&r2=1346513&view=diff
==============================================================================
--- commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java (original)
+++ commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java Tue Jun 5 18:26:02 2012
@@ -17,6 +17,7 @@
package org.apache.commons.math3.geometry.euclidean.threed;
+import org.apache.commons.math3.exception.MathArithmeticException;
import org.apache.commons.math3.exception.MathIllegalArgumentException;
import org.apache.commons.math3.util.FastMath;
import org.apache.commons.math3.util.MathUtils;
@@ -141,7 +142,7 @@ public class RotationTest {
try {
new Rotation(u1, u2, Vector3D.ZERO, v2);
Assert.fail("an exception should have been thrown");
- } catch (IllegalArgumentException e) {
+ } catch (MathArithmeticException e) {
// expected behavior
}
@@ -517,6 +518,25 @@ public class RotationTest {
Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15);
}
+ @Test
+ public void testIssue801() {
+ Vector3D u1 = new Vector3D(0.9999988431610581, -0.0015210774290851095, 0.0);
+ Vector3D u2 = new Vector3D(0.0, 0.0, 1.0);
+
+ Vector3D v1 = new Vector3D(0.9999999999999999, 0.0, 0.0);
+ Vector3D v2 = new Vector3D(0.0, 0.0, -1.0);
+
+ Rotation quat = new Rotation(u1, u2, v1, v2);
+ double q2 = quat.getQ0() * quat.getQ0() +
+ quat.getQ1() * quat.getQ1() +
+ quat.getQ2() * quat.getQ2() +
+ quat.getQ3() * quat.getQ3();
+ Assert.assertEquals(1.0, q2, 1.0e-14);
+ Assert.assertEquals(0.0, Vector3D.angle(v1, quat.applyTo(u1)), 1.0e-14);
+ Assert.assertEquals(0.0, Vector3D.angle(v2, quat.applyTo(u2)), 1.0e-14);
+
+ }
+
private void checkVector(Vector3D v1, Vector3D v2) {
Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
}