diff --git a/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Vector3D.java b/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Vector3D.java index fca6c65c8..ac4653976 100644 --- a/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Vector3D.java +++ b/src/main/java/org/apache/commons/math/geometry/euclidean/threed/Vector3D.java @@ -132,9 +132,9 @@ public class Vector3D implements Serializable, Vector { * @param u2 second base (unscaled) vector */ public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2) { - this.x = a1 * u1.x + a2 * u2.x; - this.y = a1 * u1.y + a2 * u2.y; - this.z = a1 * u1.z + a2 * u2.z; + this.x = MathUtils.linearCombination(a1, u1.x, a2, u2.x); + this.y = MathUtils.linearCombination(a1, u1.y, a2, u2.y); + this.z = MathUtils.linearCombination(a1, u1.z, a2, u2.z); } /** Linear constructor @@ -149,9 +149,9 @@ public class Vector3D implements Serializable, Vector { */ public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2, double a3, Vector3D u3) { - this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x; - this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y; - this.z = a1 * u1.z + a2 * u2.z + a3 * u3.z; + this.x = MathUtils.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x); + this.y = MathUtils.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y); + this.z = MathUtils.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z); } /** Linear constructor @@ -168,9 +168,9 @@ public class Vector3D implements Serializable, Vector { */ public Vector3D(double a1, Vector3D u1, double a2, Vector3D u2, double a3, Vector3D u3, double a4, Vector3D u4) { - this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x + a4 * u4.x; - this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y + a4 * u4.y; - this.z = a1 * u1.z + a2 * u2.z + a3 * u3.z + a4 * u4.z; + this.x = MathUtils.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x, a4, u4.x); + this.y = MathUtils.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y, a4, u4.y); + this.z = MathUtils.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z, a4, u4.z); } /** Get the abscissa of the vector. @@ -214,11 +214,13 @@ public class Vector3D implements Serializable, Vector { /** {@inheritDoc} */ public double getNorm() { + // there are no cancellation problems here, so we use the straightforward formula return FastMath.sqrt (x * x + y * y + z * z); } /** {@inheritDoc} */ public double getNormSq() { + // there are no cancellation problems here, so we use the straightforward formula return x * x + y * y + z * z; } @@ -251,8 +253,7 @@ public class Vector3D implements Serializable, Vector { /** {@inheritDoc} */ public Vector3D add(double factor, final Vector v) { - final Vector3D v3 = (Vector3D) v; - return new Vector3D(x + factor * v3.x, y + factor * v3.y, z + factor * v3.z); + return new Vector3D(1, this, factor, (Vector3D) v); } /** {@inheritDoc} */ @@ -263,8 +264,7 @@ public class Vector3D implements Serializable, Vector { /** {@inheritDoc} */ public Vector3D subtract(final double factor, final Vector v) { - final Vector3D v3 = (Vector3D) v; - return new Vector3D(x - factor * v3.x, y - factor * v3.y, z - factor * v3.z); + return new Vector3D(1, this, -factor, (Vector3D) v); } /** {@inheritDoc} */ @@ -328,7 +328,7 @@ public class Vector3D implements Serializable, Vector { throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); } - double dot = dotProduct(v1, v2); + double dot = v1.dotProduct(v2); double threshold = normProduct * 0.9999; if ((dot < -threshold) || (dot > threshold)) { // the vectors are almost aligned, compute using the sine @@ -416,52 +416,28 @@ public class Vector3D implements Serializable, Vector { return 643 * (164 * MathUtils.hash(x) + 3 * MathUtils.hash(y) + MathUtils.hash(z)); } - /** {@inheritDoc} */ + /** {@inheritDoc} + *

+ * The implementation uses specific multiplication and addition + * algorithms to preserve accuracy and reduce cancellation effects. + * It should be very accurate even for nearly orthogonal vectors. + *

+ * @see MathUtils#linearCombination(double, double, double, double, double, double) + */ public double dotProduct(final Vector v) { final Vector3D v3 = (Vector3D) v; - return x * v3.x + y * v3.y + z * v3.z; + return MathUtils.linearCombination(x, v3.x, y, v3.y, z, v3.z); } /** Compute the cross-product of the instance with another vector. - * @param v other vectorvector + * @param v other vector * @return the cross product this ^ v as a new Vector3D */ public Vector3D crossProduct(final Vector v) { final Vector3D v3 = (Vector3D) v; - - final double n1 = getNormSq(); - final double n2 = v.getNormSq(); - if ((n1 * n2) < MathUtils.SAFE_MIN) { - return ZERO; - } - - // rescale both vectors without losing precision, - // to ensure their norm are the same order of magnitude - final int deltaExp = (FastMath.getExponent(n1) - FastMath.getExponent(n2)) / 4; - final double x1 = FastMath.scalb(x, -deltaExp); - final double y1 = FastMath.scalb(y, -deltaExp); - final double z1 = FastMath.scalb(z, -deltaExp); - final double x2 = FastMath.scalb(v3.x, deltaExp); - final double y2 = FastMath.scalb(v3.y, deltaExp); - final double z2 = FastMath.scalb(v3.z, deltaExp); - - // we reduce cancellation errors by preconditioning, - // we replace v1 by v3 = v1 - rho v2 with rho chosen in order to compute - // v3 without loss of precision. See Kahan lecture - // "Computing Cross-Products and Rotations in 2- and 3-Dimensional Euclidean Spaces" - // available at http://www.cs.berkeley.edu/~wkahan/MathH110/Cross.pdf - - // compute rho as an 8 bits approximation of v1.v2 / v2.v2 - final double ratio = (x1 * x2 + y1 * y2 + z1 * z2) / FastMath.scalb(n2, 2 * deltaExp); - final double rho = FastMath.rint(256 * ratio) / 256; - - final double x3 = x1 - rho * x2; - final double y3 = y1 - rho * y2; - final double z3 = z1 - rho * z2; - - // compute cross product from v3 and v2 instead of v1 and v2 - return new Vector3D(y3 * z2 - z3 * y2, z3 * x2 - x3 * z2, x3 * y2 - y3 * x2); - + return new Vector3D(MathUtils.linearCombination(y, v3.z, -z, v3.y), + MathUtils.linearCombination(z, v3.x, -x, v3.z), + MathUtils.linearCombination(x, v3.y, -y, v3.x)); } /** {@inheritDoc} */ diff --git a/src/test/java/org/apache/commons/math/geometry/euclidean/threed/Vector3DTest.java b/src/test/java/org/apache/commons/math/geometry/euclidean/threed/Vector3DTest.java index 99b511618..61bad25e1 100644 --- a/src/test/java/org/apache/commons/math/geometry/euclidean/threed/Vector3DTest.java +++ b/src/test/java/org/apache/commons/math/geometry/euclidean/threed/Vector3DTest.java @@ -17,10 +17,9 @@ package org.apache.commons.math.geometry.euclidean.threed; -import org.apache.commons.math.geometry.euclidean.threed.Vector3D; -import org.apache.commons.math.util.FastMath; import org.apache.commons.math.exception.MathArithmeticException; - +import org.apache.commons.math.random.Well1024a; +import org.apache.commons.math.util.FastMath; import org.junit.Assert; import org.junit.Test; @@ -236,6 +235,83 @@ public class Vector3DTest { } } + @Test + public void testAccurateDotProduct() { + // the following two vectors are nearly but not exactly orthogonal + // naive dot product (i.e. computing u1.x * u2.x + u1.y * u2.y + u1.z * u2.z + // leads to a result of 0.0, instead of the correct -1.855129... + Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -7645843051051357.0 / 8589934592.0); + Vector3D u2 = new Vector3D(-5712344449280879.0 / 2097152.0, + -4550117129121957.0 / 2097152.0, + 8846951984510141.0 / 131072.0); + double sNaive = u1.getX() * u2.getX() + u1.getY() * u2.getY() + u1.getZ() * u2.getZ(); + double sAccurate = u1.dotProduct(u2); + Assert.assertEquals(0.0, sNaive, 1.0e-30); + Assert.assertEquals(-2088690039198397.0 / 1125899906842624.0, sAccurate, 1.0e-16); + } + + @Test + public void testDotProduct() { + // we compare accurate versus naive dot product implementations + // on regular vectors (i.e. not extreme cases like in the previous test) + Well1024a random = new Well1024a(553267312521321234l); + for (int i = 0; i < 10000; ++i) { + double ux = 10000 * random.nextDouble(); + double uy = 10000 * random.nextDouble(); + double uz = 10000 * random.nextDouble(); + double vx = 10000 * random.nextDouble(); + double vy = 10000 * random.nextDouble(); + double vz = 10000 * random.nextDouble(); + double sNaive = ux * vx + uy * vy + uz * vz; + double sAccurate = new Vector3D(ux, uy, uz).dotProduct(new Vector3D(vx, vy, vz)); + Assert.assertEquals(sNaive, sAccurate, 2.5e-16 * sAccurate); + } + } + + @Test + public void testAccurateCrossProduct() { + // the vectors u1 and u2 are nearly but not exactly anti-parallel + // (7.31e-16 degrees from 180 degrees) naive cross product (i.e. + // computing u1.x * u2.x + u1.y * u2.y + u1.z * u2.z + // leads to a result of [0.0009765, -0.0001220, -0.0039062], + // instead of the correct [0.0006913, -0.0001254, -0.0007909] + final Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -7645843051051357.0 / 8589934592.0); + final Vector3D u2 = new Vector3D( 1796571811118507.0 / 2147483648.0, + 7853468008299307.0 / 2147483648.0, + 2599586637357461.0 / 17179869184.0); + final Vector3D u3 = new Vector3D(12753243807587107.0 / 18446744073709551616.0, + -2313766922703915.0 / 18446744073709551616.0, + -227970081415313.0 / 288230376151711744.0); + Vector3D cNaive = new Vector3D(u1.getY() * u2.getZ() - u1.getZ() * u2.getY(), + u1.getZ() * u2.getX() - u1.getX() * u2.getZ(), + u1.getX() * u2.getY() - u1.getY() * u2.getX()); + Vector3D cAccurate = u1.crossProduct(u2); + Assert.assertTrue(u3.distance(cNaive) > 2.9 * u3.getNorm()); + Assert.assertEquals(0.0, u3.distance(cAccurate), 1.0e-30 * cAccurate.getNorm()); + } + + @Test + public void testCrossProduct() { + // we compare accurate versus naive cross product implementations + // on regular vectors (i.e. not extreme cases like in the previous test) + Well1024a random = new Well1024a(885362227452043214l); + for (int i = 0; i < 10000; ++i) { + double ux = 10000 * random.nextDouble(); + double uy = 10000 * random.nextDouble(); + double uz = 10000 * random.nextDouble(); + double vx = 10000 * random.nextDouble(); + double vy = 10000 * random.nextDouble(); + double vz = 10000 * random.nextDouble(); + Vector3D cNaive = new Vector3D(uy * vz - uz * vy, uz * vx - ux * vz, ux * vy - uy * vx); + Vector3D cAccurate = new Vector3D(ux, uy, uz).crossProduct(new Vector3D(vx, vy, vz)); + Assert.assertEquals(0.0, cAccurate.distance(cNaive), 6.0e-15 * cAccurate.getNorm()); + } + } + private void checkVector(Vector3D v, double x, double y, double z) { Assert.assertEquals(x, v.getX(), 1.0e-12); Assert.assertEquals(y, v.getY(), 1.0e-12);