Author: luc
Date: Fri Aug 5 15:05:33 2011
New Revision: 1154257
URL: http://svn.apache.org/viewvc?rev=1154257&view=rev
Log:
Fixed a wrong detection of rotation axis versus vectors plane in Rotation
constructor using two vectors pairs.
JIRA: MATH-639
Modified:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java
commons/proper/math/trunk/src/site/xdoc/changes.xml
commons/proper/math/trunk/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java
Modified:
commons/proper/math/trunk/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java?rev=1154257&r1=1154256&r2=1154257&view=diff
==============================================================================
---
commons/proper/math/trunk/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java
(original)
+++
commons/proper/math/trunk/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Rotation.java
Fri Aug 5 15:05:33 2011
@@ -313,92 +313,51 @@ public class Rotation implements Seriali
public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) {
// norms computation
- double u1u1 = Vector3D.dotProduct(u1, u1);
- double u2u2 = Vector3D.dotProduct(u2, u2);
- double v1v1 = Vector3D.dotProduct(v1, v1);
- double v2v2 = Vector3D.dotProduct(v2, v2);
+ 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
MathRuntimeException.createIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
- double u1x = u1.getX();
- double u1y = u1.getY();
- double u1z = u1.getZ();
-
- double u2x = u2.getX();
- double u2y = u2.getY();
- double u2z = u2.getZ();
-
// normalize v1 in order to have (v1'|v1') = (u1|u1)
- double coeff = FastMath.sqrt (u1u1 / v1v1);
- double v1x = coeff * v1.getX();
- double v1y = coeff * v1.getY();
- double v1z = coeff * v1.getZ();
- v1 = new Vector3D(v1x, v1y, v1z);
-
- // adjust v2 in order to have (u1|u2) = (v1|v2) and (v2'|v2') = (u2|u2)
- double u1u2 = Vector3D.dotProduct(u1, u2);
- double v1v2 = Vector3D.dotProduct(v1, v2);
+ 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;
- double v2x = alpha * v1x + beta * v2.getX();
- double v2y = alpha * v1y + beta * v2.getY();
- double v2z = alpha * v1z + beta * v2.getZ();
- v2 = new Vector3D(v2x, v2y, v2z);
-
- // preliminary computation (we use explicit formulation instead
- // of relying on the Vector3D class in order to avoid building lots
- // of temporary objects)
- Vector3D uRef = u1;
- Vector3D vRef = v1;
- double dx1 = v1x - u1.getX();
- double dy1 = v1y - u1.getY();
- double dz1 = v1z - u1.getZ();
- double dx2 = v2x - u2.getX();
- double dy2 = v2y - u2.getY();
- double dz2 = v2z - u2.getZ();
- Vector3D k = new Vector3D(dy1 * dz2 - dz1 * dy2,
- dz1 * dx2 - dx1 * dz2,
- dx1 * dy2 - dy1 * dx2);
- double c = k.getX() * (u1y * u2z - u1z * u2y) +
- k.getY() * (u1z * u2x - u1x * u2z) +
- k.getZ() * (u1x * u2y - u1y * u2x);
+ v2 = new Vector3D(alpha, v1, beta, v2);
- if (c == 0) {
- // the (q1, q2, q3) vector is in the (u1, u2) plane
+ // 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 u3 = Vector3D.crossProduct(u1, u2);
Vector3D v3 = Vector3D.crossProduct(v1, v2);
- double u3x = u3.getX();
- double u3y = u3.getY();
- double u3z = u3.getZ();
- double v3x = v3.getX();
- double v3y = v3.getY();
- double v3z = v3.getZ();
-
- double dx3 = v3x - u3x;
- double dy3 = v3y - u3y;
- double dz3 = v3z - u3z;
- k = new Vector3D(dy1 * dz3 - dz1 * dy3,
- dz1 * dx3 - dx1 * dz3,
- dx1 * dy3 - dy1 * dx3);
- c = k.getX() * (u1y * u3z - u1z * u3y) +
- k.getY() * (u1z * u3x - u1x * u3z) +
- k.getZ() * (u1x * u3y - u1y * u3x);
-
- if (c == 0) {
- // the (q1, q2, q3) vector is aligned with u1:
- // we try (u2, u3) and (v2, v3)
- k = new Vector3D(dy2 * dz3 - dz2 * dy3,
- dz2 * dx3 - dx2 * dz3,
- dx2 * dy3 - dy2 * dx3);
- c = k.getX() * (u2y * u3z - u2z * u3y) +
- k.getY() * (u2z * u3x - u2x * u3z) +
- k.getZ() * (u2x * u3y - u2y * u3x);
+ 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) {
+ if (c <= 0) {
// the (q1, q2, q3) vector is aligned with everything
// this is really the identity rotation
q0 = 1.0;
@@ -427,8 +386,7 @@ public class Rotation implements Seriali
k = new Vector3D(uRef.getY() * q3 - uRef.getZ() * q2,
uRef.getZ() * q1 - uRef.getX() * q3,
uRef.getX() * q2 - uRef.getY() * q1);
- c = Vector3D.dotProduct(k, k);
- q0 = Vector3D.dotProduct(vRef, k) / (c + c);
+ q0 = vRef.dotProduct(k) / (2 * k.getNormSq());
}
@@ -452,7 +410,7 @@ public class Rotation implements Seriali
throw
MathRuntimeException.createIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR);
}
- double dot = Vector3D.dotProduct(u, v);
+ double dot = u.dotProduct(v);
if (dot < ((2.0e-15 - 1.0) * normProduct)) {
// special case u = -v: we select a PI angle rotation around
@@ -467,9 +425,10 @@ public class Rotation implements Seriali
// the shortest possible rotation: axis orthogonal to this plane
q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct));
double coeff = 1.0 / (2.0 * q0 * normProduct);
- q1 = coeff * (v.getY() * u.getZ() - v.getZ() * u.getY());
- q2 = coeff * (v.getZ() * u.getX() - v.getX() * u.getZ());
- q3 = coeff * (v.getX() * u.getY() - v.getY() * u.getX());
+ Vector3D q = v.crossProduct(u);
+ q1 = coeff * q.getX();
+ q2 = coeff * q.getY();
+ q3 = coeff * q.getZ();
}
}
Modified: commons/proper/math/trunk/src/site/xdoc/changes.xml
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/site/xdoc/changes.xml?rev=1154257&r1=1154256&r2=1154257&view=diff
==============================================================================
--- commons/proper/math/trunk/src/site/xdoc/changes.xml (original)
+++ commons/proper/math/trunk/src/site/xdoc/changes.xml Fri Aug 5 15:05:33 2011
@@ -52,6 +52,10 @@ The <action> type attribute can be add,u
If the output is not quite correct, check for invisible trailing spaces!
-->
<release version="3.0" date="TBD" description="TBD">
+ <action dev="luc" type="fix" issue="MATH-639" >
+ Fixed a wrong detection of rotation axis versus vectors plane in
Rotation constructor
+ using two vectors pairs.
+ </action>
<action dev="luc" type="add" >
Added a few linearCombination utility methods in MathUtils to compute
accurately
linear combinations a1.b1 + a2.b2 + ... + an.bn taking great care to
compensate
Modified:
commons/proper/math/trunk/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java
URL:
http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java?rev=1154257&r1=1154256&r2=1154257&view=diff
==============================================================================
---
commons/proper/math/trunk/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java
(original)
+++
commons/proper/math/trunk/src/test/java/org/apache/commons/math/geometry/euclidean/threed/RotationTest.java
Fri Aug 5 15:05:33 2011
@@ -17,11 +17,6 @@
package org.apache.commons.math.geometry.euclidean.threed;
-import
org.apache.commons.math.geometry.euclidean.threed.CardanEulerSingularityException;
-import
org.apache.commons.math.geometry.euclidean.threed.NotARotationMatrixException;
-import org.apache.commons.math.geometry.euclidean.threed.Rotation;
-import org.apache.commons.math.geometry.euclidean.threed.RotationOrder;
-import org.apache.commons.math.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math.util.FastMath;
import org.apache.commons.math.util.MathUtils;
import org.junit.Assert;
@@ -481,6 +476,21 @@ public class RotationTest {
}
+ @Test
+ public void testIssue639(){
+ Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0,
+ -5774608829631843.0 / 268435456.0,
+ -3822921525525679.0 / 4294967296.0);
+ Vector3D u2 =new Vector3D( -5712344449280879.0 / 2097152.0,
+ -2275058564560979.0 / 1048576.0,
+ 4423475992255071.0 / 65536.0);
+ Rotation rot = new Rotation(u1, u2, Vector3D.PLUS_I,Vector3D.PLUS_K);
+ Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0(), 1.0e-15);
+ Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1(), 1.0e-15);
+ Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2(), 1.0e-15);
+ Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15);
+ }
+
private void checkVector(Vector3D v1, Vector3D v2) {
Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
}
