Apache/commons-math
1
0
Fork 0
mirror of https://github.com/apache/commons-math.git synced 2025-02-08 02:59:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed a problem when building rotations from two pairs of vectors.

Browse Source 
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

git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1346513 13f79535-47bb-0310-9956-ffa450edef68
This commit is contained in:
Luc Maisonobe 2012-06-05 18:26:02 +00:00
parent 09d7f9a7b8
commit 118e94b5e9
3 changed files with 127 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/changes/changes.xml
View File

@ -52,6 +52,11 @@ If the output is not quite correct, check for invisible trailing spaces!
<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

220
src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java
View File

@ -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 Serializable {
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 Serializable {
* @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);
}
// 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();
// normalize v1 in order to have (v1'|v1') = (u1|u1)
v1 = new Vector3D(FastMath.sqrt(u1u1 / v1v1), v1);
// 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();
// 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);
// 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())
}
};
// 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;
}
}
// 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];
}

22
src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationTest.java
View File

@ -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);
}