From 49d94ad78ccfcea4e4c6aada466b2077895310a5 Mon Sep 17 00:00:00 2001 From: Luc Maisonobe Date: Sun, 24 Feb 2013 19:13:17 +0000 Subject: [PATCH] Added a new ExtendedFieldElement interface. This interface represents anything that is real number like. It is implemented by Decimal64, Dfp and DerivativeStructure. The purpose of this interface is to be able to set up general algorithms that apply on real numbers (maybe complex too) and use genericity. A first use case corresponds to 3D geometry objects (Vector3D and Rotation). git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1449529 13f79535-47bb-0310-9956-ffa450edef68 --- src/changes/changes.xml | 6 +- .../commons/math3/ExtendedFieldElement.java | 490 ++++++++ .../differentiation/DerivativeStructure.java | 515 +++++--- .../org/apache/commons/math3/dfp/Dfp.java | 354 +++++- .../org/apache/commons/math3/dfp/DfpDec.java | 4 +- .../{RotationDS.java => FieldRotation.java} | 667 +++++----- .../euclidean/threed/FieldVector3D.java | 891 ++++++++++++++ .../geometry/euclidean/threed/Vector3DDS.java | 1087 ----------------- .../math3/linear/SparseFieldVector.java | 18 +- .../apache/commons/math3/util/Decimal64.java | 292 ++++- .../apache/commons/math3/util/MathArrays.java | 405 +----- .../DerivativeStructureTest.java | 142 +++ .../org/apache/commons/math3/dfp/DfpTest.java | 32 +- ...onDSTest.java => FieldRotationDSTest.java} | 282 ++--- .../threed/FieldRotationDfpTest.java | 716 +++++++++++ ...or3DDSTest.java => FieldVector3DTest.java} | 379 +++--- .../commons/math3/util/MathArraysTest.java | 153 +-- .../commons/math3/util/MathUtilsTest.java | 4 +- 18 files changed, 3904 insertions(+), 2533 deletions(-) create mode 100644 src/main/java/org/apache/commons/math3/ExtendedFieldElement.java rename src/main/java/org/apache/commons/math3/geometry/euclidean/threed/{RotationDS.java => FieldRotation.java} (57%) create mode 100644 src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java delete mode 100644 src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java rename src/test/java/org/apache/commons/math3/geometry/euclidean/threed/{RotationDSTest.java => FieldRotationDSTest.java} (71%) create mode 100644 src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDfpTest.java rename src/test/java/org/apache/commons/math3/geometry/euclidean/threed/{Vector3DDSTest.java => FieldVector3DTest.java} (64%) diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 06e9e1736..b1a4824e1 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -55,10 +55,14 @@ This is a minor release: It combines bug fixes and new features. Changes to existing features were made in a backwards-compatible way such as to allow drop-in replacement of the v3.1[.1] JAR file. "> + + Added ExtendFieldElement interface to represent anything that is + real number like, implemented by both Decimal64, Dfp and DerivativeStructure. + Added partial derivatives computation for 3D vectors and rotations. - + Fixed DerivativeStructure.atan2 for special cases when both arguments are +/-0. diff --git a/src/main/java/org/apache/commons/math3/ExtendedFieldElement.java b/src/main/java/org/apache/commons/math3/ExtendedFieldElement.java new file mode 100644 index 000000000..06dcf3e29 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/ExtendedFieldElement.java @@ -0,0 +1,490 @@ +package org.apache.commons.math3; + +import org.apache.commons.math3.exception.DimensionMismatchException; + +/** + * Interface representing a real + * field. + *

+ * Classes implementing this interface will often be singletons. + *

+ * @param the type of the field elements + * @see FieldElement + * @version $Id$ + * @since 3.2 + */ +public interface ExtendedFieldElement extends FieldElement { + + /** Get the real value of the number. + * @return real value + */ + double getReal(); + + /** '+' operator. + * @param a right hand side parameter of the operator + * @return this+a + */ + T add(double a); + + /** '-' operator. + * @param a right hand side parameter of the operator + * @return this-a + */ + T subtract(double a); + + /** '×' operator. + * @param a right hand side parameter of the operator + * @return this×a + */ + T multiply(double a); + + /** '÷s;' operator. + * @param a right hand side parameter of the operator + * @return this÷s;a + */ + T divide(double a); + + /** '%' operator. + * @param a right hand side parameter of the operator + * @return this%a + */ + T remainder(double a); + + /** '%' operator. + * @param a right hand side parameter of the operator + * @return this%a + * @exception DimensionMismatchException if number of free parameters or orders are inconsistent + */ + T remainder(T a) + throws DimensionMismatchException; + + /** absolute value. + * @return abs(this) + */ + T abs(); + + /** Get the smallest whole number larger than instance. + * @return ceil(this) + */ + T ceil(); + + /** Get the largest whole number smaller than instance. + * @return floor(this) + */ + T floor(); + + /** Get the whole number that is the nearest to the instance, or the even one if x is exactly half way between two integers. + * @return a double number r such that r is an integer r - 0.5 <= this <= r + 0.5 + */ + T rint(); + + /** Get the closest long to instance value. + * @return closest long to {@link #getValue()} + */ + long round(); + + /** Compute the signum of the instance. + * The signum is -1 for negative numbers, +1 for positive numbers and 0 otherwise + * @return -1.0, -0.0, +0.0, +1.0 or NaN depending on sign of a + */ + T signum(); + + /** + * Returns the instance with the sign of the argument. + * A NaN {@code sign} argument is treated as positive. + * + * @param sign the sign for the returned value + * @return the instance with the same sign as the {@code sign} argument + */ + T copySign(double sign); + + /** + * Multiply the instance by a power of 2. + * @param n power of 2 + * @return this × 2n + */ + T scalb(int n); + + /** + * Returns the hypotenuse of a triangle with sides {@code this} and {@code y} + * - sqrt(this2 +y2)
+ * avoiding intermediate overflow or underflow. + * + *
    + *
  • If either argument is infinite, then the result is positive infinity.
    • + *
  • else, if either argument is NaN then the result is NaN.
    • + *
+ * + * @param y a value + * @return sqrt(this2 +y2) + * @exception DimensionMismatchException if number of free parameters or orders are inconsistent + */ + T hypot(T y) + throws DimensionMismatchException; + + /** {@inheritDoc} */ + T reciprocal(); + + /** Square root. + * @return square root of the instance + */ + T sqrt(); + + /** Cubic root. + * @return cubic root of the instance + */ + T cbrt(); + + /** Nth root. + * @param n order of the root + * @return nth root of the instance + */ + T rootN(int n); + + /** Power operation. + * @param p power to apply + * @return thisp + */ + T pow(double p); + + /** Integer power operation. + * @param n power to apply + * @return thisn + */ + T pow(int n); + + /** Power operation. + * @param e exponent + * @return thise + * @exception DimensionMismatchException if number of free parameters or orders are inconsistent + */ + T pow(T e) + throws DimensionMismatchException; + + /** Exponential. + * @return exponential of the instance + */ + T exp(); + + /** Exponential minus 1. + * @return exponential minus one of the instance + */ + T expm1(); + + /** Natural logarithm. + * @return logarithm of the instance + */ + T log(); + + /** Shifted natural logarithm. + * @return logarithm of one plus the instance + */ + T log1p(); + + /** Base 10 logarithm. + * @return base 10 logarithm of the instance + */ + T log10(); + + /** Cosine operation. + * @return cos(this) + */ + T cos(); + + /** Sine operation. + * @return sin(this) + */ + T sin(); + + /** Tangent operation. + * @return tan(this) + */ + T tan(); + + /** Arc cosine operation. + * @return acos(this) + */ + T acos(); + + /** Arc sine operation. + * @return asin(this) + */ + T asin(); + + /** Arc tangent operation. + * @return atan(this) + */ + T atan(); + + /** Two arguments arc tangent operation. + * @param x second argument of the arc tangent + * @return atan2(this, x) + * @exception DimensionMismatchException if number of free parameters or orders are inconsistent + */ + T atan2(T x) + throws DimensionMismatchException; + + /** Hyperbolic cosine operation. + * @return cosh(this) + */ + T cosh(); + + /** Hyperbolic sine operation. + * @return sinh(this) + */ + T sinh(); + + /** Hyperbolic tangent operation. + * @return tanh(this) + */ + T tanh(); + + /** Inverse hyperbolic cosine operation. + * @return acosh(this) + */ + T acosh(); + + /** Inverse hyperbolic sine operation. + * @return asin(this) + */ + T asinh(); + + /** Inverse hyperbolic tangent operation. + * @return atanh(this) + */ + T atanh(); + + /** + * Compute a linear combination accurately. + * This method computes the sum of the products + * ai bi to high accuracy. + * It does so by using specific multiplication and addition algorithms to + * preserve accuracy and reduce cancellation effects. + *
+ * It is based on the 2005 paper + * + * Accurate Sum and Dot Product by Takeshi Ogita, Siegfried M. Rump, + * and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a Factors. + * @param b Factors. + * @return Σi ai bi. + * @throws DimensionMismatchException if arrays dimensions don't match + * @since 3.2 + */ + T linearCombination(T[] a, T[] b) + throws DimensionMismatchException; + + /** + * Compute a linear combination accurately. + * This method computes the sum of the products + * ai bi to high accuracy. + * It does so by using specific multiplication and addition algorithms to + * preserve accuracy and reduce cancellation effects. + *
+ * It is based on the 2005 paper + * + * Accurate Sum and Dot Product by Takeshi Ogita, Siegfried M. Rump, + * and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a Factors. + * @param b Factors. + * @return Σi ai bi. + * @throws DimensionMismatchException if arrays dimensions don't match + */ + public T linearCombination(double[] a, T[] b) + throws DimensionMismatchException; + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @return a1×b1 + + * a2×b2 + * @see #linearCombination(T, T, T, T, T, T) + * @see #linearCombination(T, T, T, T, T, T, T, T) + * @since 3.2 + */ + public T linearCombination(T a1, T b1, T a2, T b2); + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @return a1×b1 + + * a2×b2 + * @see #linearCombination(double, T, double, T, double, T) + * @see #linearCombination(double, T, double, T, double, T, double, T) + * @since 3.2 + */ + public T linearCombination(double a1, T b1, double a2, T b2); + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + a3×b3 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @param a3 first factor of the third term + * @param b3 second factor of the third term + * @return a1×b1 + + * a2×b2 + a3×b3 + * @see #linearCombination(T, T, T, T) + * @see #linearCombination(T, T, T, T, T, T, T, T) + * @since 3.2 + */ + public T linearCombination(T a1, T b1, T a2, T b2, T a3, T b3); + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + a3×b3 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @param a3 first factor of the third term + * @param b3 second factor of the third term + * @return a1×b1 + + * a2×b2 + a3×b3 + * @see #linearCombination(double, T, double, T) + * @see #linearCombination(double, T, double, T, double, T, double, T) + * @since 3.2 + */ + public T linearCombination(double a1, T b1, double a2, T b2, double a3, T b3); + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + a3×b3 + + * a4×b4 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @param a3 first factor of the third term + * @param b3 second factor of the third term + * @param a4 first factor of the third term + * @param b4 second factor of the third term + * @return a1×b1 + + * a2×b2 + a3×b3 + + * a4×b4 + * @see #linearCombination(T, T, T, T) + * @see #linearCombination(T, T, T, T, T, T) + * @since 3.2 + */ + public T linearCombination(T a1, T b1, T a2, T b2, T a3, T b3, T a4, T b4); + + /** + * Compute a linear combination accurately. + *

+ * This method computes a1×b1 + + * a2×b2 + a3×b3 + + * a4×b4 + * to high accuracy. It does so by using specific multiplication and + * addition algorithms to preserve accuracy and reduce cancellation effects. + * It is based on the 2005 paper + * Accurate Sum and Dot Product by Takeshi Ogita, + * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. + *

+ *

+ * Note that the instance is only used as a prototype to get proper elements dimensions. + * Its value is not used, only the parameters values are used. + *

+ * @param a1 first factor of the first term + * @param b1 second factor of the first term + * @param a2 first factor of the second term + * @param b2 second factor of the second term + * @param a3 first factor of the third term + * @param b3 second factor of the third term + * @param a4 first factor of the third term + * @param b4 second factor of the third term + * @return a1×b1 + + * a2×b2 + a3×b3 + + * a4×b4 + * @see #linearCombination(double, T, double, T) + * @see #linearCombination(double, T, double, T, double, T) + * @since 3.2 + */ + public T linearCombination(double a1, T b1, double a2, T b2, double a3, T b3, double a4, T b4); + +} diff --git a/src/main/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructure.java b/src/main/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructure.java index 2c9d1fa3f..1bd6bfd58 100644 --- a/src/main/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructure.java +++ b/src/main/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructure.java @@ -18,11 +18,14 @@ package org.apache.commons.math3.analysis.differentiation; import java.io.Serializable; +import org.apache.commons.math3.ExtendedFieldElement; import org.apache.commons.math3.Field; import org.apache.commons.math3.FieldElement; import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.NumberIsTooLargeException; import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.MathUtils; /** Class representing both the value and the differentials of a function. *

This class is the workhorse of the differentiation package.

@@ -55,7 +58,7 @@ import org.apache.commons.math3.util.FastMath; * @version $Id$ * @since 3.1 */ -public class DerivativeStructure implements FieldElement, Serializable { +public class DerivativeStructure implements ExtendedFieldElement, Serializable { /** Serializable UID. */ private static final long serialVersionUID = 20120730L; @@ -223,6 +226,11 @@ public class DerivativeStructure implements FieldElement, S return compiler.getOrder(); } + /***/ + public double getReal() { + return data[0]; + } + /** Get the value part of the derivative structure. * @return value part of the derivative structure * @see #getPartialDerivative(int...) @@ -254,21 +262,14 @@ public class DerivativeStructure implements FieldElement, S return data.clone(); } - /** '+' operator. - * @param a right hand side parameter of the operator - * @return this+a - */ + /** {@inheritDoc} */ public DerivativeStructure add(final double a) { final DerivativeStructure ds = new DerivativeStructure(this); ds.data[0] += a; return ds; } - /** '+' operator. - * @param a right hand side parameter of the operator - * @return this+a - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure add(final DerivativeStructure a) throws DimensionMismatchException { compiler.checkCompatibility(a.compiler); @@ -277,19 +278,12 @@ public class DerivativeStructure implements FieldElement, S return ds; } - /** '-' operator. - * @param a right hand side parameter of the operator - * @return this-a - */ + /** {@inheritDoc} */ public DerivativeStructure subtract(final double a) { return add(-a); } - /** '-' operator. - * @param a right hand side parameter of the operator - * @return this-a - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure subtract(final DerivativeStructure a) throws DimensionMismatchException { compiler.checkCompatibility(a.compiler); @@ -303,10 +297,7 @@ public class DerivativeStructure implements FieldElement, S return multiply((double) n); } - /** '×' operator. - * @param a right hand side parameter of the operator - * @return this×a - */ + /** {@inheritDoc} */ public DerivativeStructure multiply(final double a) { final DerivativeStructure ds = new DerivativeStructure(this); for (int i = 0; i < ds.data.length; ++i) { @@ -315,11 +306,7 @@ public class DerivativeStructure implements FieldElement, S return ds; } - /** '×' operator. - * @param a right hand side parameter of the operator - * @return this×a - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure multiply(final DerivativeStructure a) throws DimensionMismatchException { compiler.checkCompatibility(a.compiler); @@ -328,10 +315,7 @@ public class DerivativeStructure implements FieldElement, S return result; } - /** '÷s;' operator. - * @param a right hand side parameter of the operator - * @return this÷s;a - */ + /** {@inheritDoc} */ public DerivativeStructure divide(final double a) { final DerivativeStructure ds = new DerivativeStructure(this); for (int i = 0; i < ds.data.length; ++i) { @@ -340,11 +324,7 @@ public class DerivativeStructure implements FieldElement, S return ds; } - /** '÷s;' operator. - * @param a right hand side parameter of the operator - * @return this÷s;a - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure divide(final DerivativeStructure a) throws DimensionMismatchException { compiler.checkCompatibility(a.compiler); @@ -353,21 +333,14 @@ public class DerivativeStructure implements FieldElement, S return result; } - /** '%' operator. - * @param a right hand side parameter of the operator - * @return this%a - */ + /** {@inheritDoc} */ public DerivativeStructure remainder(final double a) { final DerivativeStructure ds = new DerivativeStructure(this); ds.data[0] = ds.data[0] % a; return ds; } - /** '%' operator. - * @param a right hand side parameter of the operator - * @return this%a - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure remainder(final DerivativeStructure a) throws DimensionMismatchException { compiler.checkCompatibility(a.compiler); @@ -376,9 +349,7 @@ public class DerivativeStructure implements FieldElement, S return result; } - /** unary '-' operator. - * @return -this - */ + /** {@inheritDoc} */ public DerivativeStructure negate() { final DerivativeStructure ds = new DerivativeStructure(compiler); for (int i = 0; i < ds.data.length; ++i) { @@ -387,9 +358,7 @@ public class DerivativeStructure implements FieldElement, S return ds; } - /** absolute value. - * @return abs(this) - */ + /** {@inheritDoc} */ public DerivativeStructure abs() { if (Double.doubleToLongBits(data[0]) < 0) { // we use the bits representation to also handle -0.0 @@ -399,57 +368,40 @@ public class DerivativeStructure implements FieldElement, S } } - /** Get the smallest whole number larger than instance. - * @return ceil(this) - */ + /** {@inheritDoc} */ public DerivativeStructure ceil() { return new DerivativeStructure(compiler.getFreeParameters(), compiler.getOrder(), FastMath.ceil(data[0])); } - /** Get the largest whole number smaller than instance. - * @return floor(this) - */ + /** {@inheritDoc} */ public DerivativeStructure floor() { return new DerivativeStructure(compiler.getFreeParameters(), compiler.getOrder(), FastMath.floor(data[0])); } - /** Get the whole number that is the nearest to the instance, or the even one if x is exactly half way between two integers. - * @return a double number r such that r is an integer r - 0.5 <= this <= r + 0.5 - */ + /** {@inheritDoc} */ public DerivativeStructure rint() { return new DerivativeStructure(compiler.getFreeParameters(), compiler.getOrder(), FastMath.rint(data[0])); } - /** Get the closest long to instance value. - * @return closest long to {@link #getValue()} - */ + /** {@inheritDoc} */ public long round() { return FastMath.round(data[0]); } - /** Compute the signum of the instance. - * The signum is -1 for negative numbers, +1 for positive numbers and 0 otherwise - * @return -1.0, -0.0, +0.0, +1.0 or NaN depending on sign of a - */ + /** {@inheritDoc} */ public DerivativeStructure signum() { return new DerivativeStructure(compiler.getFreeParameters(), compiler.getOrder(), FastMath.signum(data[0])); } - /** - * Returns the instance with the sign of the argument. - * A NaN {@code sign} argument is treated as positive. - * - * @param sign the sign for the returned value - * @return the instance with the same sign as the {@code sign} argument - */ + /** {@inheritDoc} */ public DerivativeStructure copySign(final double sign){ long m = Double.doubleToLongBits(data[0]); long s = Double.doubleToLongBits(sign); @@ -471,11 +423,7 @@ public class DerivativeStructure implements FieldElement, S return FastMath.getExponent(data[0]); } - /** - * Multiply the instance by a power of 2. - * @param n power of 2 - * @return this × 2n - */ + /** {@inheritDoc} */ public DerivativeStructure scalb(final int n) { final DerivativeStructure ds = new DerivativeStructure(compiler); for (int i = 0; i < ds.data.length; ++i) { @@ -484,6 +432,51 @@ public class DerivativeStructure implements FieldElement, S return ds; } + /** {@inheritDoc} */ + public DerivativeStructure hypot(final DerivativeStructure y) + throws DimensionMismatchException { + + compiler.checkCompatibility(y.compiler); + + if (Double.isInfinite(data[0]) || Double.isInfinite(y.data[0])) { + return new DerivativeStructure(compiler.getFreeParameters(), + compiler.getFreeParameters(), + Double.POSITIVE_INFINITY); + } else if (Double.isNaN(data[0]) || Double.isNaN(y.data[0])) { + return new DerivativeStructure(compiler.getFreeParameters(), + compiler.getFreeParameters(), + Double.NaN); + } else { + + final int expX = getExponent(); + final int expY = y.getExponent(); + if (expX > expY + 27) { + // y is neglectible with respect to x + return abs(); + } else if (expY > expX + 27) { + // x is neglectible with respect to y + return y.abs(); + } else { + + // find an intermediate scale to avoid both overflow and underflow + final int middleExp = (expX + expY) / 2; + + // scale parameters without losing precision + final DerivativeStructure scaledX = scalb(-middleExp); + final DerivativeStructure scaledY = y.scalb(-middleExp); + + // compute scaled hypotenuse + final DerivativeStructure scaledH = + scaledX.multiply(scaledX).add(scaledY.multiply(scaledY)).sqrt(); + + // remove scaling + return scaledH.scalb(middleExp); + + } + + } + } + /** * Returns the hypotenuse of a triangle with sides {@code x} and {@code y} * - sqrt(x2 +y2)
@@ -501,46 +494,7 @@ public class DerivativeStructure implements FieldElement, S */ public static DerivativeStructure hypot(final DerivativeStructure x, final DerivativeStructure y) throws DimensionMismatchException { - - x.compiler.checkCompatibility(y.compiler); - - if (Double.isInfinite(x.data[0]) || Double.isInfinite(y.data[0])) { - return new DerivativeStructure(x.compiler.getFreeParameters(), - x.compiler.getFreeParameters(), - Double.POSITIVE_INFINITY); - } else if (Double.isNaN(x.data[0]) || Double.isNaN(y.data[0])) { - return new DerivativeStructure(x.compiler.getFreeParameters(), - x.compiler.getFreeParameters(), - Double.NaN); - } else { - - final int expX = x.getExponent(); - final int expY = y.getExponent(); - if (expX > expY + 27) { - // y is neglectible with respect to x - return x.abs(); - } else if (expY > expX + 27) { - // x is neglectible with respect to y - return y.abs(); - } else { - - // find an intermediate scale to avoid both overflow and underflow - final int middleExp = (expX + expY) / 2; - - // scale parameters without losing precision - final DerivativeStructure scaledX = x.scalb(-middleExp); - final DerivativeStructure scaledY = y.scalb(-middleExp); - - // compute scaled hypotenuse - final DerivativeStructure scaledH = - scaledX.multiply(scaledX).add(scaledY.multiply(scaledY)).sqrt(); - - // remove scaling - return scaledH.scalb(middleExp); - - } - - } + return x.hypot(y); } /** Compute composition of the instance by a univariate function. @@ -567,24 +521,17 @@ public class DerivativeStructure implements FieldElement, S return result; } - /** Square root. - * @return square root of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure sqrt() { return rootN(2); } - /** Cubic root. - * @return cubic root of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure cbrt() { return rootN(3); } - /** Nth root. - * @param n order of the root - * @return nth root of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure rootN(final int n) { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.rootN(data, 0, n, result.data, 0); @@ -613,31 +560,21 @@ public class DerivativeStructure implements FieldElement, S }; } - /** Power operation. - * @param p power to apply - * @return thisp - */ + /** {@inheritDoc} */ public DerivativeStructure pow(final double p) { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.pow(data, 0, p, result.data, 0); return result; } - /** Integer power operation. - * @param n power to apply - * @return thisn - */ + /** {@inheritDoc} */ public DerivativeStructure pow(final int n) { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.pow(data, 0, n, result.data, 0); return result; } - /** Power operation. - * @param e exponent - * @return thise - * @exception DimensionMismatchException if number of free parameters or orders are inconsistent - */ + /** {@inheritDoc} */ public DerivativeStructure pow(final DerivativeStructure e) throws DimensionMismatchException { compiler.checkCompatibility(e.compiler); @@ -646,105 +583,92 @@ public class DerivativeStructure implements FieldElement, S return result; } - /** Exponential. - * @return exponential of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure exp() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.exp(data, 0, result.data, 0); return result; } - /** Exponential minus 1. - * @return exponential minus one of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure expm1() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.expm1(data, 0, result.data, 0); return result; } - /** Natural logarithm. - * @return logarithm of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure log() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.log(data, 0, result.data, 0); return result; } - /** Shifted natural logarithm. - * @return logarithm of one plus the instance - */ + /** {@inheritDoc} */ public DerivativeStructure log1p() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.log1p(data, 0, result.data, 0); return result; } - /** Base 10 logarithm. - * @return base 10 logarithm of the instance - */ + /** {@inheritDoc} */ public DerivativeStructure log10() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.log10(data, 0, result.data, 0); return result; } - /** Cosine operation. - * @return cos(this) - */ + /** {@inheritDoc} */ public DerivativeStructure cos() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.cos(data, 0, result.data, 0); return result; } - /** Sine operation. - * @return sin(this) - */ + /** {@inheritDoc} */ public DerivativeStructure sin() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.sin(data, 0, result.data, 0); return result; } - /** Tangent operation. - * @return tan(this) - */ + /** {@inheritDoc} */ public DerivativeStructure tan() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.tan(data, 0, result.data, 0); return result; } - /** Arc cosine operation. - * @return acos(this) - */ + /** {@inheritDoc} */ public DerivativeStructure acos() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.acos(data, 0, result.data, 0); return result; } - /** Arc sine operation. - * @return asin(this) - */ + /** {@inheritDoc} */ public DerivativeStructure asin() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.asin(data, 0, result.data, 0); return result; } - /** Arc tangent operation. - * @return atan(this) - */ + /** {@inheritDoc} */ public DerivativeStructure atan() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.atan(data, 0, result.data, 0); return result; } + /** {@inheritDoc} */ + public DerivativeStructure atan2(final DerivativeStructure x) + throws DimensionMismatchException { + compiler.checkCompatibility(x.compiler); + final DerivativeStructure result = new DerivativeStructure(compiler); + compiler.atan2(data, 0, x.data, 0, result.data, 0); + return result; + } + /** Two arguments arc tangent operation. * @param y first argument of the arc tangent * @param x second argument of the arc tangent @@ -753,60 +677,45 @@ public class DerivativeStructure implements FieldElement, S */ public static DerivativeStructure atan2(final DerivativeStructure y, final DerivativeStructure x) throws DimensionMismatchException { - y.compiler.checkCompatibility(x.compiler); - final DerivativeStructure result = new DerivativeStructure(y.compiler); - y.compiler.atan2(y.data, 0, x.data, 0, result.data, 0); - return result; + return y.atan2(x); } - /** Hyperbolic cosine operation. - * @return cosh(this) - */ + /** {@inheritDoc} */ public DerivativeStructure cosh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.cosh(data, 0, result.data, 0); return result; } - /** Hyperbolic sine operation. - * @return sinh(this) - */ + /** {@inheritDoc} */ public DerivativeStructure sinh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.sinh(data, 0, result.data, 0); return result; } - /** Hyperbolic tangent operation. - * @return tanh(this) - */ + /** {@inheritDoc} */ public DerivativeStructure tanh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.tanh(data, 0, result.data, 0); return result; } - /** Inverse hyperbolic cosine operation. - * @return acosh(this) - */ + /** {@inheritDoc} */ public DerivativeStructure acosh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.acosh(data, 0, result.data, 0); return result; } - /** Inverse hyperbolic sine operation. - * @return asin(this) - */ + /** {@inheritDoc} */ public DerivativeStructure asinh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.asinh(data, 0, result.data, 0); return result; } - /** Inverse hyperbolic tangent operation. - * @return atanh(this) - */ + /** {@inheritDoc} */ public DerivativeStructure atanh() { final DerivativeStructure result = new DerivativeStructure(compiler); compiler.atanh(data, 0, result.data, 0); @@ -843,6 +752,216 @@ public class DerivativeStructure implements FieldElement, S return compiler.taylor(data, 0, delta); } + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final DerivativeStructure[] a, final DerivativeStructure[] b) + throws DimensionMismatchException { + + // compute an accurate value, taking care of cancellations + final double[] aDouble = new double[a.length]; + for (int i = 0; i < a.length; ++i) { + aDouble[i] = a[i].getValue(); + } + final double[] bDouble = new double[b.length]; + for (int i = 0; i < b.length; ++i) { + bDouble[i] = b[i].getValue(); + } + final double accurateValue = MathArrays.linearCombination(aDouble, bDouble); + + // compute a simple value, with all partial derivatives + DerivativeStructure simpleValue = a[0].getField().getZero(); + for (int i = 0; i < a.length; ++i) { + simpleValue = simpleValue.add(a[i].multiply(b[i])); + } + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final double[] a, final DerivativeStructure[] b) + throws DimensionMismatchException { + + // compute an accurate value, taking care of cancellations + final double[] bDouble = new double[b.length]; + for (int i = 0; i < b.length; ++i) { + bDouble[i] = b[i].getValue(); + } + final double accurateValue = MathArrays.linearCombination(a, bDouble); + + // compute a simple value, with all partial derivatives + DerivativeStructure simpleValue = b[0].getField().getZero(); + for (int i = 0; i < a.length; ++i) { + simpleValue = simpleValue.add(b[i].multiply(a[i])); + } + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, + final DerivativeStructure a2, final DerivativeStructure b2) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), + a2.getValue(), b2.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, + final double a2, final DerivativeStructure b2) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), + a2, b2.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, + final DerivativeStructure a2, final DerivativeStructure b2, + final DerivativeStructure a3, final DerivativeStructure b3) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), + a2.getValue(), b2.getValue(), + a3.getValue(), b3.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, + final double a2, final DerivativeStructure b2, + final double a3, final DerivativeStructure b3) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), + a2, b2.getValue(), + a3, b3.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, + final DerivativeStructure a2, final DerivativeStructure b2, + final DerivativeStructure a3, final DerivativeStructure b3, + final DerivativeStructure a4, final DerivativeStructure b4) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), + a2.getValue(), b2.getValue(), + a3.getValue(), b3.getValue(), + a4.getValue(), b4.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)).add(a4.multiply(b4)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** {@inheritDoc} */ + public DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, + final double a2, final DerivativeStructure b2, + final double a3, final DerivativeStructure b3, + final double a4, final DerivativeStructure b4) { + + // compute an accurate value, taking care of cancellations + final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), + a2, b2.getValue(), + a3, b3.getValue(), + a4, b4.getValue()); + + // compute a simple value, with all partial derivatives + final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)).add(b4.multiply(a4)); + + // create a result with accurate value and all derivatives (not necessarily as accurate as the value) + final double[] data = simpleValue.getAllDerivatives(); + data[0] = accurateValue; + return new DerivativeStructure(getFreeParameters(), getOrder(), data); + + } + + /** + * Test for the equality of two derivative structures. + *

+ * Derivative structures are considered equal if they have the same number + * of free parameters, the same derivation order, and the same derivatives. + *

+ * @param other Object to test for equality to this + * @return true if two derivative structures are equal + * @since 3.2 + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof DerivativeStructure) { + final DerivativeStructure rhs = (DerivativeStructure)other; + return (getFreeParameters() == rhs.getFreeParameters()) && + (getOrder() == rhs.getOrder()) && + MathArrays.equals(data, rhs.data); + } + + return false; + + } + + /** + * Get a hashCode for the derivative structure. + * @return a hash code value for this object + * @since 3.2 + */ + @Override + public int hashCode() { + return 227 + 229 * getFreeParameters() + 233 * getOrder() + 239 * MathUtils.hash(data); + } + /** * Replace the instance with a data transfer object for serialization. * @return data transfer object that will be serialized diff --git a/src/main/java/org/apache/commons/math3/dfp/Dfp.java b/src/main/java/org/apache/commons/math3/dfp/Dfp.java index 3a9198271..1ac3f4103 100644 --- a/src/main/java/org/apache/commons/math3/dfp/Dfp.java +++ b/src/main/java/org/apache/commons/math3/dfp/Dfp.java @@ -19,7 +19,9 @@ package org.apache.commons.math3.dfp; import java.util.Arrays; -import org.apache.commons.math3.FieldElement; +import org.apache.commons.math3.ExtendedFieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.util.FastMath; /** * Decimal floating point library for Java @@ -93,7 +95,7 @@ import org.apache.commons.math3.FieldElement; * @version $Id$ * @since 2.2 */ -public class Dfp implements FieldElement { +public class Dfp implements ExtendedFieldElement { /** The radix, or base of this system. Set to 10000 */ public static final int RADIX = 10000; @@ -1178,7 +1180,7 @@ public class Dfp implements FieldElement { /** Get the exponent of the greatest power of 10 that is less than or equal to abs(this). * @return integer base 10 logarithm */ - public int log10() { + public int intLog10() { if (mant[mant.length-1] > 1000) { return exp * 4 - 1; } @@ -2429,7 +2431,7 @@ public class Dfp implements FieldElement { /* Find the exponent, first estimate by integer log10, then adjust. Should be faster than doing a natural logarithm. */ - int exponent = (int)(y.log10() * 3.32); + int exponent = (int)(y.intLog10() * 3.32); if (exponent < 0) { exponent--; } @@ -2503,4 +2505,348 @@ public class Dfp implements FieldElement { return split; } + /** {@inheritDoc} + * @since 3.2 + */ + public double getReal() { + return toDouble(); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp add(final double a) { + return add(newInstance(a)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp subtract(final double a) { + return subtract(newInstance(a)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp multiply(final double a) { + return multiply(newInstance(a)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp divide(final double a) { + return divide(newInstance(a)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp remainder(final double a) { + return remainder(newInstance(a)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public long round() { + return FastMath.round(toDouble()); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp signum() { + if (isNaN() || isZero()) { + return this; + } else { + return newInstance(sign > 0 ? +1 : -1); + } + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp copySign(final double sign) { + long s = Double.doubleToLongBits(sign); + if ((sign >= 0 && s >= 0) || (sign < 0 && s < 0)) { // Sign is currently OK + return this; + } + return negate(); // flip sign + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp scalb(final int n) { + return multiply(DfpMath.pow(getTwo(), n)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp hypot(final Dfp y) { + return multiply(this).add(y.multiply(y)).sqrt(); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp cbrt() { + return rootN(3); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp rootN(final int n) { + return DfpMath.pow(this, getOne().divide(n)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp pow(final double p) { + return DfpMath.pow(this, newInstance(p)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp pow(final int n) { + return DfpMath.pow(this, n); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp pow(final Dfp e) { + return DfpMath.pow(this, e); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp exp() { + return DfpMath.exp(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp expm1() { + return DfpMath.exp(this).subtract(getOne()); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp log() { + return DfpMath.log(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp log1p() { + return DfpMath.log(this.add(getOne())); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp log10() { + return DfpMath.log(this).divide(DfpMath.log(newInstance(10))); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp cos() { + return DfpMath.cos(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp sin() { + return DfpMath.sin(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp tan() { + return DfpMath.tan(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp acos() { + return DfpMath.acos(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp asin() { + return DfpMath.asin(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp atan() { + return DfpMath.atan(this); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp atan2(final Dfp x) + throws DimensionMismatchException { + + // compute r = sqrt(x^2+y^2) + final Dfp r = x.multiply(x).add(multiply(this)).sqrt(); + + if (x.sign >= 0) { + + // compute atan2(y, x) = 2 atan(y / (r + x)) + return getTwo().multiply(divide(r.add(x)).atan()); + + } else { + + // compute atan2(y, x) = +/- pi - 2 atan(y / (r - x)) + final Dfp tmp = getTwo().multiply(divide(r.subtract(x)).atan()); + final Dfp pmPi = newInstance((tmp.sign <= 0) ? -FastMath.PI : FastMath.PI); + return pmPi.subtract(tmp); + + } + + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp cosh() { + return DfpMath.exp(this).add(DfpMath.exp(negate())).divide(2); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp sinh() { + return DfpMath.exp(this).subtract(DfpMath.exp(negate())).divide(2); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp tanh() { + final Dfp ePlus = DfpMath.exp(this); + final Dfp eMinus = DfpMath.exp(negate()); + return ePlus.add(eMinus).divide(ePlus.subtract(eMinus)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp acosh() { + return multiply(this).subtract(getOne()).sqrt().add(this).log(); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp asinh() { + return multiply(this).add(getOne()).sqrt().add(this).log(); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp atanh() { + return getOne().add(this).divide(getOne().subtract(this)).log().divide(2); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final Dfp[] a, final Dfp[] b) + throws DimensionMismatchException { + if (a.length != b.length) { + throw new DimensionMismatchException(a.length, b.length); + } + Dfp r = getZero(); + for (int i = 0; i < a.length; ++i) { + r = r.add(a[i].multiply(b[i])); + } + return r; + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final double[] a, final Dfp[] b) + throws DimensionMismatchException { + if (a.length != b.length) { + throw new DimensionMismatchException(a.length, b.length); + } + Dfp r = getZero(); + for (int i = 0; i < a.length; ++i) { + r = r.add(b[i].multiply(a[i])); + } + return r; + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final Dfp a1, final Dfp b1, final Dfp a2, final Dfp b2) { + return a1.multiply(b1).add(a2.multiply(b2)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final double a1, final Dfp b1, final double a2, final Dfp b2) { + return b1.multiply(a1).add(b2.multiply(a2)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final Dfp a1, final Dfp b1, + final Dfp a2, final Dfp b2, + final Dfp a3, final Dfp b3) { + return a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final double a1, final Dfp b1, + final double a2, final Dfp b2, + final double a3, final Dfp b3) { + return b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final Dfp a1, final Dfp b1, final Dfp a2, final Dfp b2, + final Dfp a3, final Dfp b3, final Dfp a4, final Dfp b4) { + return a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)).add(a4.multiply(b4)); + } + + /** {@inheritDoc} + * @since 3.2 + */ + public Dfp linearCombination(final double a1, final Dfp b1, final double a2, final Dfp b2, + final double a3, final Dfp b3, final double a4, final Dfp b4) { + return b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)).add(b4.multiply(a4)); + } + } diff --git a/src/main/java/org/apache/commons/math3/dfp/DfpDec.java b/src/main/java/org/apache/commons/math3/dfp/DfpDec.java index fb4656436..4c20a6498 100644 --- a/src/main/java/org/apache/commons/math3/dfp/DfpDec.java +++ b/src/main/java/org/apache/commons/math3/dfp/DfpDec.java @@ -320,7 +320,7 @@ public class DfpDec extends Dfp { } if (up) { - inc = power10(log10() - getDecimalDigits() + 1); + inc = power10(intLog10() - getDecimalDigits() + 1); inc = copysign(inc, this); if (this.equals(getZero())) { @@ -333,7 +333,7 @@ public class DfpDec extends Dfp { result = add(inc); } } else { - inc = power10(log10()); + inc = power10(intLog10()); inc = copysign(inc, this); if (this.equals(inc)) { diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java similarity index 57% rename from src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java rename to src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java index 2ba8a4def..0a23ca230 100644 --- a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java @@ -19,8 +19,8 @@ package org.apache.commons.math3.geometry.euclidean.threed; import java.io.Serializable; +import org.apache.commons.math3.ExtendedFieldElement; import org.apache.commons.math3.Field; -import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; import org.apache.commons.math3.exception.MathArithmeticException; import org.apache.commons.math3.exception.MathIllegalArgumentException; import org.apache.commons.math3.exception.util.LocalizedFormats; @@ -28,31 +28,32 @@ import org.apache.commons.math3.util.FastMath; import org.apache.commons.math3.util.MathArrays; /** - * This class is a re-implementation of {@link Rotation} using {@link DerivativeStructure}. + * This class is a re-implementation of {@link Rotation} using {@link ExtendedFieldElement}. *

Instance of this class are guaranteed to be immutable.

* + * @param the type of the field elements * @version $Id$ * @see Vector3DDSDS * @see RotationOrder * @since 3.2 */ -public class RotationDS implements Serializable { +public class FieldRotation> implements Serializable { /** Serializable version identifier */ - private static final long serialVersionUID = 20130215l; + private static final long serialVersionUID = 20130224l; /** Scalar coordinate of the quaternion. */ - private final DerivativeStructure q0; + private final T q0; /** First coordinate of the vectorial part of the quaternion. */ - private final DerivativeStructure q1; + private final T q1; /** Second coordinate of the vectorial part of the quaternion. */ - private final DerivativeStructure q2; + private final T q2; /** Third coordinate of the vectorial part of the quaternion. */ - private final DerivativeStructure q3; + private final T q3; /** Build a rotation from the quaternion coordinates. *

A rotation can be built from a normalized quaternion, @@ -72,13 +73,11 @@ public class RotationDS implements Serializable { * not to be normalized, a normalization preprocessing step is performed * before using them */ - public RotationDS(final DerivativeStructure q0, final DerivativeStructure q1, - final DerivativeStructure q2, final DerivativeStructure q3, - final boolean needsNormalization) { + public FieldRotation(final T q0, final T q1, final T q2, final T q3, final boolean needsNormalization) { if (needsNormalization) { // normalization preprocessing - final DerivativeStructure inv = + final T inv = q0.multiply(q0).add(q1.multiply(q1)).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().reciprocal(); this.q0 = inv.multiply(q0); this.q1 = inv.multiply(q1); @@ -98,7 +97,7 @@ public class RotationDS implements Serializable { * the effect of the rotation on vectors around the axis. That means * that if (i, j, k) is a direct frame and if we first provide +k as * the axis and π/2 as the angle to this constructor, and then - * {@link #applyTo(Vector3DDS) apply} the instance to +i, we will get + * {@link #applyTo(FieldVector3D) apply} the instance to +i, we will get * +j.

*

Another way to represent our convention is to say that a rotation * of angle θ about the unit vector (x, y, z) is the same as the @@ -114,16 +113,16 @@ public class RotationDS implements Serializable { * @param angle rotation angle. * @exception MathIllegalArgumentException if the axis norm is zero */ - public RotationDS(final Vector3DDS axis, final DerivativeStructure angle) + public FieldRotation(final FieldVector3D axis, final T angle) throws MathIllegalArgumentException { - final DerivativeStructure norm = axis.getNorm(); - if (norm.getValue() == 0) { + final T norm = axis.getNorm(); + if (norm.getReal() == 0) { throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); } - final DerivativeStructure halfAngle = angle.multiply(-0.5); - final DerivativeStructure coeff = halfAngle.sin().divide(norm); + final T halfAngle = angle.multiply(-0.5); + final T coeff = halfAngle.sin().divide(norm); q0 = halfAngle.cos(); q1 = coeff.multiply(axis.getX()); @@ -162,7 +161,7 @@ public class RotationDS implements Serializable { * orthogonal matrix is negative */ - public RotationDS(final DerivativeStructure[][] m, final double threshold) + public FieldRotation(final T[][] m, final double threshold) throws NotARotationMatrixException { // dimension check @@ -174,21 +173,21 @@ public class RotationDS implements Serializable { } // compute a "close" orthogonal matrix - final DerivativeStructure[][] ort = orthogonalizeMatrix(m, threshold); + final T[][] ort = orthogonalizeMatrix(m, threshold); // check the sign of the determinant - final DerivativeStructure d0 = ort[1][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[1][2])); - final DerivativeStructure d1 = ort[0][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[0][2])); - final DerivativeStructure d2 = ort[0][1].multiply(ort[1][2]).subtract(ort[1][1].multiply(ort[0][2])); - final DerivativeStructure det = + final T d0 = ort[1][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[1][2])); + final T d1 = ort[0][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[0][2])); + final T d2 = ort[0][1].multiply(ort[1][2]).subtract(ort[1][1].multiply(ort[0][2])); + final T det = ort[0][0].multiply(d0).subtract(ort[1][0].multiply(d1)).add(ort[2][0].multiply(d2)); - if (det.getValue() < 0.0) { + if (det.getReal() < 0.0) { throw new NotARotationMatrixException( LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, det); } - final DerivativeStructure[] quat = mat2quat(ort); + final T[] quat = mat2quat(ort); q0 = quat[0]; q1 = quat[1]; q2 = quat[2]; @@ -215,41 +214,34 @@ public class RotationDS implements Serializable { * @exception MathArithmeticException 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 RotationDS(Vector3DDS u1, Vector3DDS u2, Vector3DDS v1, Vector3DDS v2) + public FieldRotation(FieldVector3D u1, FieldVector3D u2, FieldVector3D v1, FieldVector3D v2) throws MathArithmeticException { // build orthonormalized base from u1, u2 // this fails when vectors are null or colinear, which is forbidden to define a rotation - final Vector3DDS u3 = u1.crossProduct(u2).normalize(); + final FieldVector3D 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 Vector3DDS v3 = v1.crossProduct(v2).normalize(); + final FieldVector3D 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 DerivativeStructure[][] m = new DerivativeStructure[][] { - { - 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()) - } - }; + final T[][] array = MathArrays.buildArray(u1.getX().getField(), 3, 3); + array[0][0] = u1.getX().multiply(v1.getX()).add(u2.getX().multiply(v2.getX())).add(u3.getX().multiply(v3.getX())); + array[0][1] = u1.getY().multiply(v1.getX()).add(u2.getY().multiply(v2.getX())).add(u3.getY().multiply(v3.getX())); + array[0][2] = u1.getZ().multiply(v1.getX()).add(u2.getZ().multiply(v2.getX())).add(u3.getZ().multiply(v3.getX())); + array[1][0] = u1.getX().multiply(v1.getY()).add(u2.getX().multiply(v2.getY())).add(u3.getX().multiply(v3.getY())); + array[1][1] = u1.getY().multiply(v1.getY()).add(u2.getY().multiply(v2.getY())).add(u3.getY().multiply(v3.getY())); + array[1][2] = u1.getZ().multiply(v1.getY()).add(u2.getZ().multiply(v2.getY())).add(u3.getZ().multiply(v3.getY())); + array[2][0] = u1.getX().multiply(v1.getZ()).add(u2.getX().multiply(v2.getZ())).add(u3.getX().multiply(v3.getZ())); + array[2][1] = u1.getY().multiply(v1.getZ()).add(u2.getY().multiply(v2.getZ())).add(u3.getY().multiply(v3.getZ())); + array[2][2] = u1.getZ().multiply(v1.getZ()).add(u2.getZ().multiply(v2.getZ())).add(u3.getZ().multiply(v3.getZ())); - DerivativeStructure[] quat = mat2quat(m); + T[] quat = mat2quat(array); q0 = quat[0]; q1 = quat[1]; q2 = quat[2]; @@ -270,19 +262,19 @@ public class RotationDS implements Serializable { * @param v desired image of u by the rotation * @exception MathArithmeticException if the norm of one of the vectors is zero */ - public RotationDS(final Vector3DDS u, final Vector3DDS v) throws MathArithmeticException { + public FieldRotation(final FieldVector3D u, final FieldVector3D v) throws MathArithmeticException { - final DerivativeStructure normProduct = u.getNorm().multiply(v.getNorm()); - if (normProduct.getValue() == 0) { + final T normProduct = u.getNorm().multiply(v.getNorm()); + if (normProduct.getReal() == 0) { throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); } - final DerivativeStructure dot = u.dotProduct(v); + final T dot = u.dotProduct(v); - if (dot.getValue() < ((2.0e-15 - 1.0) * normProduct.getValue())) { + if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) { // special case u = -v: we select a PI angle rotation around // an arbitrary vector orthogonal to u - final Vector3DDS w = u.orthogonal(); + final FieldVector3D w = u.orthogonal(); q0 = normProduct.getField().getZero(); q1 = w.getX().negate(); q2 = w.getY().negate(); @@ -291,8 +283,8 @@ public class RotationDS implements Serializable { // general case: (u, v) defines a plane, we select // the shortest possible rotation: axis orthogonal to this plane q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt(); - final DerivativeStructure coeff = q0.multiply(normProduct).multiply(2.0).reciprocal(); - final Vector3DDS q = v.crossProduct(u); + final T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal(); + final FieldVector3D q = v.crossProduct(u); q1 = coeff.multiply(q.getX()); q2 = coeff.multiply(q.getY()); q3 = coeff.multiply(q.getZ()); @@ -319,26 +311,12 @@ public class RotationDS implements Serializable { * @param alpha2 angle of the second elementary rotation * @param alpha3 angle of the third elementary rotation */ - public RotationDS(final RotationOrder order, final DerivativeStructure alpha1, - final DerivativeStructure alpha2, final DerivativeStructure alpha3) { - final int p = alpha1.getFreeParameters(); - final int o = alpha1.getOrder(); - final RotationDS r1 = - new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA1().getX()), - new DerivativeStructure(p, o, order.getA1().getY()), - new DerivativeStructure(p, o, order.getA1().getZ())), - alpha1); - final RotationDS r2 = - new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA2().getX()), - new DerivativeStructure(p, o, order.getA2().getY()), - new DerivativeStructure(p, o, order.getA2().getZ())), - alpha2); - final RotationDS r3 = - new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA3().getX()), - new DerivativeStructure(p, o, order.getA3().getY()), - new DerivativeStructure(p, o, order.getA3().getZ())), - alpha3); - final RotationDS composed = r1.applyTo(r2.applyTo(r3)); + public FieldRotation(final RotationOrder order, final T alpha1, final T alpha2, final T alpha3) { + final T one = alpha1.getField().getOne(); + final FieldRotation r1 = new FieldRotation(new FieldVector3D(one, order.getA1()), alpha1); + final FieldRotation r2 = new FieldRotation(new FieldVector3D(one, order.getA2()), alpha2); + final FieldRotation r3 = new FieldRotation(new FieldVector3D(one, order.getA3()), alpha3); + final FieldRotation composed = r1.applyTo(r2.applyTo(r3)); q0 = composed.q0; q1 = composed.q1; q2 = composed.q2; @@ -349,9 +327,9 @@ public class RotationDS implements Serializable { * @param ort orthogonal rotation matrix * @return quaternion corresponding to the matrix */ - private static DerivativeStructure[] mat2quat(final DerivativeStructure[][] ort) { + private T[] mat2quat(final T[][] ort) { - final DerivativeStructure[] quat = new DerivativeStructure[4]; + final T[] quat = MathArrays.buildArray(ort[0][0].getField(), 4); // There are different ways to compute the quaternions elements // from the matrix. They all involve computing one element from @@ -364,29 +342,29 @@ public class RotationDS implements Serializable { // 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) - DerivativeStructure s = ort[0][0].add(ort[1][1]).add(ort[2][2]); - if (s.getValue() > -0.19) { + T s = ort[0][0].add(ort[1][1]).add(ort[2][2]); + if (s.getReal() > -0.19) { // compute q0 and deduce q1, q2 and q3 quat[0] = s.add(1.0).sqrt().multiply(0.5); - DerivativeStructure inv = quat[0].reciprocal().multiply(0.25); + T inv = quat[0].reciprocal().multiply(0.25); quat[1] = inv.multiply(ort[1][2].subtract(ort[2][1])); quat[2] = inv.multiply(ort[2][0].subtract(ort[0][2])); quat[3] = inv.multiply(ort[0][1].subtract(ort[1][0])); } else { s = ort[0][0].subtract(ort[1][1]).subtract(ort[2][2]); - if (s.getValue() > -0.19) { + if (s.getReal() > -0.19) { // compute q1 and deduce q0, q2 and q3 quat[1] = s.add(1.0).sqrt().multiply(0.5); - DerivativeStructure inv = quat[1].reciprocal().multiply(0.25); + T inv = quat[1].reciprocal().multiply(0.25); quat[0] = inv.multiply(ort[1][2].subtract(ort[2][1])); quat[2] = inv.multiply(ort[0][1].add(ort[1][0])); quat[3] = inv.multiply(ort[0][2].add(ort[2][0])); } else { s = ort[1][1].subtract(ort[0][0]).subtract(ort[2][2]); - if (s.getValue() > -0.19) { + if (s.getReal() > -0.19) { // compute q2 and deduce q0, q1 and q3 quat[2] = s.add(1.0).sqrt().multiply(0.5); - DerivativeStructure inv = quat[2].reciprocal().multiply(0.25); + T inv = quat[2].reciprocal().multiply(0.25); quat[0] = inv.multiply(ort[2][0].subtract(ort[0][2])); quat[1] = inv.multiply(ort[0][1].add(ort[1][0])); quat[3] = inv.multiply(ort[2][1].add(ort[1][2])); @@ -394,7 +372,7 @@ public class RotationDS implements Serializable { // compute q3 and deduce q0, q1 and q2 s = ort[2][2].subtract(ort[0][0]).subtract(ort[1][1]); quat[3] = s.add(1.0).sqrt().multiply(0.5); - DerivativeStructure inv = quat[3].reciprocal().multiply(0.25); + T inv = quat[3].reciprocal().multiply(0.25); quat[0] = inv.multiply(ort[0][1].subtract(ort[1][0])); quat[1] = inv.multiply(ort[0][2].add(ort[2][0])); quat[2] = inv.multiply(ort[2][1].add(ort[1][2])); @@ -413,63 +391,63 @@ public class RotationDS implements Serializable { * @return a new rotation whose effect is the reverse of the effect * of the instance */ - public RotationDS revert() { - return new RotationDS(q0.negate(), q1, q2, q3, false); + public FieldRotation revert() { + return new FieldRotation(q0.negate(), q1, q2, q3, false); } /** Get the scalar coordinate of the quaternion. * @return scalar coordinate of the quaternion */ - public DerivativeStructure getQ0() { + public T getQ0() { return q0; } /** Get the first coordinate of the vectorial part of the quaternion. * @return first coordinate of the vectorial part of the quaternion */ - public DerivativeStructure getQ1() { + public T getQ1() { return q1; } /** Get the second coordinate of the vectorial part of the quaternion. * @return second coordinate of the vectorial part of the quaternion */ - public DerivativeStructure getQ2() { + public T getQ2() { return q2; } /** Get the third coordinate of the vectorial part of the quaternion. * @return third coordinate of the vectorial part of the quaternion */ - public DerivativeStructure getQ3() { + public T getQ3() { return q3; } /** Get the normalized axis of the rotation. * @return normalized axis of the rotation - * @see #Rotation(Vector3DDS, DerivativeStructure) + * @see #Rotation(FieldVector3D, T) */ - public Vector3DDS getAxis() { - final DerivativeStructure squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)); - if (squaredSine.getValue() == 0) { - final Field field = squaredSine.getField(); - return new Vector3DDS(field.getOne(), field.getZero(), field.getZero()); - } else if (q0.getValue() < 0) { - DerivativeStructure inverse = squaredSine.sqrt().reciprocal(); - return new Vector3DDS(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + public FieldVector3D getAxis() { + final T squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)); + if (squaredSine.getReal() == 0) { + final Field field = squaredSine.getField(); + return new FieldVector3D(field.getOne(), field.getZero(), field.getZero()); + } else if (q0.getReal() < 0) { + T inverse = squaredSine.sqrt().reciprocal(); + return new FieldVector3D(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); } - final DerivativeStructure inverse = squaredSine.sqrt().reciprocal().negate(); - return new Vector3DDS(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + final T inverse = squaredSine.sqrt().reciprocal().negate(); + return new FieldVector3D(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); } /** Get the angle of the rotation. * @return angle of the rotation (between 0 and π) - * @see #Rotation(Vector3DDS, DerivativeStructure) + * @see #Rotation(FieldVector3D, T) */ - public DerivativeStructure getAngle() { - if ((q0.getValue() < -0.1) || (q0.getValue() > 0.1)) { + public T getAngle() { + if ((q0.getReal() < -0.1) || (q0.getReal() > 0.1)) { return q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().asin().multiply(2); - } else if (q0.getValue() < 0) { + } else if (q0.getReal() < 0) { return q0.negate().acos().multiply(2); } return q0.acos().multiply(2); @@ -510,7 +488,7 @@ public class RotationDS implements Serializable { * @exception CardanEulerSingularityException if the rotation is * singular with respect to the angles set specified */ - public DerivativeStructure[] getAngles(final RotationOrder order) + public T[] getAngles(final RotationOrder order) throws CardanEulerSingularityException { if (order == RotationOrder.XYZ) { @@ -520,16 +498,14 @@ public class RotationDS implements Serializable { // (-r) (+I) coordinates are : // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) final // and we can choose to have theta in the interval [-PI/2 ; +PI/2] - Vector3DDS v1 = applyTo(vector(0, 0, 1)); - final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + FieldVector3D v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getY().negate(), v1.getZ()), - v2.getZ().asin(), - DerivativeStructure.atan2(v2.getY().negate(), v2.getX()) - }; + return buildArray(v1.getY().negate().atan2(v1.getZ()), + v2.getZ().asin(), + v2.getY().negate().atan2(v2.getX())); } else if (order == RotationOrder.XZY) { @@ -538,16 +514,14 @@ public class RotationDS implements Serializable { // (-r) (+I) coordinates are : // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) // and we can choose to have psi in the interval [-PI/2 ; +PI/2] - final Vector3DDS v1 = applyTo(vector(0, 1, 0)); - final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getZ(), v1.getY()), - v2.getY().asin().negate(), - DerivativeStructure.atan2(v2.getZ(), v2.getX()) - }; + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getY().asin().negate(), + v2.getZ().atan2(v2.getX())); } else if (order == RotationOrder.YXZ) { @@ -556,16 +530,14 @@ public class RotationDS implements Serializable { // (-r) (+J) coordinates are : // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) // and we can choose to have phi in the interval [-PI/2 ; +PI/2] - final Vector3DDS v1 = applyTo(vector(0, 0, 1)); - final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getX(), v1.getZ()), - v2.getZ().asin().negate(), - DerivativeStructure.atan2(v2.getX(), v2.getY()) - }; + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getZ().asin().negate(), + v2.getX().atan2(v2.getY())); } else if (order == RotationOrder.YZX) { @@ -574,16 +546,14 @@ public class RotationDS implements Serializable { // (-r) (+J) coordinates are : // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) // and we can choose to have psi in the interval [-PI/2 ; +PI/2] - final Vector3DDS v1 = applyTo(vector(1, 0, 0)); - final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getZ().negate(), v1.getX()), - v2.getX().asin(), - DerivativeStructure.atan2(v2.getZ().negate(), v2.getY()) - }; + return buildArray(v1.getZ().negate().atan2(v1.getX()), + v2.getX().asin(), + v2.getZ().negate().atan2(v2.getY())); } else if (order == RotationOrder.ZXY) { @@ -592,16 +562,14 @@ public class RotationDS implements Serializable { // (-r) (+K) coordinates are : // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) // and we can choose to have phi in the interval [-PI/2 ; +PI/2] - final Vector3DDS v1 = applyTo(vector(0, 1, 0)); - final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getX().negate(), v1.getY()), - v2.getY().asin(), - DerivativeStructure.atan2(v2.getX().negate(), v2.getZ()) - }; + return buildArray(v1.getX().negate().atan2(v1.getY()), + v2.getY().asin(), + v2.getX().negate().atan2(v2.getZ())); } else if (order == RotationOrder.ZYX) { @@ -610,16 +578,14 @@ public class RotationDS implements Serializable { // (-r) (+K) coordinates are : // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) // and we can choose to have theta in the interval [-PI/2 ; +PI/2] - final Vector3DDS v1 = applyTo(vector(1, 0, 0)); - final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getY(), v1.getX()), - v2.getX().asin().negate(), - DerivativeStructure.atan2(v2.getY(), v2.getZ()) - }; + return buildArray(v1.getY().atan2(v1.getX()), + v2.getX().asin().negate(), + v2.getY().atan2(v2.getZ())); } else if (order == RotationOrder.XYX) { @@ -628,16 +594,14 @@ public class RotationDS implements Serializable { // (-r) (+I) coordinates are : // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) // and we can choose to have theta in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(1, 0, 0)); - final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getY(), v1.getZ().negate()), - v2.getX().acos(), - DerivativeStructure.atan2(v2.getY(), v2.getZ()) - }; + return buildArray(v1.getY().atan2(v1.getZ().negate()), + v2.getX().acos(), + v2.getY().atan2(v2.getZ())); } else if (order == RotationOrder.XZX) { @@ -646,16 +610,14 @@ public class RotationDS implements Serializable { // (-r) (+I) coordinates are : // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) // and we can choose to have psi in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(1, 0, 0)); - final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getZ(), v1.getY()), - v2.getX().acos(), - DerivativeStructure.atan2(v2.getZ(), v2.getY().negate()) - }; + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getX().acos(), + v2.getZ().atan2(v2.getY().negate())); } else if (order == RotationOrder.YXY) { @@ -664,16 +626,14 @@ public class RotationDS implements Serializable { // (-r) (+J) coordinates are : // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) // and we can choose to have phi in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(0, 1, 0)); - final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getX(), v1.getZ()), - v2.getY().acos(), - DerivativeStructure.atan2(v2.getX(), v2.getZ().negate()) - }; + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getY().acos(), + v2.getX().atan2(v2.getZ().negate())); } else if (order == RotationOrder.YZY) { @@ -682,16 +642,14 @@ public class RotationDS implements Serializable { // (-r) (+J) coordinates are : // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) // and we can choose to have psi in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(0, 1, 0)); - final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getZ(), v1.getX().negate()), - v2.getY().acos(), - DerivativeStructure.atan2(v2.getZ(), v2.getX()) - }; + return buildArray(v1.getZ().atan2(v1.getX().negate()), + v2.getY().acos(), + v2.getZ().atan2(v2.getX())); } else if (order == RotationOrder.ZXZ) { @@ -700,16 +658,14 @@ public class RotationDS implements Serializable { // (-r) (+K) coordinates are : // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) // and we can choose to have phi in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(0, 0, 1)); - final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getX(), v1.getY().negate()), - v2.getZ().acos(), - DerivativeStructure.atan2(v2.getX(), v2.getY()) - }; + return buildArray(v1.getX().atan2(v1.getY().negate()), + v2.getZ().acos(), + v2.getX().atan2(v2.getY())); } else { // last possibility is ZYZ @@ -718,57 +674,63 @@ public class RotationDS implements Serializable { // (-r) (+K) coordinates are : // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) // and we can choose to have theta in the interval [0 ; PI] - final Vector3DDS v1 = applyTo(vector(0, 0, 1)); - final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + final FieldVector3D v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } - return new DerivativeStructure[] { - DerivativeStructure.atan2(v1.getY(), v1.getX()), - v2.getZ().acos(), - DerivativeStructure.atan2(v2.getY(), v2.getX().negate()) - }; + return buildArray(v1.getY().atan2(v1.getX()), + v2.getZ().acos(), + v2.getY().atan2(v2.getX().negate())); } } - /** Create a constant vector with appropriate derivation parameters. + /** Create a dimension 3 array. + * @param a0 first array element + * @param a1 second array element + * @param a2 third array element + * @return new array + */ + private T[] buildArray(final T a0, final T a1, final T a2) { + final T[] array = MathArrays.buildArray(a0.getField(), 3); + array[0] = a0; + array[1] = a1; + array[2] = a2; + return array; + } + + /** Create a constant vector. * @param x abscissa * @param y ordinate * @param z height * @return a constant vector */ - private Vector3DDS vector(final double x, final double y, final double z) { - final int parameters = q0.getFreeParameters(); - final int order = q0.getOrder(); - return new Vector3DDS(new DerivativeStructure(parameters, order, x), - new DerivativeStructure(parameters, order, y), - new DerivativeStructure(parameters, order, z)); + private FieldVector3D vector(final double x, final double y, final double z) { + final T zero = q0.getField().getZero(); + return new FieldVector3D(zero.add(x), zero.add(y), zero.add(z)); } /** Get the 3X3 matrix corresponding to the instance * @return the matrix corresponding to the instance */ - public DerivativeStructure[][] getMatrix() { + public T[][] getMatrix() { // products - final DerivativeStructure q0q0 = q0.multiply(q0); - final DerivativeStructure q0q1 = q0.multiply(q1); - final DerivativeStructure q0q2 = q0.multiply(q2); - final DerivativeStructure q0q3 = q0.multiply(q3); - final DerivativeStructure q1q1 = q1.multiply(q1); - final DerivativeStructure q1q2 = q1.multiply(q2); - final DerivativeStructure q1q3 = q1.multiply(q3); - final DerivativeStructure q2q2 = q2.multiply(q2); - final DerivativeStructure q2q3 = q2.multiply(q3); - final DerivativeStructure q3q3 = q3.multiply(q3); + final T q0q0 = q0.multiply(q0); + final T q0q1 = q0.multiply(q1); + final T q0q2 = q0.multiply(q2); + final T q0q3 = q0.multiply(q3); + final T q1q1 = q1.multiply(q1); + final T q1q2 = q1.multiply(q2); + final T q1q3 = q1.multiply(q3); + final T q2q2 = q2.multiply(q2); + final T q2q3 = q2.multiply(q3); + final T q3q3 = q3.multiply(q3); // create the matrix - final DerivativeStructure[][] m = new DerivativeStructure[3][]; - m[0] = new DerivativeStructure[3]; - m[1] = new DerivativeStructure[3]; - m[2] = new DerivativeStructure[3]; + final T[][] m = MathArrays.buildArray(q0.getField(), 3, 3); m [0][0] = q0q0.add(q1q1).multiply(2).subtract(1); m [1][0] = q1q2.subtract(q0q3).multiply(2); @@ -790,24 +752,24 @@ public class RotationDS implements Serializable { * @return a constant vector */ public Rotation toRotation() { - return new Rotation(q0.getValue(), q1.getValue(), q2.getValue(), q3.getValue(), false); + return new Rotation(q0.getReal(), q1.getReal(), q2.getReal(), q3.getReal(), false); } /** Apply the rotation to a vector. * @param u vector to apply the rotation to * @return a new vector which is the image of u by the rotation */ - public Vector3DDS applyTo(final Vector3DDS u) { + public FieldVector3D applyTo(final FieldVector3D u) { - final DerivativeStructure x = u.getX(); - final DerivativeStructure y = u.getY(); - final DerivativeStructure z = u.getZ(); + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - return new Vector3DDS(q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), - q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), - q0.multiply(z.multiply(q0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + return new FieldVector3D(q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + q0.multiply(z.multiply(q0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); } @@ -815,17 +777,17 @@ public class RotationDS implements Serializable { * @param u vector to apply the rotation to * @return a new vector which is the image of u by the rotation */ - public Vector3DDS applyTo(final Vector3D u) { + public FieldVector3D applyTo(final Vector3D u) { final double x = u.getX(); final double y = u.getY(); final double z = u.getZ(); - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - return new Vector3DDS(q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), - q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), - q0.multiply(q0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + return new FieldVector3D(q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + q0.multiply(q0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); } @@ -834,13 +796,13 @@ public class RotationDS implements Serializable { * @param out an array with three items to put result to (it can be the same * array as in) */ - public void applyTo(final DerivativeStructure[] in, final DerivativeStructure[] out) { + public void applyTo(final T[] in, final T[] out) { - final DerivativeStructure x = in[0]; - final DerivativeStructure y = in[1]; - final DerivativeStructure z = in[2]; + final T x = in[0]; + final T y = in[1]; + final T z = in[2]; - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); out[0] = q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); out[1] = q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); @@ -852,13 +814,13 @@ public class RotationDS implements Serializable { * @param in an array with three items which stores vector to rotate * @param out an array with three items to put result to */ - public void applyTo(final double[] in, final DerivativeStructure[] out) { + public void applyTo(final double[] in, final T[] out) { final double x = in[0]; final double y = in[1]; final double z = in[2]; - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); out[0] = q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); out[1] = q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); @@ -871,17 +833,17 @@ public class RotationDS implements Serializable { * @param u vector to apply the rotation to * @return a new vector which is the image of u by the rotation */ - public static Vector3DDS applyTo(final Rotation r, final Vector3DDS u) { + public static > FieldVector3D applyTo(final Rotation r, final FieldVector3D u) { - final DerivativeStructure x = u.getX(); - final DerivativeStructure y = u.getY(); - final DerivativeStructure z = u.getZ(); + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); - final DerivativeStructure s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + final T s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); - return new Vector3DDS(x.multiply(r.getQ0()).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(r.getQ0()).add(s.multiply(r.getQ1())).multiply(2).subtract(x), - y.multiply(r.getQ0()).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(r.getQ0()).add(s.multiply(r.getQ2())).multiply(2).subtract(y), - z.multiply(r.getQ0()).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(r.getQ0()).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); + return new FieldVector3D(x.multiply(r.getQ0()).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(r.getQ0()).add(s.multiply(r.getQ1())).multiply(2).subtract(x), + y.multiply(r.getQ0()).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(r.getQ0()).add(s.multiply(r.getQ2())).multiply(2).subtract(y), + z.multiply(r.getQ0()).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(r.getQ0()).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); } @@ -889,18 +851,18 @@ public class RotationDS implements Serializable { * @param u vector to apply the inverse of the rotation to * @return a new vector which such that u is its image by the rotation */ - public Vector3DDS applyInverseTo(final Vector3DDS u) { + public FieldVector3D applyInverseTo(final FieldVector3D u) { - final DerivativeStructure x = u.getX(); - final DerivativeStructure y = u.getY(); - final DerivativeStructure z = u.getZ(); + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - final DerivativeStructure m0 = q0.negate(); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); - return new Vector3DDS(m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), - m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), - m0.multiply(z.multiply(m0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + return new FieldVector3D(m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + m0.multiply(z.multiply(m0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); } @@ -908,18 +870,18 @@ public class RotationDS implements Serializable { * @param u vector to apply the inverse of the rotation to * @return a new vector which such that u is its image by the rotation */ - public Vector3DDS applyInverseTo(final Vector3D u) { + public FieldVector3D applyInverseTo(final Vector3D u) { final double x = u.getX(); final double y = u.getY(); final double z = u.getZ(); - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - final DerivativeStructure m0 = q0.negate(); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); - return new Vector3DDS(m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), - m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), - m0.multiply(m0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + return new FieldVector3D(m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + m0.multiply(m0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); } @@ -928,14 +890,14 @@ public class RotationDS implements Serializable { * @param out an array with three items to put result to (it can be the same * array as in) */ - public void applyInverseTo(final DerivativeStructure[] in, final DerivativeStructure[] out) { + public void applyInverseTo(final T[] in, final T[] out) { - final DerivativeStructure x = in[0]; - final DerivativeStructure y = in[1]; - final DerivativeStructure z = in[2]; + final T x = in[0]; + final T y = in[1]; + final T z = in[2]; - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - final DerivativeStructure m0 = q0.negate(); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); out[0] = m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); out[1] = m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); @@ -947,14 +909,14 @@ public class RotationDS implements Serializable { * @param in an array with three items which stores vector to rotate * @param out an array with three items to put result to */ - public void applyInverseTo(final double[] in, final DerivativeStructure[] out) { + public void applyInverseTo(final double[] in, final T[] out) { final double x = in[0]; final double y = in[1]; final double z = in[2]; - final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); - final DerivativeStructure m0 = q0.negate(); + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); out[0] = m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); out[1] = m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); @@ -967,18 +929,18 @@ public class RotationDS implements Serializable { * @param u vector to apply the inverse of the rotation to * @return a new vector which such that u is its image by the rotation */ - public static Vector3DDS applyInverseTo(final Rotation r, final Vector3DDS u) { + public static > FieldVector3D applyInverseTo(final Rotation r, final FieldVector3D u) { - final DerivativeStructure x = u.getX(); - final DerivativeStructure y = u.getY(); - final DerivativeStructure z = u.getZ(); + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); - final DerivativeStructure s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + final T s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); final double m0 = -r.getQ0(); - return new Vector3DDS(x.multiply(m0).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(m0).add(s.multiply(r.getQ1())).multiply(2).subtract(x), - y.multiply(m0).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(m0).add(s.multiply(r.getQ2())).multiply(2).subtract(y), - z.multiply(m0).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(m0).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); + return new FieldVector3D(x.multiply(m0).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(m0).add(s.multiply(r.getQ1())).multiply(2).subtract(x), + y.multiply(m0).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(m0).add(s.multiply(r.getQ2())).multiply(2).subtract(y), + z.multiply(m0).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(m0).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); } @@ -991,12 +953,12 @@ public class RotationDS implements Serializable { * @param r rotation to apply the rotation to * @return a new rotation which is the composition of r by the instance */ - public RotationDS applyTo(final RotationDS r) { - return new RotationDS(r.q0.multiply(q0).subtract(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))), - r.q1.multiply(q0).add(r.q0.multiply(q1)).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))), - r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))), - r.q3.multiply(q0).add(r.q0.multiply(q3)).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))), - false); + public FieldRotation applyTo(final FieldRotation r) { + return new FieldRotation(r.q0.multiply(q0).subtract(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))), + r.q1.multiply(q0).add(r.q0.multiply(q1)).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))), + r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))), + r.q3.multiply(q0).add(r.q0.multiply(q3)).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))), + false); } /** Apply the instance to another rotation. @@ -1008,12 +970,12 @@ public class RotationDS implements Serializable { * @param r rotation to apply the rotation to * @return a new rotation which is the composition of r by the instance */ - public RotationDS applyTo(final Rotation r) { - return new RotationDS(q0.multiply(r.getQ0()).subtract(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))), - q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))), - q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))), - q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))), - false); + public FieldRotation applyTo(final Rotation r) { + return new FieldRotation(q0.multiply(r.getQ0()).subtract(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))), + q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))), + q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))), + q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))), + false); } /** Apply a rotation to another rotation. @@ -1026,12 +988,12 @@ public class RotationDS implements Serializable { * @param rInner rotation to apply the rotation to * @return a new rotation which is the composition of r by the instance */ - public static RotationDS applyTo(final Rotation r1, final RotationDS rInner) { - return new RotationDS(rInner.q0.multiply(r1.getQ0()).subtract(rInner.q1.multiply(r1.getQ1()).add(rInner.q2.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ3()))), - rInner.q1.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ1())).add(rInner.q2.multiply(r1.getQ3()).subtract(rInner.q3.multiply(r1.getQ2()))), - rInner.q2.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ1()).subtract(rInner.q1.multiply(r1.getQ3()))), - rInner.q3.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ3())).add(rInner.q1.multiply(r1.getQ2()).subtract(rInner.q2.multiply(r1.getQ1()))), - false); + public static > FieldRotation applyTo(final Rotation r1, final FieldRotation rInner) { + return new FieldRotation(rInner.q0.multiply(r1.getQ0()).subtract(rInner.q1.multiply(r1.getQ1()).add(rInner.q2.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ3()))), + rInner.q1.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ1())).add(rInner.q2.multiply(r1.getQ3()).subtract(rInner.q3.multiply(r1.getQ2()))), + rInner.q2.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ1()).subtract(rInner.q1.multiply(r1.getQ3()))), + rInner.q3.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ3())).add(rInner.q1.multiply(r1.getQ2()).subtract(rInner.q2.multiply(r1.getQ1()))), + false); } /** Apply the inverse of the instance to another rotation. @@ -1045,12 +1007,12 @@ public class RotationDS implements Serializable { * @return a new rotation which is the composition of r by the inverse * of the instance */ - public RotationDS applyInverseTo(final RotationDS r) { - return new RotationDS(r.q0.multiply(q0).add(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))).negate(), - r.q0.multiply(q1).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))).subtract(r.q1.multiply(q0)), - r.q0.multiply(q2).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))).subtract(r.q2.multiply(q0)), - r.q0.multiply(q3).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))).subtract(r.q3.multiply(q0)), - false); + public FieldRotation applyInverseTo(final FieldRotation r) { + return new FieldRotation(r.q0.multiply(q0).add(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))).negate(), + r.q0.multiply(q1).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))).subtract(r.q1.multiply(q0)), + r.q0.multiply(q2).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))).subtract(r.q2.multiply(q0)), + r.q0.multiply(q3).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))).subtract(r.q3.multiply(q0)), + false); } /** Apply the inverse of the instance to another rotation. @@ -1064,12 +1026,12 @@ public class RotationDS implements Serializable { * @return a new rotation which is the composition of r by the inverse * of the instance */ - public RotationDS applyInverseTo(final Rotation r) { - return new RotationDS(q0.multiply(r.getQ0()).add(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))).negate(), - q1.multiply(r.getQ0()).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))).subtract(q0.multiply(r.getQ1())), - q2.multiply(r.getQ0()).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))).subtract(q0.multiply(r.getQ2())), - q3.multiply(r.getQ0()).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))).subtract(q0.multiply(r.getQ3())), - false); + public FieldRotation applyInverseTo(final Rotation r) { + return new FieldRotation(q0.multiply(r.getQ0()).add(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))).negate(), + q1.multiply(r.getQ0()).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))).subtract(q0.multiply(r.getQ1())), + q2.multiply(r.getQ0()).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))).subtract(q0.multiply(r.getQ2())), + q3.multiply(r.getQ0()).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))).subtract(q0.multiply(r.getQ3())), + false); } /** Apply the inverse of a rotation to another rotation. @@ -1084,12 +1046,12 @@ public class RotationDS implements Serializable { * @return a new rotation which is the composition of r by the inverse * of the instance */ - public static RotationDS applyInverseTo(final Rotation rOuter, final RotationDS rInner) { - return new RotationDS(rInner.q0.multiply(rOuter.getQ0()).add(rInner.q1.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ2())).add(rInner.q3.multiply(rOuter.getQ3()))).negate(), - rInner.q0.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ3()).subtract(rInner.q3.multiply(rOuter.getQ2()))).subtract(rInner.q1.multiply(rOuter.getQ0())), - rInner.q0.multiply(rOuter.getQ2()).add(rInner.q3.multiply(rOuter.getQ1()).subtract(rInner.q1.multiply(rOuter.getQ3()))).subtract(rInner.q2.multiply(rOuter.getQ0())), - rInner.q0.multiply(rOuter.getQ3()).add(rInner.q1.multiply(rOuter.getQ2()).subtract(rInner.q2.multiply(rOuter.getQ1()))).subtract(rInner.q3.multiply(rOuter.getQ0())), - false); + public static > FieldRotation applyInverseTo(final Rotation rOuter, final FieldRotation rInner) { + return new FieldRotation(rInner.q0.multiply(rOuter.getQ0()).add(rInner.q1.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ2())).add(rInner.q3.multiply(rOuter.getQ3()))).negate(), + rInner.q0.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ3()).subtract(rInner.q3.multiply(rOuter.getQ2()))).subtract(rInner.q1.multiply(rOuter.getQ0())), + rInner.q0.multiply(rOuter.getQ2()).add(rInner.q3.multiply(rOuter.getQ1()).subtract(rInner.q1.multiply(rOuter.getQ3()))).subtract(rInner.q2.multiply(rOuter.getQ0())), + rInner.q0.multiply(rOuter.getQ3()).add(rInner.q1.multiply(rOuter.getQ2()).subtract(rInner.q2.multiply(rOuter.getQ1()))).subtract(rInner.q3.multiply(rOuter.getQ0())), + false); } /** Perfect orthogonality on a 3X3 matrix. @@ -1102,38 +1064,37 @@ public class RotationDS implements Serializable { * @exception NotARotationMatrixException if the matrix cannot be * orthogonalized with the given threshold after 10 iterations */ - private DerivativeStructure[][] orthogonalizeMatrix(final DerivativeStructure[][] m, - final double threshold) + private T[][] orthogonalizeMatrix(final T[][] m, final double threshold) throws NotARotationMatrixException { - DerivativeStructure x00 = m[0][0]; - DerivativeStructure x01 = m[0][1]; - DerivativeStructure x02 = m[0][2]; - DerivativeStructure x10 = m[1][0]; - DerivativeStructure x11 = m[1][1]; - DerivativeStructure x12 = m[1][2]; - DerivativeStructure x20 = m[2][0]; - DerivativeStructure x21 = m[2][1]; - DerivativeStructure x22 = m[2][2]; + T x00 = m[0][0]; + T x01 = m[0][1]; + T x02 = m[0][2]; + T x10 = m[1][0]; + T x11 = m[1][1]; + T x12 = m[1][2]; + T x20 = m[2][0]; + T x21 = m[2][1]; + T x22 = m[2][2]; double fn = 0; double fn1; - final DerivativeStructure[][] o = new DerivativeStructure[3][3]; + final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3); // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) int i = 0; while (++i < 11) { // Mt.Xn - final DerivativeStructure mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20)); - final DerivativeStructure mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20)); - final DerivativeStructure mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20)); - final DerivativeStructure mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21)); - final DerivativeStructure mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21)); - final DerivativeStructure mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21)); - final DerivativeStructure mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22)); - final DerivativeStructure mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22)); - final DerivativeStructure mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22)); + final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20)); + final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20)); + final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20)); + final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21)); + final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21)); + final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21)); + final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22)); + final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22)); + final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22)); // Xn+1 o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5)); @@ -1147,15 +1108,15 @@ public class RotationDS implements Serializable { o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5)); // correction on each elements - final double corr00 = o[0][0].getValue() - m[0][0].getValue(); - final double corr01 = o[0][1].getValue() - m[0][1].getValue(); - final double corr02 = o[0][2].getValue() - m[0][2].getValue(); - final double corr10 = o[1][0].getValue() - m[1][0].getValue(); - final double corr11 = o[1][1].getValue() - m[1][1].getValue(); - final double corr12 = o[1][2].getValue() - m[1][2].getValue(); - final double corr20 = o[2][0].getValue() - m[2][0].getValue(); - final double corr21 = o[2][1].getValue() - m[2][1].getValue(); - final double corr22 = o[2][2].getValue() - m[2][2].getValue(); + final double corr00 = o[0][0].getReal() - m[0][0].getReal(); + final double corr01 = o[0][1].getReal() - m[0][1].getReal(); + final double corr02 = o[0][2].getReal() - m[0][2].getReal(); + final double corr10 = o[1][0].getReal() - m[1][0].getReal(); + final double corr11 = o[1][1].getReal() - m[1][1].getReal(); + final double corr12 = o[1][2].getReal() - m[1][2].getReal(); + final double corr20 = o[2][0].getReal() - m[2][0].getReal(); + final double corr21 = o[2][1].getReal() - m[2][1].getReal(); + final double corr22 = o[2][2].getReal() - m[2][2].getReal(); // Frobenius norm of the correction fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 + @@ -1211,7 +1172,7 @@ public class RotationDS implements Serializable { * @param r2 second rotation * @return distance between r1 and r2 */ - public static DerivativeStructure distance(final RotationDS r1, final RotationDS r2) { + public static > T distance(final FieldRotation r1, final FieldRotation r2) { return r1.applyInverseTo(r2).getAngle(); } diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java new file mode 100644 index 000000000..ed49b9a25 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java @@ -0,0 +1,891 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.apache.commons.math3.geometry.euclidean.threed; + +import java.io.Serializable; +import java.text.NumberFormat; + +import org.apache.commons.math3.ExtendedFieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; + +/** + * This class is a re-implementation of {@link Vector3D} using {@link ExtendedFieldElement}. + *

Instance of this class are guaranteed to be immutable.

+ * @param the type of the field elements + * @version $Id$ + * @since 3.2 + */ +public class FieldVector3D> implements Serializable { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 20130224L; + + /** Abscissa. */ + private final T x; + + /** Ordinate. */ + private final T y; + + /** Height. */ + private final T z; + + /** Simple constructor. + * Build a vector from its coordinates + * @param x abscissa + * @param y ordinate + * @param z height + * @see #getX() + * @see #getY() + * @see #getZ() + */ + public FieldVector3D(final T x, final T y, final T z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** Simple constructor. + * Build a vector from its coordinates + * @param v coordinates array + * @exception DimensionMismatchException if array does not have 3 elements + * @see #toArray() + */ + public FieldVector3D(final T[] v) throws DimensionMismatchException { + if (v.length != 3) { + throw new DimensionMismatchException(v.length, 3); + } + this.x = v[0]; + this.y = v[1]; + this.z = v[2]; + } + + /** Simple constructor. + * Build a vector from its azimuthal coordinates + * @param alpha azimuth (α) around Z + * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y) + * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/2 + * @see #getAlpha() + * @see #getDelta() + */ + public FieldVector3D(final T alpha, final T delta) { + T cosDelta = delta.cos(); + this.x = alpha.cos().multiply(cosDelta); + this.y = alpha.sin().multiply(cosDelta); + this.z = delta.sin(); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final T a, final FieldVector3Du) { + this.x = a.multiply(u.x); + this.y = a.multiply(u.y); + this.z = a.multiply(u.z); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final T a, final Vector3D u) { + this.x = a.multiply(u.getX()); + this.y = a.multiply(u.getY()); + this.z = a.multiply(u.getZ()); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final double a, final FieldVector3D u) { + this.x = u.x.multiply(a); + this.y = u.y.multiply(a); + this.z = u.z.multiply(a); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D u1, + final T a2, final FieldVector3D u2) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D u1, + final double a2, final FieldVector3D u2) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D u1, + final T a2, final FieldVector3D u2, + final T a3, final FieldVector3D u3) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2, + final T a3, final Vector3D u3) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D u1, + final double a2, final FieldVector3D u2, + final double a3, final FieldVector3D u3) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D u1, + final T a2, final FieldVector3D u2, + final T a3, final FieldVector3D u3, + final T a4, final FieldVector3D u4) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2, + final T a3, final Vector3D u3, + final T a4, final Vector3D u4) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3, u4.getX(), a4); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3, u4.getY(), a4); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3, u4.getZ(), a4); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D u1, + final double a2, final FieldVector3D u2, + final double a3, final FieldVector3D u3, + final double a4, final FieldVector3D u4) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); + } + + /** Get the abscissa of the vector. + * @return abscissa of the vector + * @see #Vector3D(T, T, T) + */ + public T getX() { + return x; + } + + /** Get the ordinate of the vector. + * @return ordinate of the vector + * @see #Vector3D(T, T, T) + */ + public T getY() { + return y; + } + + /** Get the height of the vector. + * @return height of the vector + * @see #Vector3D(T, T, T) + */ + public T getZ() { + return z; + } + + /** Get the vector coordinates as a dimension 3 array. + * @return vector coordinates + * @see #Vector3D(T[]) + */ + public T[] toArray() { + final T[] array = MathArrays.buildArray(x.getField(), 3); + array[0] = x; + array[1] = y; + array[2] = z; + return array; + } + + /** Convert to a constant vector without derivatives. + * @return a constant vector + */ + public Vector3D toVector3D() { + return new Vector3D(x.getReal(), y.getReal(), z.getReal()); + } + + /** Get the L1 norm for the vector. + * @return L1 norm for the vector + */ + public T getNorm1() { + return x.abs().add(y.abs()).add(z.abs()); + } + + /** Get the L2 norm for the vector. + * @return Euclidean norm for the vector + */ + public T getNorm() { + // there are no cancellation problems here, so we use the straightforward formula + return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)).sqrt(); + } + + /** Get the square of the norm for the vector. + * @return square of the Euclidean norm for the vector + */ + public T getNormSq() { + // there are no cancellation problems here, so we use the straightforward formula + return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)); + } + + /** Get the L∞ norm for the vector. + * @return L∞ norm for the vector + */ + public T getNormInf() { + final T xAbs = x.abs(); + final T yAbs = y.abs(); + final T zAbs = z.abs(); + if (xAbs.getReal() <= yAbs.getReal()) { + if (yAbs.getReal() <= zAbs.getReal()) { + return zAbs; + } else { + return yAbs; + } + } else { + if (xAbs.getReal() <= zAbs.getReal()) { + return zAbs; + } else { + return xAbs; + } + } + } + + /** Get the azimuth of the vector. + * @return azimuth (α) of the vector, between -π and +π + * @see #Vector3D(T, T) + */ + public T getAlpha() { + return y.atan2(x); + } + + /** Get the elevation of the vector. + * @return elevation (δ) of the vector, between -π/2 and +π/2 + * @see #Vector3D(T, T) + */ + public T getDelta() { + return z.divide(getNorm()).asin(); + } + + /** Add a vector to the instance. + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final FieldVector3D v) { + return new FieldVector3D(x.add(v.x), y.add(v.y), z.add(v.z)); + } + + /** Add a vector to the instance. + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final Vector3D v) { + return new FieldVector3D(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ())); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final T factor, final FieldVector3D v) { + return new FieldVector3D(x.getField().getOne(), this, factor, v); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final T factor, final Vector3D v) { + return new FieldVector3D(x.add(factor.multiply(v.getX())), + y.add(factor.multiply(v.getY())), + z.add(factor.multiply(v.getZ()))); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final double factor, final FieldVector3D v) { + return new FieldVector3D(1.0, this, factor, v); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D add(final double factor, final Vector3D v) { + return new FieldVector3D(x.add(factor * v.getX()), + y.add(factor * v.getY()), + z.add(factor * v.getZ())); + } + + /** Subtract a vector from the instance. + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final FieldVector3D v) { + return new FieldVector3D(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z)); + } + + /** Subtract a vector from the instance. + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final Vector3D v) { + return new FieldVector3D(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ())); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final T factor, final FieldVector3D v) { + return new FieldVector3D(x.getField().getOne(), this, factor.negate(), v); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final T factor, final Vector3D v) { + return new FieldVector3D(x.subtract(factor.multiply(v.getX())), + y.subtract(factor.multiply(v.getY())), + z.subtract(factor.multiply(v.getZ()))); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final double factor, final FieldVector3D v) { + return new FieldVector3D(1.0, this, -factor, v); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D subtract(final double factor, final Vector3D v) { + return new FieldVector3D(x.subtract(factor * v.getX()), + y.subtract(factor * v.getY()), + z.subtract(factor * v.getZ())); + } + + /** Get a normalized vector aligned with the instance. + * @return a new normalized vector + * @exception MathArithmeticException if the norm is zero + */ + public FieldVector3D normalize() throws MathArithmeticException { + final T s = getNorm(); + if (s.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); + } + return scalarMultiply(s.reciprocal()); + } + + /** Get a vector orthogonal to the instance. + *

There are an infinite number of normalized vectors orthogonal + * to the instance. This method picks up one of them almost + * arbitrarily. It is useful when one needs to compute a reference + * frame with one of the axes in a predefined direction. The + * following example shows how to build a frame having the k axis + * aligned with the known vector u : + * 


+     *   Vector3D k = u.normalize();
+     *   Vector3D i = k.orthogonal();
+     *   Vector3D j = Vector3D.crossProduct(k, i);
+     *

+ * @return a new normalized vector orthogonal to the instance + * @exception MathArithmeticException if the norm of the instance is null + */ + public FieldVector3D orthogonal() throws MathArithmeticException { + + final double threshold = 0.6 * getNorm().getReal(); + if (threshold == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + if (FastMath.abs(x.getReal()) <= threshold) { + final T inverse = y.multiply(y).add(z.multiply(z)).sqrt().reciprocal(); + return new FieldVector3D(inverse.getField().getZero(), inverse.multiply(z), inverse.multiply(y).negate()); + } else if (FastMath.abs(y.getReal()) <= threshold) { + final T inverse = x.multiply(x).add(z.multiply(z)).sqrt().reciprocal(); + return new FieldVector3D(inverse.multiply(z).negate(), inverse.getField().getZero(), inverse.multiply(x)); + } else { + final T inverse = x.multiply(x).add(y.multiply(y)).sqrt().reciprocal(); + return new FieldVector3D(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero()); + } + + } + + /** Compute the angular separation between the instance and another vector. + *

This method computes the angular separation between two + * vectors using the dot product for well separated vectors and the + * cross product for almost aligned vectors. This allows to have a + * good accuracy in all cases, even for vectors very close to each + * other.

+ * @param v second vector + * @return angular separation between the instance and v + * @exception MathArithmeticException if either vector has a null norm + */ + public T angle(FieldVector3D v) throws MathArithmeticException { + + final T normProduct = getNorm().multiply(v.getNorm()); + if (normProduct.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + final T dot = dotProduct(v); + final double threshold = normProduct.getReal() * 0.9999; + if ((dot.getReal() < -threshold) || (dot.getReal() > threshold)) { + // the vectors are almost aligned, compute using the sine + FieldVector3D v3 = crossProduct(v); + if (dot.getReal() >= 0) { + return v3.getNorm().divide(normProduct).asin(); + } + return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); + } + + // the vectors are sufficiently separated to use the cosine + return dot.divide(normProduct).acos(); + + } + + /** Get the opposite of the instance. + * @return a new vector which is opposite to the instance + */ + public FieldVector3D negate() { + return new FieldVector3D(x.negate(), y.negate(), z.negate()); + } + + /** Multiply the instance by a scalar. + * @param a scalar + * @return a new vector + */ + public FieldVector3D scalarMultiply(final T a) { + return new FieldVector3D(x.multiply(a), y.multiply(a), z.multiply(a)); + } + + /** Multiply the instance by a scalar. + * @param a scalar + * @return a new vector + */ + public FieldVector3D scalarMultiply(final double a) { + return new FieldVector3D(x.multiply(a), y.multiply(a), z.multiply(a)); + } + + /** + * Returns true if any coordinate of this vector is NaN; false otherwise + * @return true if any coordinate of this vector is NaN; false otherwise + */ + public boolean isNaN() { + return Double.isNaN(x.getReal()) || Double.isNaN(y.getReal()) || Double.isNaN(z.getReal()); + } + + /** + * Returns true if any coordinate of this vector is infinite and none are NaN; + * false otherwise + * @return true if any coordinate of this vector is infinite and none are NaN; + * false otherwise + */ + public boolean isInfinite() { + return !isNaN() && (Double.isInfinite(x.getReal()) || Double.isInfinite(y.getReal()) || Double.isInfinite(z.getReal())); + } + + /** + * Test for the equality of two 3D vectors. + *

+ * If all coordinates of two 3D vectors are exactly the same, and none are + * T.NaN, the two 3D vectors are considered to be equal. + *

+ *

+ * NaN coordinates are considered to affect globally the vector + * and be equals to each other - i.e, if either (or all) coordinates of the + * 3D vector are equal to T.NaN, the 3D vector is equal to + * {@link #NaN}. + *

+ * + * @param other Object to test for equality to this + * @return true if two 3D vector objects are equal, false if + * object is null, not an instance of Vector3D, or + * not equal to this Vector3D instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof FieldVector3D) { + @SuppressWarnings("unchecked") + final FieldVector3D rhs = (FieldVector3D) other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return x.equals(rhs.x) && y.equals(rhs.y) && z.equals(rhs.z); + + } + return false; + } + + /** + * Get a hashCode for the 3D vector. + *

+ * All NaN values have the same hash code.

+ * + * @return a hash code value for this object + */ + @Override + public int hashCode() { + if (isNaN()) { + return 409; + } + return 311 * (107 * x.hashCode() + 83 * y.hashCode() + z.hashCode()); + } + + /** Compute the dot-product of the instance and another vector. + *

+ * 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 MathArrays#linearCombination(double, double, double, double, double, double) + * @param v second vector + * @return the dot product this.v + */ + public T dotProduct(final FieldVector3D v) { + return x.linearCombination(x, v.x, y, v.y, z, v.z); + } + + /** Compute the dot-product of the instance and another vector. + *

+ * 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 MathArrays#linearCombination(double, double, double, double, double, double) + * @param v second vector + * @return the dot product this.v + */ + public T dotProduct(final Vector3D v) { + return x.linearCombination(v.getX(), x, v.getY(), y, v.getZ(), z); + } + + /** Compute the cross-product of the instance with another vector. + * @param v other vector + * @return the cross product this ^ v as a new Vector3D + */ + public FieldVector3D crossProduct(final FieldVector3D v) { + return new FieldVector3D(x.linearCombination(y, v.z, z.negate(), v.y), + y.linearCombination(z, v.x, x.negate(), v.z), + z.linearCombination(x, v.y, y.negate(), v.x)); + } + + /** Compute the cross-product of the instance with another vector. + * @param v other vector + * @return the cross product this ^ v as a new Vector3D + */ + public FieldVector3D crossProduct(final Vector3D v) { + return new FieldVector3D(x.linearCombination(v.getZ(), y, -v.getY(), z), + y.linearCombination(v.getX(), z, -v.getZ(), x), + z.linearCombination(v.getY(), x, -v.getX(), y)); + } + + /** Compute the distance between the instance and another vector according to the L1 norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNorm1() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L1 norm + */ + public T distance1(final FieldVector3D v) { + final T dx = v.x.subtract(x).abs(); + final T dy = v.y.subtract(y).abs(); + final T dz = v.z.subtract(z).abs(); + return dx.add(dy).add(dz); + } + + /** Compute the distance between the instance and another vector according to the L1 norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNorm1() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L1 norm + */ + public T distance1(final Vector3D v) { + final T dx = x.subtract(v.getX()).abs(); + final T dy = y.subtract(v.getY()).abs(); + final T dz = z.subtract(v.getZ()).abs(); + return dx.add(dy).add(dz); + } + + /** Compute the distance between the instance and another vector according to the L2 norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNorm() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L2 norm + */ + public T distance(final FieldVector3D v) { + final T dx = v.x.subtract(x); + final T dy = v.y.subtract(y); + final T dz = v.z.subtract(z); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); + } + + /** Compute the distance between the instance and another vector according to the L2 norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNorm() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L2 norm + */ + public T distance(final Vector3D v) { + final T dx = x.subtract(v.getX()); + final T dy = y.subtract(v.getY()); + final T dz = z.subtract(v.getZ()); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); + } + + /** Compute the distance between the instance and another vector according to the L∞ norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNormInf() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L∞ norm + */ + public T distanceInf(final FieldVector3D v) { + final T dx = v.x.subtract(x).abs(); + final T dy = v.y.subtract(y).abs(); + final T dz = v.z.subtract(z).abs(); + if (dx.getReal() <= dy.getReal()) { + if (dy.getReal() <= dz.getReal()) { + return dz; + } else { + return dy; + } + } else { + if (dx.getReal() <= dz.getReal()) { + return dz; + } else { + return dx; + } + } + } + + /** Compute the distance between the instance and another vector according to the L∞ norm. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNormInf() except that no intermediate + * vector is built

+ * @param v second vector + * @return the distance between the instance and p according to the L∞ norm + */ + public T distanceInf(final Vector3D v) { + final T dx = x.subtract(v.getX()).abs(); + final T dy = y.subtract(v.getY()).abs(); + final T dz = z.subtract(v.getZ()).abs(); + if (dx.getReal() <= dy.getReal()) { + if (dy.getReal() <= dz.getReal()) { + return dz; + } else { + return dy; + } + } else { + if (dx.getReal() <= dz.getReal()) { + return dz; + } else { + return dx; + } + } + } + + /** Compute the square of the distance between the instance and another vector. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNormSq() except that no intermediate + * vector is built

+ * @param v second vector + * @return the square of the distance between the instance and p + */ + public T distanceSq(final FieldVector3D v) { + final T dx = v.x.subtract(x); + final T dy = v.y.subtract(y); + final T dz = v.z.subtract(z); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); + } + + /** Compute the square of the distance between the instance and another vector. + *

Calling this method is equivalent to calling: + * q.subtract(p).getNormSq() except that no intermediate + * vector is built

+ * @param v second vector + * @return the square of the distance between the instance and p + */ + public T distanceSq(final Vector3D v) { + final T dx = x.subtract(v.getX()); + final T dy = y.subtract(v.getY()); + final T dz = z.subtract(v.getZ()); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); + } + + /** Get a string representation of this vector. + * @return a string representation of this vector + */ + @Override + public String toString() { + return Vector3DFormat.getInstance().format(toVector3D()); + } + + /** {@inheritDoc} */ + public String toString(final NumberFormat format) { + return new Vector3DFormat(format).format(toVector3D()); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java deleted file mode 100644 index 31e392ebb..000000000 --- a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java +++ /dev/null @@ -1,1087 +0,0 @@ -/* - * Licensed to the Apache Software Foundation (ASF) under one or more - * contributor license agreements. See the NOTICE file distributed with - * this work for additional information regarding copyright ownership. - * The ASF licenses this file to You under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with - * the License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -package org.apache.commons.math3.geometry.euclidean.threed; - -import java.io.Serializable; -import java.text.NumberFormat; - -import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; -import org.apache.commons.math3.exception.DimensionMismatchException; -import org.apache.commons.math3.exception.MathArithmeticException; -import org.apache.commons.math3.exception.util.LocalizedFormats; -import org.apache.commons.math3.util.FastMath; -import org.apache.commons.math3.util.MathArrays; - -/** - * This class is a re-implementation of {@link Vector3D} using {@link DerivativeStructure}. - *

Instance of this class are guaranteed to be immutable.

- * @version $Id$ - * @since 3.2 - */ -public class Vector3DDS implements Serializable { - - /** Serializable version identifier. */ - private static final long serialVersionUID = 20130214L; - - /** Abscissa. */ - private final DerivativeStructure x; - - /** Ordinate. */ - private final DerivativeStructure y; - - /** Height. */ - private final DerivativeStructure z; - - /** Simple constructor. - * Build a vector from its coordinates - * @param x abscissa - * @param y ordinate - * @param z height - * @see #getX() - * @see #getY() - * @see #getZ() - */ - public Vector3DDS(final DerivativeStructure x, - final DerivativeStructure y, - final DerivativeStructure z) { - this.x = x; - this.y = y; - this.z = z; - } - - /** Simple constructor. - * Build a vector from its coordinates - * @param v coordinates array - * @exception DimensionMismatchException if array does not have 3 elements - * @see #toArray() - */ - public Vector3DDS(final DerivativeStructure[] v) throws DimensionMismatchException { - if (v.length != 3) { - throw new DimensionMismatchException(v.length, 3); - } - this.x = v[0]; - this.y = v[1]; - this.z = v[2]; - } - - /** Simple constructor. - * Build a vector from its azimuthal coordinates - * @param alpha azimuth (α) around Z - * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y) - * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/2 - * @see #getAlpha() - * @see #getDelta() - */ - public Vector3DDS(final DerivativeStructure alpha, final DerivativeStructure delta) { - DerivativeStructure cosDelta = delta.cos(); - this.x = alpha.cos().multiply(cosDelta); - this.y = alpha.sin().multiply(cosDelta); - this.z = delta.sin(); - } - - /** Multiplicative constructor - * Build a vector from another one and a scale factor. - * The vector built will be a * u - * @param a scale factor - * @param u base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a, final Vector3DDS u) { - this.x = a.multiply(u.x); - this.y = a.multiply(u.y); - this.z = a.multiply(u.z); - } - - /** Multiplicative constructor - * Build a vector from another one and a scale factor. - * The vector built will be a * u - * @param a scale factor - * @param u base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a, final Vector3D u) { - this.x = a.multiply(u.getX()); - this.y = a.multiply(u.getY()); - this.z = a.multiply(u.getZ()); - } - - /** Multiplicative constructor - * Build a vector from another one and a scale factor. - * The vector built will be a * u - * @param a scale factor - * @param u base (unscaled) vector - */ - public Vector3DDS(final double a, final Vector3DDS u) { - this.x = u.x.multiply(a); - this.y = u.y.multiply(a); - this.z = u.z.multiply(a); - } - - /** Linear constructor - * Build a vector from two other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1, - final DerivativeStructure a2, final Vector3DDS u2) { - this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)); - this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)); - this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)); - } - - /** Linear constructor - * Build a vector from two other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3D u1, - final DerivativeStructure a2, final Vector3D u2) { - this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())); - this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())); - this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())); - } - - /** Linear constructor - * Build a vector from two other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - */ - public Vector3DDS(final double a1, final Vector3DDS u1, - final double a2, final Vector3DDS u2) { - this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)); - this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)); - this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)); - } - - /** Linear constructor - * Build a vector from three other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1, - final DerivativeStructure a2, final Vector3DDS u2, - final DerivativeStructure a3, final Vector3DDS u3) { - this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x)); - this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y)); - this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z)); - } - - /** Linear constructor - * Build a vector from three other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3D u1, - final DerivativeStructure a2, final Vector3D u2, - final DerivativeStructure a3, final Vector3D u3) { - this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX())); - this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY())); - this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ())); - } - - /** Linear constructor - * Build a vector from three other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - */ - public Vector3DDS(final double a1, final Vector3DDS u1, - final double a2, final Vector3DDS u2, - final double a3, final Vector3DDS u3) { - this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3)); - this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3)); - this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3)); - } - - /** Linear constructor - * Build a vector from four other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - * @param a4 fourth scale factor - * @param u4 fourth base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1, - final DerivativeStructure a2, final Vector3DDS u2, - final DerivativeStructure a3, final Vector3DDS u3, - final DerivativeStructure a4, final Vector3DDS u4) { - this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x)).add(a4.multiply(u4.x)); - this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y)).add(a4.multiply(u4.y)); - this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z)).add(a4.multiply(u4.z)); - } - - /** Linear constructor - * Build a vector from four other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - * @param a4 fourth scale factor - * @param u4 fourth base (unscaled) vector - */ - public Vector3DDS(final DerivativeStructure a1, final Vector3D u1, - final DerivativeStructure a2, final Vector3D u2, - final DerivativeStructure a3, final Vector3D u3, - final DerivativeStructure a4, final Vector3D u4) { - this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX())).add(a4.multiply(u4.getX())); - this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY())).add(a4.multiply(u4.getY())); - this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ())).add(a4.multiply(u4.getZ())); - } - - /** Linear constructor - * Build a vector from four other ones and corresponding scale factors. - * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 - * @param a1 first scale factor - * @param u1 first base (unscaled) vector - * @param a2 second scale factor - * @param u2 second base (unscaled) vector - * @param a3 third scale factor - * @param u3 third base (unscaled) vector - * @param a4 fourth scale factor - * @param u4 fourth base (unscaled) vector - */ - public Vector3DDS(final double a1, final Vector3DDS u1, - final double a2, final Vector3DDS u2, - final double a3, final Vector3DDS u3, - final double a4, final Vector3DDS u4) { - this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3)).add(u4.x.multiply(a4)); - this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3)).add(u4.y.multiply(a4)); - this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3)).add(u4.z.multiply(a4)); - } - - /** Get the abscissa of the vector. - * @return abscissa of the vector - * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure) - */ - public DerivativeStructure getX() { - return x; - } - - /** Get the ordinate of the vector. - * @return ordinate of the vector - * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure) - */ - public DerivativeStructure getY() { - return y; - } - - /** Get the height of the vector. - * @return height of the vector - * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure) - */ - public DerivativeStructure getZ() { - return z; - } - - /** Get the vector coordinates as a dimension 3 array. - * @return vector coordinates - * @see #Vector3D(DerivativeStructure[]) - */ - public DerivativeStructure[] toArray() { - return new DerivativeStructure[] { x, y, z }; - } - - /** Convert to a constant vector without derivatives. - * @return a constant vector - */ - public Vector3D toVector3D() { - return new Vector3D(x.getValue(), y.getValue(), z.getValue()); - } - - /** Get the L1 norm for the vector. - * @return L1 norm for the vector - */ - public DerivativeStructure getNorm1() { - return x.abs().add(y.abs()).add(z.abs()); - } - - /** Get the L2 norm for the vector. - * @return Euclidean norm for the vector - */ - public DerivativeStructure getNorm() { - // there are no cancellation problems here, so we use the straightforward formula - return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)).sqrt(); - } - - /** Get the square of the norm for the vector. - * @return square of the Euclidean norm for the vector - */ - public DerivativeStructure getNormSq() { - // there are no cancellation problems here, so we use the straightforward formula - return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)); - } - - /** Get the L∞ norm for the vector. - * @return L∞ norm for the vector - */ - public DerivativeStructure getNormInf() { - final DerivativeStructure xAbs = x.abs(); - final DerivativeStructure yAbs = y.abs(); - final DerivativeStructure zAbs = z.abs(); - if (xAbs.getValue() <= yAbs.getValue()) { - if (yAbs.getValue() <= zAbs.getValue()) { - return zAbs; - } else { - return yAbs; - } - } else { - if (xAbs.getValue() <= zAbs.getValue()) { - return zAbs; - } else { - return xAbs; - } - } - } - - /** Get the azimuth of the vector. - * @return azimuth (α) of the vector, between -π and +π - * @see #Vector3D(DerivativeStructure, DerivativeStructure) - */ - public DerivativeStructure getAlpha() { - return DerivativeStructure.atan2(y, x); - } - - /** Get the elevation of the vector. - * @return elevation (δ) of the vector, between -π/2 and +π/2 - * @see #Vector3D(DerivativeStructure, DerivativeStructure) - */ - public DerivativeStructure getDelta() { - return z.divide(getNorm()).asin(); - } - - /** Add a vector to the instance. - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final Vector3DDS v) { - return new Vector3DDS(x.add(v.x), y.add(v.y), z.add(v.z)); - } - - /** Add a vector to the instance. - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final Vector3D v) { - return new Vector3DDS(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ())); - } - - /** Add a scaled vector to the instance. - * @param factor scale factor to apply to v before adding it - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final DerivativeStructure factor, final Vector3DDS v) { - return new Vector3DDS(x.add(factor.multiply(v.x)), - y.add(factor.multiply(v.y)), - z.add(factor.multiply(v.z))); - } - - /** Add a scaled vector to the instance. - * @param factor scale factor to apply to v before adding it - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final DerivativeStructure factor, final Vector3D v) { - return new Vector3DDS(x.add(factor.multiply(v.getX())), - y.add(factor.multiply(v.getY())), - z.add(factor.multiply(v.getZ()))); - } - - /** Add a scaled vector to the instance. - * @param factor scale factor to apply to v before adding it - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final double factor, final Vector3DDS v) { - return new Vector3DDS(x.add(v.x.multiply(factor)), - y.add(v.y.multiply(factor)), - z.add(v.z.multiply(factor))); - } - - /** Add a scaled vector to the instance. - * @param factor scale factor to apply to v before adding it - * @param v vector to add - * @return a new vector - */ - public Vector3DDS add(final double factor, final Vector3D v) { - return new Vector3DDS(x.add(factor * v.getX()), - y.add(factor * v.getY()), - z.add(factor * v.getZ())); - } - - /** Subtract a vector from the instance. - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final Vector3DDS v) { - return new Vector3DDS(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z)); - } - - /** Subtract a vector from the instance. - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final Vector3D v) { - return new Vector3DDS(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ())); - } - - /** Subtract a scaled vector from the instance. - * @param factor scale factor to apply to v before subtracting it - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final DerivativeStructure factor, final Vector3DDS v) { - return new Vector3DDS(x.subtract(factor.multiply(v.x)), - y.subtract(factor.multiply(v.y)), - z.subtract(factor.multiply(v.z))); - } - - /** Subtract a scaled vector from the instance. - * @param factor scale factor to apply to v before subtracting it - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final DerivativeStructure factor, final Vector3D v) { - return new Vector3DDS(x.subtract(factor.multiply(v.getX())), - y.subtract(factor.multiply(v.getY())), - z.subtract(factor.multiply(v.getZ()))); - } - - /** Subtract a scaled vector from the instance. - * @param factor scale factor to apply to v before subtracting it - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final double factor, final Vector3DDS v) { - return new Vector3DDS(x.subtract(v.x.multiply(factor)), - y.subtract(v.y.multiply(factor)), - z.subtract(v.z.multiply(factor))); - } - - /** Subtract a scaled vector from the instance. - * @param factor scale factor to apply to v before subtracting it - * @param v vector to subtract - * @return a new vector - */ - public Vector3DDS subtract(final double factor, final Vector3D v) { - return new Vector3DDS(x.subtract(factor * v.getX()), - y.subtract(factor * v.getY()), - z.subtract(factor * v.getZ())); - } - - /** Get a normalized vector aligned with the instance. - * @return a new normalized vector - * @exception MathArithmeticException if the norm is zero - */ - public Vector3DDS normalize() throws MathArithmeticException { - final DerivativeStructure s = getNorm(); - if (s.getValue() == 0) { - throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); - } - return scalarMultiply(s.reciprocal()); - } - - /** Get a vector orthogonal to the instance. - *

There are an infinite number of normalized vectors orthogonal - * to the instance. This method picks up one of them almost - * arbitrarily. It is useful when one needs to compute a reference - * frame with one of the axes in a predefined direction. The - * following example shows how to build a frame having the k axis - * aligned with the known vector u : - * 


-     *   Vector3D k = u.normalize();
-     *   Vector3D i = k.orthogonal();
-     *   Vector3D j = Vector3D.crossProduct(k, i);
-     *

- * @return a new normalized vector orthogonal to the instance - * @exception MathArithmeticException if the norm of the instance is null - */ - public Vector3DDS orthogonal() throws MathArithmeticException { - - final double threshold = 0.6 * getNorm().getValue(); - if (threshold == 0) { - throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); - } - - if (FastMath.abs(x.getValue()) <= threshold) { - final DerivativeStructure inverse = y.multiply(y).add(z.multiply(z)).sqrt().reciprocal(); - return new Vector3DDS(inverse.getField().getZero(), inverse.multiply(z), inverse.multiply(y).negate()); - } else if (FastMath.abs(y.getValue()) <= threshold) { - final DerivativeStructure inverse = x.multiply(x).add(z.multiply(z)).sqrt().reciprocal(); - return new Vector3DDS(inverse.multiply(z).negate(), inverse.getField().getZero(), inverse.multiply(x)); - } else { - final DerivativeStructure inverse = x.multiply(x).add(y.multiply(y)).sqrt().reciprocal(); - return new Vector3DDS(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero()); - } - - } - - /** Compute the angular separation between two vectors. - *

This method computes the angular separation between two - * vectors using the dot product for well separated vectors and the - * cross product for almost aligned vectors. This allows to have a - * good accuracy in all cases, even for vectors very close to each - * other.

- * @param v1 first vector - * @param v2 second vector - * @return angular separation between v1 and v2 - * @exception MathArithmeticException if either vector has a null norm - */ - public static DerivativeStructure angle(Vector3DDS v1, Vector3DDS v2) throws MathArithmeticException { - - final DerivativeStructure normProduct = v1.getNorm().multiply(v2.getNorm()); - if (normProduct.getValue() == 0) { - throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); - } - - final DerivativeStructure dot = v1.dotProduct(v2); - final double threshold = normProduct.getValue() * 0.9999; - if ((dot.getValue() < -threshold) || (dot.getValue() > threshold)) { - // the vectors are almost aligned, compute using the sine - Vector3DDS v3 = crossProduct(v1, v2); - if (dot.getValue() >= 0) { - return v3.getNorm().divide(normProduct).asin(); - } - return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); - } - - // the vectors are sufficiently separated to use the cosine - return dot.divide(normProduct).acos(); - - } - - /** Get the opposite of the instance. - * @return a new vector which is opposite to the instance - */ - public Vector3DDS negate() { - return new Vector3DDS(x.negate(), y.negate(), z.negate()); - } - - /** Multiply the instance by a scalar. - * @param a scalar - * @return a new vector - */ - public Vector3DDS scalarMultiply(final DerivativeStructure a) { - return new Vector3DDS(x.multiply(a), y.multiply(a), z.multiply(a)); - } - - /** Multiply the instance by a scalar. - * @param a scalar - * @return a new vector - */ - public Vector3DDS scalarMultiply(final double a) { - return new Vector3DDS(x.multiply(a), y.multiply(a), z.multiply(a)); - } - - /** - * Returns true if any coordinate of this vector is NaN; false otherwise - * @return true if any coordinate of this vector is NaN; false otherwise - */ - public boolean isNaN() { - return Double.isNaN(x.getValue()) || Double.isNaN(y.getValue()) || Double.isNaN(z.getValue()); - } - - /** - * Returns true if any coordinate of this vector is infinite and none are NaN; - * false otherwise - * @return true if any coordinate of this vector is infinite and none are NaN; - * false otherwise - */ - public boolean isInfinite() { - return !isNaN() && (Double.isInfinite(x.getValue()) || Double.isInfinite(y.getValue()) || Double.isInfinite(z.getValue())); - } - - /** - * Test for the equality of two 3D vectors. - *

- * If all coordinates of two 3D vectors are exactly the same, and none are - * DerivativeStructure.NaN, the two 3D vectors are considered to be equal. - *

- *

- * NaN coordinates are considered to affect globally the vector - * and be equals to each other - i.e, if either (or all) coordinates of the - * 3D vector are equal to DerivativeStructure.NaN, the 3D vector is equal to - * {@link #NaN}. - *

- * - * @param other Object to test for equality to this - * @return true if two 3D vector objects are equal, false if - * object is null, not an instance of Vector3D, or - * not equal to this Vector3D instance - * - */ - @Override - public boolean equals(Object other) { - - if (this == other) { - return true; - } - - if (other instanceof Vector3DDS) { - final Vector3DDS rhs = (Vector3DDS)other; - if (rhs.isNaN()) { - return this.isNaN(); - } - - return MathArrays.equals(x.getAllDerivatives(), rhs.x.getAllDerivatives()) && - MathArrays.equals(y.getAllDerivatives(), rhs.y.getAllDerivatives()) && - MathArrays.equals(z.getAllDerivatives(), rhs.z.getAllDerivatives()); - - } - return false; - } - - /** - * Get a hashCode for the 3D vector. - *

- * All NaN values have the same hash code.

- * - * @return a hash code value for this object - */ - @Override - public int hashCode() { - if (isNaN()) { - return 409; - } - return 311 * (107 * x.hashCode() + 83 * y.hashCode() + z.hashCode()); - } - - /** Compute the dot-product of the instance and another vector. - *

- * 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 MathArrays#linearCombination(double, double, double, double, double, double) - * @param v second vector - * @return the dot product this.v - */ - public DerivativeStructure dotProduct(final Vector3DDS v) { - return MathArrays.linearCombination(x, v.x, y, v.y, z, v.z); - } - - /** Compute the dot-product of the instance and another vector. - *

- * 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 MathArrays#linearCombination(double, double, double, double, double, double) - * @param v second vector - * @return the dot product this.v - */ - public DerivativeStructure dotProduct(final Vector3D v) { - return MathArrays.linearCombination(v.getX(), x, v.getY(), y, v.getZ(), z); - } - - /** Compute the cross-product of the instance with another vector. - * @param v other vector - * @return the cross product this ^ v as a new Vector3D - */ - public Vector3DDS crossProduct(final Vector3DDS v) { - return new Vector3DDS(MathArrays.linearCombination(y, v.z, z.negate(), v.y), - MathArrays.linearCombination(z, v.x, x.negate(), v.z), - MathArrays.linearCombination(x, v.y, y.negate(), v.x)); - } - - /** Compute the cross-product of the instance with another vector. - * @param v other vector - * @return the cross product this ^ v as a new Vector3D - */ - public Vector3DDS crossProduct(final Vector3D v) { - return new Vector3DDS(MathArrays.linearCombination(v.getZ(), y, v.getY(), z.negate()), - MathArrays.linearCombination(v.getX(), z, v.getZ(), x.negate()), - MathArrays.linearCombination(v.getY(), x, v.getX(), y.negate())); - } - - /** Compute the distance between the instance and another vector according to the L1 norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNorm1() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L1 norm - */ - public DerivativeStructure distance1(final Vector3DDS v) { - final DerivativeStructure dx = v.x.subtract(x).abs(); - final DerivativeStructure dy = v.y.subtract(y).abs(); - final DerivativeStructure dz = v.z.subtract(z).abs(); - return dx.add(dy).add(dz); - } - - /** Compute the distance between the instance and another vector according to the L1 norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNorm1() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L1 norm - */ - public DerivativeStructure distance1(final Vector3D v) { - final DerivativeStructure dx = x.subtract(v.getX()).abs(); - final DerivativeStructure dy = y.subtract(v.getY()).abs(); - final DerivativeStructure dz = z.subtract(v.getZ()).abs(); - return dx.add(dy).add(dz); - } - - /** Compute the distance between the instance and another vector according to the L2 norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNorm() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L2 norm - */ - public DerivativeStructure distance(final Vector3DDS v) { - final DerivativeStructure dx = v.x.subtract(x); - final DerivativeStructure dy = v.y.subtract(y); - final DerivativeStructure dz = v.z.subtract(z); - return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); - } - - /** Compute the distance between the instance and another vector according to the L2 norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNorm() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L2 norm - */ - public DerivativeStructure distance(final Vector3D v) { - final DerivativeStructure dx = x.subtract(v.getX()); - final DerivativeStructure dy = y.subtract(v.getY()); - final DerivativeStructure dz = z.subtract(v.getZ()); - return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); - } - - /** Compute the distance between the instance and another vector according to the L∞ norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNormInf() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L∞ norm - */ - public DerivativeStructure distanceInf(final Vector3DDS v) { - final DerivativeStructure dx = v.x.subtract(x).abs(); - final DerivativeStructure dy = v.y.subtract(y).abs(); - final DerivativeStructure dz = v.z.subtract(z).abs(); - if (dx.getValue() <= dy.getValue()) { - if (dy.getValue() <= dz.getValue()) { - return dz; - } else { - return dy; - } - } else { - if (dx.getValue() <= dz.getValue()) { - return dz; - } else { - return dx; - } - } - } - - /** Compute the distance between the instance and another vector according to the L∞ norm. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNormInf() except that no intermediate - * vector is built

- * @param v second vector - * @return the distance between the instance and p according to the L∞ norm - */ - public DerivativeStructure distanceInf(final Vector3D v) { - final DerivativeStructure dx = x.subtract(v.getX()).abs(); - final DerivativeStructure dy = y.subtract(v.getY()).abs(); - final DerivativeStructure dz = z.subtract(v.getZ()).abs(); - if (dx.getValue() <= dy.getValue()) { - if (dy.getValue() <= dz.getValue()) { - return dz; - } else { - return dy; - } - } else { - if (dx.getValue() <= dz.getValue()) { - return dz; - } else { - return dx; - } - } - } - - /** Compute the square of the distance between the instance and another vector. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNormSq() except that no intermediate - * vector is built

- * @param v second vector - * @return the square of the distance between the instance and p - */ - public DerivativeStructure distanceSq(final Vector3DDS v) { - final DerivativeStructure dx = v.x.subtract(x); - final DerivativeStructure dy = v.y.subtract(y); - final DerivativeStructure dz = v.z.subtract(z); - return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); - } - - /** Compute the square of the distance between the instance and another vector. - *

Calling this method is equivalent to calling: - * q.subtract(p).getNormSq() except that no intermediate - * vector is built

- * @param v second vector - * @return the square of the distance between the instance and p - */ - public DerivativeStructure distanceSq(final Vector3D v) { - final DerivativeStructure dx = x.subtract(v.getX()); - final DerivativeStructure dy = y.subtract(v.getY()); - final DerivativeStructure dz = z.subtract(v.getZ()); - return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); - } - - /** Compute the dot-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the dot product v1.v2 - */ - public static DerivativeStructure dotProduct(Vector3DDS v1, Vector3DDS v2) { - return v1.dotProduct(v2); - } - - /** Compute the dot-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the dot product v1.v2 - */ - public static DerivativeStructure dotProduct(Vector3DDS v1, Vector3D v2) { - return v1.dotProduct(v2); - } - - /** Compute the dot-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the dot product v1.v2 - */ - public static DerivativeStructure dotProduct(Vector3D v1, Vector3DDS v2) { - return v2.dotProduct(v1); - } - - /** Compute the cross-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the cross product v1 ^ v2 as a new Vector - */ - public static Vector3DDS crossProduct(final Vector3DDS v1, final Vector3DDS v2) { - return v1.crossProduct(v2); - } - - /** Compute the cross-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the cross product v1 ^ v2 as a new Vector - */ - public static Vector3DDS crossProduct(final Vector3DDS v1, final Vector3D v2) { - return v1.crossProduct(v2); - } - - /** Compute the cross-product of two vectors. - * @param v1 first vector - * @param v2 second vector - * @return the cross product v1 ^ v2 as a new Vector - */ - public static Vector3DDS crossProduct(final Vector3D v1, final Vector3DDS v2) { - return v2.crossProduct(v1).negate(); - } - - /** Compute the distance between two vectors according to the L1 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm1() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L1 norm - */ - public static DerivativeStructure distance1(Vector3DDS v1, Vector3DDS v2) { - return v1.distance1(v2); - } - - /** Compute the distance between two vectors according to the L1 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm1() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L1 norm - */ - public static DerivativeStructure distance1(Vector3DDS v1, Vector3D v2) { - return v1.distance1(v2); - } - - /** Compute the distance between two vectors according to the L1 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm1() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L1 norm - */ - public static DerivativeStructure distance1(Vector3D v1, Vector3DDS v2) { - return v2.distance1(v1); - } - - /** Compute the distance between two vectors according to the L2 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L2 norm - */ - public static DerivativeStructure distance(Vector3DDS v1, Vector3DDS v2) { - return v1.distance(v2); - } - - /** Compute the distance between two vectors according to the L2 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L2 norm - */ - public static DerivativeStructure distance(Vector3DDS v1, Vector3D v2) { - return v1.distance(v2); - } - - /** Compute the distance between two vectors according to the L2 norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNorm() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L2 norm - */ - public static DerivativeStructure distance(Vector3D v1, Vector3DDS v2) { - return v2.distance(v1); - } - - /** Compute the distance between two vectors according to the L∞ norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormInf() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L∞ norm - */ - public static DerivativeStructure distanceInf(Vector3DDS v1, Vector3DDS v2) { - return v1.distanceInf(v2); - } - - /** Compute the distance between two vectors according to the L∞ norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormInf() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L∞ norm - */ - public static DerivativeStructure distanceInf(Vector3DDS v1, Vector3D v2) { - return v1.distanceInf(v2); - } - - /** Compute the distance between two vectors according to the L∞ norm. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormInf() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the distance between v1 and v2 according to the L∞ norm - */ - public static DerivativeStructure distanceInf(Vector3D v1, Vector3DDS v2) { - return v2.distanceInf(v1); - } - - /** Compute the square of the distance between two vectors. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormSq() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the square of the distance between v1 and v2 - */ - public static DerivativeStructure distanceSq(Vector3DDS v1, Vector3DDS v2) { - return v1.distanceSq(v2); - } - - /** Compute the square of the distance between two vectors. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormSq() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the square of the distance between v1 and v2 - */ - public static DerivativeStructure distanceSq(Vector3DDS v1, Vector3D v2) { - return v1.distanceSq(v2); - } - - /** Compute the square of the distance between two vectors. - *

Calling this method is equivalent to calling: - * v1.subtract(v2).getNormSq() except that no intermediate - * vector is built

- * @param v1 first vector - * @param v2 second vector - * @return the square of the distance between v1 and v2 - */ - public static DerivativeStructure distanceSq(Vector3D v1, Vector3DDS v2) { - return v2.distanceSq(v1); - } - - /** Get a string representation of this vector. - * @return a string representation of this vector - */ - @Override - public String toString() { - return Vector3DFormat.getInstance().format(toVector3D()); - } - - /** {@inheritDoc} */ - public String toString(final NumberFormat format) { - return new Vector3DFormat(format).format(toVector3D()); - } - -} diff --git a/src/main/java/org/apache/commons/math3/linear/SparseFieldVector.java b/src/main/java/org/apache/commons/math3/linear/SparseFieldVector.java index ab4489d62..1f5d6e74d 100644 --- a/src/main/java/org/apache/commons/math3/linear/SparseFieldVector.java +++ b/src/main/java/org/apache/commons/math3/linear/SparseFieldVector.java @@ -17,16 +17,16 @@ package org.apache.commons.math3.linear; import java.io.Serializable; -import java.lang.reflect.Array; import org.apache.commons.math3.Field; import org.apache.commons.math3.FieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.MathArithmeticException; import org.apache.commons.math3.exception.NotPositiveException; import org.apache.commons.math3.exception.NullArgumentException; import org.apache.commons.math3.exception.OutOfRangeException; -import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.util.MathArrays; import org.apache.commons.math3.util.OpenIntToFieldHashMap; /** @@ -479,7 +479,7 @@ public class SparseFieldVector> implements FieldVector /** {@inheritDoc} */ public T[] toArray() { - T[] res = buildArray(virtualSize); + T[] res = MathArrays.buildArray(field, virtualSize); OpenIntToFieldHashMap.Iterator iter = entries.iterator(); while (iter.hasNext()) { iter.advance(); @@ -529,18 +529,6 @@ public class SparseFieldVector> implements FieldVector } } - /** - * Build an array of elements. - * - * @param length Size of the array to build. - * @return a new array. - */ - @SuppressWarnings("unchecked") // field is type T - private T[] buildArray(final int length) { - return (T[]) Array.newInstance(field.getRuntimeClass(), length); - } - - /** {@inheritDoc} */ @Override public int hashCode() { diff --git a/src/main/java/org/apache/commons/math3/util/Decimal64.java b/src/main/java/org/apache/commons/math3/util/Decimal64.java index 02048ae7c..724d410c6 100644 --- a/src/main/java/org/apache/commons/math3/util/Decimal64.java +++ b/src/main/java/org/apache/commons/math3/util/Decimal64.java @@ -16,19 +16,20 @@ */ package org.apache.commons.math3.util; +import org.apache.commons.math3.ExtendedFieldElement; import org.apache.commons.math3.Field; -import org.apache.commons.math3.FieldElement; +import org.apache.commons.math3.exception.DimensionMismatchException; /** * This class wraps a {@code double} value in an object. It is similar to the * standard class {@link Double}, while also implementing the - * {@link FieldElement} interface. + * {@link ExtendedFieldElement} interface. * * @since 3.1 * @version $Id$ */ -public class Decimal64 extends Number implements FieldElement, -Comparable { +public class Decimal64 extends Number + implements ExtendedFieldElement, Comparable { /** The constant value of {@code 0d} as a {@code Decimal64}. */ public static final Decimal64 ZERO; @@ -301,4 +302,287 @@ Comparable { public boolean isNaN() { return Double.isNaN(value); } + + /** {@inheritDoc} */ + public double getReal() { + return value; + } + + /** {@inheritDoc} */ + public Decimal64 add(final double a) { + return new Decimal64(value + a); + } + + /** {@inheritDoc} */ + public Decimal64 subtract(final double a) { + return new Decimal64(value - a); + } + + /** {@inheritDoc} */ + public Decimal64 multiply(final double a) { + return new Decimal64(value * a); + } + + /** {@inheritDoc} */ + public Decimal64 divide(final double a) { + return new Decimal64(value / a); + } + + /** {@inheritDoc} */ + public Decimal64 remainder(final double a) { + return new Decimal64(value % a); + } + + /** {@inheritDoc} */ + public Decimal64 remainder(final Decimal64 a) { + return new Decimal64(value % a.value); + } + + /** {@inheritDoc} */ + public Decimal64 abs() { + return new Decimal64(FastMath.abs(value)); + } + + /** {@inheritDoc} */ + public Decimal64 ceil() { + return new Decimal64(FastMath.ceil(value)); + } + + /** {@inheritDoc} */ + public Decimal64 floor() { + return new Decimal64(FastMath.floor(value)); + } + + /** {@inheritDoc} */ + public Decimal64 rint() { + return new Decimal64(FastMath.rint(value)); + } + + /** {@inheritDoc} */ + public long round() { + return FastMath.round(value); + } + + /** {@inheritDoc} */ + public Decimal64 signum() { + return new Decimal64(FastMath.signum(value)); + } + + /** {@inheritDoc} */ + public Decimal64 copySign(final double sign) { + return new Decimal64(FastMath.copySign(value, sign)); + } + + /** {@inheritDoc} */ + public Decimal64 scalb(final int n) { + return new Decimal64(FastMath.scalb(value, n)); + } + + /** {@inheritDoc} */ + public Decimal64 hypot(final Decimal64 y) { + return new Decimal64(FastMath.hypot(value, y.value)); + } + + /** {@inheritDoc} */ + public Decimal64 sqrt() { + return new Decimal64(FastMath.sqrt(value)); + } + + /** {@inheritDoc} */ + public Decimal64 cbrt() { + return new Decimal64(FastMath.cbrt(value)); + } + + /** {@inheritDoc} */ + public Decimal64 rootN(final int n) { + return new Decimal64(FastMath.pow(value, 1.0 / n)); + } + + /** {@inheritDoc} */ + public Decimal64 pow(final double p) { + return new Decimal64(FastMath.pow(value, p)); + } + + /** {@inheritDoc} */ + public Decimal64 pow(final int n) { + return new Decimal64(FastMath.pow(value, n)); + } + + /** {@inheritDoc} */ + public Decimal64 pow(final Decimal64 e) { + return new Decimal64(FastMath.pow(value, e.value)); + } + + /** {@inheritDoc} */ + public Decimal64 exp() { + return new Decimal64(FastMath.exp(value)); + } + + /** {@inheritDoc} */ + public Decimal64 expm1() { + return new Decimal64(FastMath.expm1(value)); + } + + /** {@inheritDoc} */ + public Decimal64 log() { + return new Decimal64(FastMath.log(value)); + } + + /** {@inheritDoc} */ + public Decimal64 log1p() { + return new Decimal64(FastMath.log1p(value)); + } + + /** {@inheritDoc} */ + public Decimal64 log10() { + return new Decimal64(FastMath.log10(value)); + } + + /** {@inheritDoc} */ + public Decimal64 cos() { + return new Decimal64(FastMath.cos(value)); + } + + /** {@inheritDoc} */ + public Decimal64 sin() { + return new Decimal64(FastMath.sin(value)); + } + + /** {@inheritDoc} */ + public Decimal64 tan() { + return new Decimal64(FastMath.tan(value)); + } + + /** {@inheritDoc} */ + public Decimal64 acos() { + return new Decimal64(FastMath.acos(value)); + } + + /** {@inheritDoc} */ + public Decimal64 asin() { + return new Decimal64(FastMath.asin(value)); + } + + /** {@inheritDoc} */ + public Decimal64 atan() { + return new Decimal64(FastMath.atan(value)); + } + + /** {@inheritDoc} */ + public Decimal64 atan2(final Decimal64 x) { + return new Decimal64(FastMath.atan2(value, x.value)); + } + + /** {@inheritDoc} */ + public Decimal64 cosh() { + return new Decimal64(FastMath.cosh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 sinh() { + return new Decimal64(FastMath.sinh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 tanh() { + return new Decimal64(FastMath.tanh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 acosh() { + return new Decimal64(FastMath.acosh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 asinh() { + return new Decimal64(FastMath.asinh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 atanh() { + return new Decimal64(FastMath.atanh(value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final Decimal64[] a, final Decimal64[] b) + throws DimensionMismatchException { + if (a.length != b.length) { + throw new DimensionMismatchException(a.length, b.length); + } + final double[] aDouble = new double[a.length]; + final double[] bDouble = new double[b.length]; + for (int i = 0; i < a.length; ++i) { + aDouble[i] = a[i].value; + bDouble[i] = b[i].value; + } + return new Decimal64(MathArrays.linearCombination(aDouble, bDouble)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final double[] a, final Decimal64[] b) + throws DimensionMismatchException { + if (a.length != b.length) { + throw new DimensionMismatchException(a.length, b.length); + } + final double[] bDouble = new double[b.length]; + for (int i = 0; i < a.length; ++i) { + bDouble[i] = b[i].value; + } + return new Decimal64(MathArrays.linearCombination(a, bDouble)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final Decimal64 a1, final Decimal64 b1, + final Decimal64 a2, final Decimal64 b2) { + return new Decimal64(MathArrays.linearCombination(a1.value, b1.value, + a2.value, b2.value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final double a1, final Decimal64 b1, + final double a2, final Decimal64 b2) { + return new Decimal64(MathArrays.linearCombination(a1, b1.value, + a2, b2.value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final Decimal64 a1, final Decimal64 b1, + final Decimal64 a2, final Decimal64 b2, + final Decimal64 a3, final Decimal64 b3) { + return new Decimal64(MathArrays.linearCombination(a1.value, b1.value, + a2.value, b2.value, + a3.value, b3.value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final double a1, final Decimal64 b1, + final double a2, final Decimal64 b2, + final double a3, final Decimal64 b3) { + return new Decimal64(MathArrays.linearCombination(a1, b1.value, + a2, b2.value, + a3, b3.value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final Decimal64 a1, final Decimal64 b1, + final Decimal64 a2, final Decimal64 b2, + final Decimal64 a3, final Decimal64 b3, + final Decimal64 a4, final Decimal64 b4) { + return new Decimal64(MathArrays.linearCombination(a1.value, b1.value, + a2.value, b2.value, + a3.value, b3.value, + a4.value, b4.value)); + } + + /** {@inheritDoc} */ + public Decimal64 linearCombination(final double a1, final Decimal64 b1, + final double a2, final Decimal64 b2, + final double a3, final Decimal64 b3, + final double a4, final Decimal64 b4) { + return new Decimal64(MathArrays.linearCombination(a1, b1.value, + a2, b2.value, + a3, b3.value, + a4, b4.value)); + } + } diff --git a/src/main/java/org/apache/commons/math3/util/MathArrays.java b/src/main/java/org/apache/commons/math3/util/MathArrays.java index 8eccae460..045a9cd86 100644 --- a/src/main/java/org/apache/commons/math3/util/MathArrays.java +++ b/src/main/java/org/apache/commons/math3/util/MathArrays.java @@ -17,21 +17,23 @@ package org.apache.commons.math3.util; -import java.util.List; +import java.lang.reflect.Array; import java.util.ArrayList; -import java.util.Comparator; +import java.util.Arrays; import java.util.Collections; +import java.util.Comparator; +import java.util.List; -import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; +import org.apache.commons.math3.Field; import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; import org.apache.commons.math3.exception.MathInternalError; import org.apache.commons.math3.exception.NonMonotonicSequenceException; import org.apache.commons.math3.exception.NotPositiveException; import org.apache.commons.math3.exception.NotStrictlyPositiveException; import org.apache.commons.math3.exception.NullArgumentException; -import org.apache.commons.math3.exception.MathIllegalArgumentException; import org.apache.commons.math3.exception.util.LocalizedFormats; -import org.apache.commons.math3.exception.MathArithmeticException; /** * Arrays utilities. @@ -1118,353 +1120,6 @@ public class MathArrays { return result; } - /** - * Compute a linear combination accurately. - * This method computes the sum of the products - * ai bi to high accuracy. - * It does so by using specific multiplication and addition algorithms to - * preserve accuracy and reduce cancellation effects. - *
- * It is based on the 2005 paper - * - * Accurate Sum and Dot Product by Takeshi Ogita, Siegfried M. Rump, - * and Shin'ichi Oishi published in SIAM J. Sci. Comput. - * - * @param a Factors. - * @param b Factors. - * @return Σi ai bi. - * @throws DimensionMismatchException if arrays dimensions don't match - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final DerivativeStructure[] a, final DerivativeStructure[] b) - throws DimensionMismatchException { - - // compute an accurate value, taking care of cancellations - final double[] aDouble = new double[a.length]; - for (int i = 0; i < a.length; ++i) { - aDouble[i] = a[i].getValue(); - } - final double[] bDouble = new double[b.length]; - for (int i = 0; i < b.length; ++i) { - bDouble[i] = b[i].getValue(); - } - final double accurateValue = MathArrays.linearCombination(aDouble, bDouble); - - // compute a simple value, with all partial derivatives - DerivativeStructure simpleValue = a[0].getField().getZero(); - for (int i = 0; i < a.length; ++i) { - simpleValue = simpleValue.add(a[i].multiply(b[i])); - } - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - * This method computes the sum of the products - * ai bi to high accuracy. - * It does so by using specific multiplication and addition algorithms to - * preserve accuracy and reduce cancellation effects. - *
- * It is based on the 2005 paper - * - * Accurate Sum and Dot Product by Takeshi Ogita, Siegfried M. Rump, - * and Shin'ichi Oishi published in SIAM J. Sci. Comput. - * - * @param a Factors. - * @param b Factors. - * @return Σi ai bi. - * @throws DimensionMismatchException if arrays dimensions don't match - */ - public static DerivativeStructure linearCombination(final double[] a, final DerivativeStructure[] b) - throws DimensionMismatchException { - - // compute an accurate value, taking care of cancellations - final double[] bDouble = new double[b.length]; - for (int i = 0; i < b.length; ++i) { - bDouble[i] = b[i].getValue(); - } - final double accurateValue = MathArrays.linearCombination(a, bDouble); - - // compute a simple value, with all partial derivatives - DerivativeStructure simpleValue = b[0].getField().getZero(); - for (int i = 0; i < a.length; ++i) { - simpleValue = simpleValue.add(b[i].multiply(a[i])); - } - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @return a1×b1 + - * a2×b2 - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, - final DerivativeStructure a2, final DerivativeStructure b2) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), - a2.getValue(), b2.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @return a1×b1 + - * a2×b2 - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure) - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, - final double a2, final DerivativeStructure b2) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), - a2, b2.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 + a3×b3 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @param a3 first factor of the third term - * @param b3 second factor of the third term - * @return a1×b1 + - * a2×b2 + a3×b3 - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, - final DerivativeStructure a2, final DerivativeStructure b2, - final DerivativeStructure a3, final DerivativeStructure b3) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), - a2.getValue(), b2.getValue(), - a3.getValue(), b3.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 + a3×b3 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @param a3 first factor of the third term - * @param b3 second factor of the third term - * @return a1×b1 + - * a2×b2 + a3×b3 - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure) - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, - final double a2, final DerivativeStructure b2, - final double a3, final DerivativeStructure b3) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), - a2, b2.getValue(), - a3, b3.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 + a3×b3 + - * a4×b4 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @param a3 first factor of the third term - * @param b3 second factor of the third term - * @param a4 first factor of the third term - * @param b4 second factor of the third term - * @return a1×b1 + - * a2×b2 + a3×b3 + - * a4×b4 - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @see #linearCombination(DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final DerivativeStructure a1, final DerivativeStructure b1, - final DerivativeStructure a2, final DerivativeStructure b2, - final DerivativeStructure a3, final DerivativeStructure b3, - final DerivativeStructure a4, final DerivativeStructure b4) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1.getValue(), b1.getValue(), - a2.getValue(), b2.getValue(), - a3.getValue(), b3.getValue(), - a4.getValue(), b4.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = a1.multiply(b1).add(a2.multiply(b2)).add(a3.multiply(b3)).add(a4.multiply(b4)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - - /** - * Compute a linear combination accurately. - *

- * This method computes a1×b1 + - * a2×b2 + a3×b3 + - * a4×b4 - * to high accuracy. It does so by using specific multiplication and - * addition algorithms to preserve accuracy and reduce cancellation effects. - * It is based on the 2005 paper - * Accurate Sum and Dot Product by Takeshi Ogita, - * Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput. - *

- * @param a1 first factor of the first term - * @param b1 second factor of the first term - * @param a2 first factor of the second term - * @param b2 second factor of the second term - * @param a3 first factor of the third term - * @param b3 second factor of the third term - * @param a4 first factor of the third term - * @param b4 second factor of the third term - * @return a1×b1 + - * a2×b2 + a3×b3 + - * a4×b4 - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure) - * @see #linearCombination(double, DerivativeStructure, double, DerivativeStructure, double, DerivativeStructure) - * @since 3.2 - */ - public static DerivativeStructure linearCombination(final double a1, final DerivativeStructure b1, - final double a2, final DerivativeStructure b2, - final double a3, final DerivativeStructure b3, - final double a4, final DerivativeStructure b4) { - - // compute an accurate value, taking care of cancellations - final double accurateValue = MathArrays.linearCombination(a1, b1.getValue(), - a2, b2.getValue(), - a3, b3.getValue(), - a4, b4.getValue()); - - // compute a simple value, with all partial derivatives - final DerivativeStructure simpleValue = b1.multiply(a1).add(b2.multiply(a2)).add(b3.multiply(a3)).add(b4.multiply(a4)); - - // create a result with accurate value and all derivatives (not necessarily as accurate as the value) - final double[] data = simpleValue.getAllDerivatives(); - data[0] = accurateValue; - return new DerivativeStructure(simpleValue.getFreeParameters(), simpleValue.getOrder(), data); - - } - /** * Returns true iff both arguments are null or have same dimensions and all * their elements are equal as defined by @@ -1620,4 +1275,50 @@ public class MathArrays { } return out; } + + /** Build an array of elements. + *

+ * Arrays are filled with field.getZero() + *

+ * @param the type of the field elements + * @param field field to which array elements belong + * @param length of the array + * @return a new array + */ + public static T[] buildArray(final Field field, final int length) { + @SuppressWarnings("unchecked") // OK because field must be correct class + T[] array = (T[]) Array.newInstance(field.getRuntimeClass(), length); + Arrays.fill(array, field.getZero()); + return array; + } + + /** Build a double dimension array of elements. + *

+ * Arrays are filled with field.getZero() + *

+ * @param the type of the field elements + * @param field field to which array elements belong + * @param rows number of rows in the array + * @param columns number of columns (may be negative to build partial + * arrays in the same way new Field[rows][] works) + * @return a new array + */ + @SuppressWarnings("unchecked") + public static T[][] buildArray(final Field field, final int rows, final int columns) { + final T[][] array; + if (columns < 0) { + T[] dummyRow = buildArray(field, 0); + array = (T[][]) Array.newInstance(dummyRow.getClass(), rows); + } else { + array = (T[][]) Array.newInstance(field.getRuntimeClass(), + new int[] { + rows, columns + }); + for (int i = 0; i < rows; ++i) { + Arrays.fill(array[i], field.getZero()); + } + } + return array; + } + } diff --git a/src/test/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructureTest.java b/src/test/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructureTest.java index 17e400aec..e74153fca 100644 --- a/src/test/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructureTest.java +++ b/src/test/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructureTest.java @@ -24,6 +24,7 @@ import org.apache.commons.math3.TestUtils; import org.apache.commons.math3.analysis.polynomials.PolynomialFunction; import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.NumberIsTooLargeException; +import org.apache.commons.math3.random.Well1024a; import org.apache.commons.math3.util.ArithmeticUtils; import org.apache.commons.math3.util.FastMath; import org.junit.Assert; @@ -1267,6 +1268,147 @@ public class DerivativeStructureTest { TestUtils.assertEquals(derivatives, xRef.add(yRef.subtract(zRef)).getAllDerivatives(), 1.0e-15); } + @Test + public void testLinearCombination1DSDS() { + final DerivativeStructure[] a = new DerivativeStructure[] { + new DerivativeStructure(6, 1, 0, -1321008684645961.0 / 268435456.0), + new DerivativeStructure(6, 1, 1, -5774608829631843.0 / 268435456.0), + new DerivativeStructure(6, 1, 2, -7645843051051357.0 / 8589934592.0) + }; + final DerivativeStructure[] b = new DerivativeStructure[] { + new DerivativeStructure(6, 1, 3, -5712344449280879.0 / 2097152.0), + new DerivativeStructure(6, 1, 4, -4550117129121957.0 / 2097152.0), + new DerivativeStructure(6, 1, 5, 8846951984510141.0 / 131072.0) + }; + + final DerivativeStructure abSumInline = a[0].linearCombination(a[0], b[0], a[1], b[1], a[2], b[2]); + final DerivativeStructure abSumArray = a[0].linearCombination(a, b); + + Assert.assertEquals(abSumInline.getValue(), abSumArray.getValue(), 0); + Assert.assertEquals(-1.8551294182586248737720779899, abSumInline.getValue(), 1.0e-15); + Assert.assertEquals(b[0].getValue(), abSumInline.getPartialDerivative(1, 0, 0, 0, 0, 0), 1.0e-15); + Assert.assertEquals(b[1].getValue(), abSumInline.getPartialDerivative(0, 1, 0, 0, 0, 0), 1.0e-15); + Assert.assertEquals(b[2].getValue(), abSumInline.getPartialDerivative(0, 0, 1, 0, 0, 0), 1.0e-15); + Assert.assertEquals(a[0].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 1, 0, 0), 1.0e-15); + Assert.assertEquals(a[1].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 0, 1, 0), 1.0e-15); + Assert.assertEquals(a[2].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 0, 0, 1), 1.0e-15); + + } + + @Test + public void testLinearCombination1DoubleDS() { + final double[] a = new double[] { + -1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -7645843051051357.0 / 8589934592.0 + }; + final DerivativeStructure[] b = new DerivativeStructure[] { + new DerivativeStructure(3, 1, 0, -5712344449280879.0 / 2097152.0), + new DerivativeStructure(3, 1, 1, -4550117129121957.0 / 2097152.0), + new DerivativeStructure(3, 1, 2, 8846951984510141.0 / 131072.0) + }; + + final DerivativeStructure abSumInline = b[0].linearCombination(a[0], b[0], + a[1], b[1], + a[2], b[2]); + final DerivativeStructure abSumArray = b[0].linearCombination(a, b); + + Assert.assertEquals(abSumInline.getValue(), abSumArray.getValue(), 0); + Assert.assertEquals(-1.8551294182586248737720779899, abSumInline.getValue(), 1.0e-15); + Assert.assertEquals(a[0], abSumInline.getPartialDerivative(1, 0, 0), 1.0e-15); + Assert.assertEquals(a[1], abSumInline.getPartialDerivative(0, 1, 0), 1.0e-15); + Assert.assertEquals(a[2], abSumInline.getPartialDerivative(0, 0, 1), 1.0e-15); + + } + + @Test + public void testLinearCombination2DSDS() { + // 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(0xc6af886975069f11l); + + for (int i = 0; i < 10000; ++i) { + final DerivativeStructure[] u = new DerivativeStructure[4]; + final DerivativeStructure[] v = new DerivativeStructure[4]; + for (int j = 0; j < u.length; ++j) { + u[j] = new DerivativeStructure(u.length, 1, j, 1e17 * random.nextDouble()); + v[j] = new DerivativeStructure(u.length, 1, 1e17 * random.nextDouble()); + } + + DerivativeStructure lin = u[0].linearCombination(u[0], v[0], u[1], v[1]); + double ref = u[0].getValue() * v[0].getValue() + + u[1].getValue() * v[1].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + + lin = u[0].linearCombination(u[0], v[0], u[1], v[1], u[2], v[2]); + ref = u[0].getValue() * v[0].getValue() + + u[1].getValue() * v[1].getValue() + + u[2].getValue() * v[2].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + Assert.assertEquals(v[2].getValue(), lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); + + lin = u[0].linearCombination(u[0], v[0], u[1], v[1], u[2], v[2], u[3], v[3]); + ref = u[0].getValue() * v[0].getValue() + + u[1].getValue() * v[1].getValue() + + u[2].getValue() * v[2].getValue() + + u[3].getValue() * v[3].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + Assert.assertEquals(v[2].getValue(), lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); + Assert.assertEquals(v[3].getValue(), lin.getPartialDerivative(0, 0, 0, 1), 1.0e-15 * FastMath.abs(v[3].getValue())); + + } + } + + @Test + public void testLinearCombination2DoubleDS() { + // 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(0xc6af886975069f11l); + + for (int i = 0; i < 10000; ++i) { + final double[] u = new double[4]; + final DerivativeStructure[] v = new DerivativeStructure[4]; + for (int j = 0; j < u.length; ++j) { + u[j] = 1e17 * random.nextDouble(); + v[j] = new DerivativeStructure(u.length, 1, j, 1e17 * random.nextDouble()); + } + + DerivativeStructure lin = v[0].linearCombination(u[0], v[0], u[1], v[1]); + double ref = u[0] * v[0].getValue() + + u[1] * v[1].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + + lin = v[0].linearCombination(u[0], v[0], u[1], v[1], u[2], v[2]); + ref = u[0] * v[0].getValue() + + u[1] * v[1].getValue() + + u[2] * v[2].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + Assert.assertEquals(u[2], lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); + + lin = v[0].linearCombination(u[0], v[0], u[1], v[1], u[2], v[2], u[3], v[3]); + ref = u[0] * v[0].getValue() + + u[1] * v[1].getValue() + + u[2] * v[2].getValue() + + u[3] * v[3].getValue(); + Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); + Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); + Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); + Assert.assertEquals(u[2], lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); + Assert.assertEquals(u[3], lin.getPartialDerivative(0, 0, 0, 1), 1.0e-15 * FastMath.abs(v[3].getValue())); + + } + } + @Test public void testSerialization() { DerivativeStructure a = new DerivativeStructure(3, 2, 0, 1.3); diff --git a/src/test/java/org/apache/commons/math3/dfp/DfpTest.java b/src/test/java/org/apache/commons/math3/dfp/DfpTest.java index 6f224665c..3ea842f53 100644 --- a/src/test/java/org/apache/commons/math3/dfp/DfpTest.java +++ b/src/test/java/org/apache/commons/math3/dfp/DfpTest.java @@ -1435,23 +1435,23 @@ public class DfpTest { public void testLog10() { - Assert.assertEquals("log10 #1", 1, field.newDfp("12").log10()); - Assert.assertEquals("log10 #2", 2, field.newDfp("123").log10()); - Assert.assertEquals("log10 #3", 3, field.newDfp("1234").log10()); - Assert.assertEquals("log10 #4", 4, field.newDfp("12345").log10()); - Assert.assertEquals("log10 #5", 5, field.newDfp("123456").log10()); - Assert.assertEquals("log10 #6", 6, field.newDfp("1234567").log10()); - Assert.assertEquals("log10 #6", 7, field.newDfp("12345678").log10()); - Assert.assertEquals("log10 #7", 8, field.newDfp("123456789").log10()); - Assert.assertEquals("log10 #8", 9, field.newDfp("1234567890").log10()); - Assert.assertEquals("log10 #9", 10, field.newDfp("12345678901").log10()); - Assert.assertEquals("log10 #10", 11, field.newDfp("123456789012").log10()); - Assert.assertEquals("log10 #11", 12, field.newDfp("1234567890123").log10()); + Assert.assertEquals("log10 #1", 1, field.newDfp("12").intLog10()); + Assert.assertEquals("log10 #2", 2, field.newDfp("123").intLog10()); + Assert.assertEquals("log10 #3", 3, field.newDfp("1234").intLog10()); + Assert.assertEquals("log10 #4", 4, field.newDfp("12345").intLog10()); + Assert.assertEquals("log10 #5", 5, field.newDfp("123456").intLog10()); + Assert.assertEquals("log10 #6", 6, field.newDfp("1234567").intLog10()); + Assert.assertEquals("log10 #6", 7, field.newDfp("12345678").intLog10()); + Assert.assertEquals("log10 #7", 8, field.newDfp("123456789").intLog10()); + Assert.assertEquals("log10 #8", 9, field.newDfp("1234567890").intLog10()); + Assert.assertEquals("log10 #9", 10, field.newDfp("12345678901").intLog10()); + Assert.assertEquals("log10 #10", 11, field.newDfp("123456789012").intLog10()); + Assert.assertEquals("log10 #11", 12, field.newDfp("1234567890123").intLog10()); - Assert.assertEquals("log10 #12", 0, field.newDfp("2").log10()); - Assert.assertEquals("log10 #13", 0, field.newDfp("1").log10()); - Assert.assertEquals("log10 #14", -1, field.newDfp("0.12").log10()); - Assert.assertEquals("log10 #15", -2, field.newDfp("0.012").log10()); + Assert.assertEquals("log10 #12", 0, field.newDfp("2").intLog10()); + Assert.assertEquals("log10 #13", 0, field.newDfp("1").intLog10()); + Assert.assertEquals("log10 #14", -1, field.newDfp("0.12").intLog10()); + Assert.assertEquals("log10 #15", -2, field.newDfp("0.012").intLog10()); } @Test diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java similarity index 71% rename from src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java rename to src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java index 924baff34..0e85d6bb4 100644 --- a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java @@ -30,12 +30,12 @@ import org.junit.Assert; import org.junit.Test; -public class RotationDSTest { +public class FieldRotationDSTest { @Test public void testIdentity() { - RotationDS r = createRotation(1, 0, 0, 0, false); + FieldRotation r = createRotation(1, 0, 0, 0, false); checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0)); checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0)); checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1)); @@ -58,7 +58,7 @@ public class RotationDSTest { @Test public void testAxisAngle() throws MathIllegalArgumentException { - RotationDS r = new RotationDS(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3)); + FieldRotation r = new FieldRotation(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3)); checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0)); checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1)); checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0)); @@ -67,16 +67,16 @@ public class RotationDSTest { checkAngle(r.getAngle(), 2 * FastMath.PI / 3); try { - new RotationDS(createAxis(0, 0, 0), createAngle(2 * FastMath.PI / 3)); + new FieldRotation(createAxis(0, 0, 0), createAngle(2 * FastMath.PI / 3)); Assert.fail("an exception should have been thrown"); } catch (MathIllegalArgumentException e) { } - r = new RotationDS(createAxis(0, 0, 1), createAngle(1.5 * FastMath.PI)); + r = new FieldRotation(createAxis(0, 0, 1), createAngle(1.5 * FastMath.PI)); checkVector(r.getAxis(), createVector(0, 0, -1)); checkAngle(r.getAngle(), 0.5 * FastMath.PI); - r = new RotationDS(createAxis(0, 1, 0), createAngle(FastMath.PI)); + r = new FieldRotation(createAxis(0, 1, 0), createAngle(FastMath.PI)); checkVector(r.getAxis(), createVector(0, 1, 0)); checkAngle(r.getAngle(), FastMath.PI); @@ -90,7 +90,7 @@ public class RotationDSTest { double b = 0.36; double c = 0.48; double d = 0.8; - RotationDS r = createRotation(a, b, c, d, true); + FieldRotation r = createRotation(a, b, c, d, true); double a2 = a * a; double b2 = b * b; double c2 = c * c; @@ -112,8 +112,8 @@ public class RotationDSTest { Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15); Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15); Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15); - RotationDS reverted = r.revert(); - RotationDS rrT = r.applyTo(reverted); + FieldRotation reverted = r.revert(); + FieldRotation rrT = r.applyTo(reverted); checkRotationDS(rrT, 1, 0, 0, 0); Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15); Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15); @@ -131,7 +131,7 @@ public class RotationDSTest { Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15); Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15); Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15); - RotationDS rTr = reverted.applyTo(r); + FieldRotation rTr = reverted.applyTo(r); checkRotationDS(rTr, 1, 0, 0, 0); Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15); Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15); @@ -149,22 +149,22 @@ public class RotationDSTest { Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15); Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15); Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15); - Assert.assertEquals(r.getAngle().getValue(), reverted.getAngle().getValue(), 1.0e-15); - Assert.assertEquals(-1, Vector3DDS.dotProduct(r.getAxis(), reverted.getAxis()).getValue(), 1.0e-15); + Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15); + Assert.assertEquals(-1, r.getAxis().dotProduct(reverted.getAxis()).getReal(), 1.0e-15); } @Test public void testVectorOnePair() throws MathArithmeticException { - Vector3DDS u = createVector(3, 2, 1); - Vector3DDS v = createVector(-4, 2, 2); - RotationDS r = new RotationDS(u, v); + FieldVector3D u = createVector(3, 2, 1); + FieldVector3D v = createVector(-4, 2, 2); + FieldRotation r = new FieldRotation(u, v); checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm())); - checkAngle(new RotationDS(u, u.negate()).getAngle(), FastMath.PI); + checkAngle(new FieldRotation(u, u.negate()).getAngle(), FastMath.PI); try { - new RotationDS(u, createVector(0, 0, 0)); + new FieldRotation(u, createVector(0, 0, 0)); Assert.fail("an exception should have been thrown"); } catch (MathArithmeticException e) { // expected behavior @@ -175,17 +175,17 @@ public class RotationDSTest { @Test public void testVectorTwoPairs() throws MathArithmeticException { - Vector3DDS u1 = createVector(3, 0, 0); - Vector3DDS u2 = createVector(0, 5, 0); - Vector3DDS v1 = createVector(0, 0, 2); - Vector3DDS v2 = createVector(-2, 0, 2); - RotationDS r = new RotationDS(u1, u2, v1, v2); + FieldVector3D u1 = createVector(3, 0, 0); + FieldVector3D u2 = createVector(0, 5, 0); + FieldVector3D v1 = createVector(0, 0, 2); + FieldVector3D v2 = createVector(-2, 0, 2); + FieldRotation r = new FieldRotation(u1, u2, v1, v2); checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1)); checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0)); - r = new RotationDS(u1, u2, u1.negate(), u2.negate()); - Vector3DDS axis = r.getAxis(); - if (Vector3DDS.dotProduct(axis, createVector(0, 0, 1)).getValue() > 0) { + r = new FieldRotation(u1, u2, u1.negate(), u2.negate()); + FieldVector3D axis = r.getAxis(); + if (axis.dotProduct(createVector(0, 0, 1)).getReal() > 0) { checkVector(axis, createVector(0, 0, 1)); } else { checkVector(axis, createVector(0, 0, -1)); @@ -193,18 +193,18 @@ public class RotationDSTest { checkAngle(r.getAngle(), FastMath.PI); double sqrt = FastMath.sqrt(2) / 2; - r = new RotationDS(createVector(1, 0, 0), createVector(0, 1, 0), + r = new FieldRotation(createVector(1, 0, 0), createVector(0, 1, 0), createVector(0.5, 0.5, sqrt), createVector(0.5, 0.5, -sqrt)); checkRotationDS(r, sqrt, 0.5, 0.5, 0); - r = new RotationDS(u1, u2, u1, Vector3DDS.crossProduct(u1, u2)); + r = new FieldRotation(u1, u2, u1, u1.crossProduct(u2)); checkRotationDS(r, sqrt, -sqrt, 0, 0); - checkRotationDS(new RotationDS(u1, u2, u1, u2), 1, 0, 0, 0); + checkRotationDS(new FieldRotation(u1, u2, u1, u2), 1, 0, 0, 0); try { - new RotationDS(u1, u2, createVector(0, 0, 0), v2); + new FieldRotation(u1, u2, createVector(0, 0, 0), v2); Assert.fail("an exception should have been thrown"); } catch (MathArithmeticException e) { // expected behavior @@ -279,7 +279,7 @@ public class RotationDSTest { double[][] m1 = { { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 }, { 1.0, 0.0, 0.0 } }; - RotationDS r = createRotation(m1, 1.0e-7); + FieldRotation r = createRotation(m1, 1.0e-7); checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1)); checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0)); checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0)); @@ -290,15 +290,15 @@ public class RotationDSTest { r = createRotation(m2, 1.0e-12); DerivativeStructure[][] m3 = r.getMatrix(); - double d00 = m2[0][0] - m3[0][0].getValue(); - double d01 = m2[0][1] - m3[0][1].getValue(); - double d02 = m2[0][2] - m3[0][2].getValue(); - double d10 = m2[1][0] - m3[1][0].getValue(); - double d11 = m2[1][1] - m3[1][1].getValue(); - double d12 = m2[1][2] - m3[1][2].getValue(); - double d20 = m2[2][0] - m3[2][0].getValue(); - double d21 = m2[2][1] - m3[2][1].getValue(); - double d22 = m2[2][2] - m3[2][2].getValue(); + double d00 = m2[0][0] - m3[0][0].getReal(); + double d01 = m2[0][1] - m3[0][1].getReal(); + double d02 = m2[0][2] - m3[0][2].getReal(); + double d10 = m2[1][0] - m3[1][0].getReal(); + double d11 = m2[1][1] - m3[1][1].getReal(); + double d12 = m2[1][2] - m3[1][2].getReal(); + double d20 = m2[2][0] - m3[2][0].getReal(); + double d21 = m2[2][1] - m3[2][1].getReal(); + double d22 = m2[2][2] - m3[2][2].getReal(); Assert.assertTrue(FastMath.abs(d00) < 6.0e-6); Assert.assertTrue(FastMath.abs(d01) < 6.0e-6); @@ -322,9 +322,9 @@ public class RotationDSTest { for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { - double m3tm3 = m3[i][0].getValue() * m3[j][0].getValue() + - m3[i][1].getValue() * m3[j][1].getValue() + - m3[i][2].getValue() * m3[j][2].getValue(); + double m3tm3 = m3[i][0].getReal() * m3[j][0].getReal() + + m3[i][1].getReal() * m3[j][1].getReal() + + m3[i][2].getReal() * m3[j][2].getReal(); if (i == j) { Assert.assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10); } else { @@ -334,11 +334,11 @@ public class RotationDSTest { } checkVector(r.applyTo(createVector(1, 0, 0)), - new Vector3DDS(m3[0][0], m3[1][0], m3[2][0])); + new FieldVector3D(m3[0][0], m3[1][0], m3[2][0])); checkVector(r.applyTo(createVector(0, 1, 0)), - new Vector3DDS(m3[0][1], m3[1][1], m3[2][1])); + new FieldVector3D(m3[0][1], m3[1][1], m3[2][1])); checkVector(r.applyTo(createVector(0, 0, 1)), - new Vector3DDS(m3[0][2], m3[1][2], m3[2][2])); + new FieldVector3D(m3[0][2], m3[1][2], m3[2][2])); double[][] m4 = { { 1.0, 0.0, 0.0 }, { 0.0, -1.0, 0.0 }, @@ -371,7 +371,7 @@ public class RotationDSTest { for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) { for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { - RotationDS r = new RotationDS(CardanOrders[i], + FieldRotation r = new FieldRotation(CardanOrders[i], new DerivativeStructure(3, 1, 0, alpha1), new DerivativeStructure(3, 1, 1, alpha2), new DerivativeStructure(3, 1, 2, alpha3)); @@ -393,7 +393,7 @@ public class RotationDSTest { for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) { for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) { for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) { - RotationDS r = new RotationDS(EulerOrders[i], + FieldRotation r = new FieldRotation(EulerOrders[i], new DerivativeStructure(3, 1, 0, alpha1), new DerivativeStructure(3, 1, 1, alpha2), new DerivativeStructure(3, 1, 2, alpha3)); @@ -419,7 +419,7 @@ public class RotationDSTest { double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 }; for (int i = 0; i < CardanOrders.length; ++i) { for (int j = 0; j < singularCardanAngle.length; ++j) { - RotationDS r = new RotationDS(CardanOrders[i], + FieldRotation r = new FieldRotation(CardanOrders[i], new DerivativeStructure(3, 1, 0, 0.1), new DerivativeStructure(3, 1, 1, singularCardanAngle[j]), new DerivativeStructure(3, 1, 2, 0.3)); @@ -440,7 +440,7 @@ public class RotationDSTest { double[] singularEulerAngle = { 0, FastMath.PI }; for (int i = 0; i < EulerOrders.length; ++i) { for (int j = 0; j < singularEulerAngle.length; ++j) { - RotationDS r = new RotationDS(EulerOrders[i], + FieldRotation r = new FieldRotation(EulerOrders[i], new DerivativeStructure(3, 1, 0, 0.1), new DerivativeStructure(3, 1, 1, singularEulerAngle[j]), new DerivativeStructure(3, 1, 2, 0.3)); @@ -459,15 +459,15 @@ public class RotationDSTest { @Test public void testQuaternion() throws MathIllegalArgumentException { - RotationDS r1 = new RotationDS(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); double n = 23.5; - RotationDS r2 = new RotationDS(r1.getQ0().multiply(n), r1.getQ1().multiply(n), + FieldRotation r2 = new FieldRotation(r1.getQ0().multiply(n), r1.getQ1().multiply(n), r1.getQ2().multiply(n), r1.getQ3().multiply(n), true); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS u = createVector(x, y, z); + FieldVector3D u = createVector(x, y, z); checkVector(r2.applyTo(u), r1.applyTo(u)); } } @@ -475,23 +475,27 @@ public class RotationDSTest { r1 = createRotation(0.288, 0.384, 0.36, 0.8, false); checkRotationDS(r1, - -r1.getQ0().getValue(), -r1.getQ1().getValue(), - -r1.getQ2().getValue(), -r1.getQ3().getValue()); + -r1.getQ0().getReal(), -r1.getQ1().getReal(), + -r1.getQ2().getReal(), -r1.getQ3().getReal()); } @Test public void testCompose() throws MathIllegalArgumentException { - RotationDS r1 = new RotationDS(createVector(2, -3, 5), createAngle(1.7)); - RotationDS r2 = new RotationDS(createVector(-1, 3, 2), createAngle(0.3)); - RotationDS r3 = r2.applyTo(r1); - RotationDS r3Double = r2.applyTo(r1.toRotation()); + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r2 = new FieldRotation(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation r3 = r2.applyTo(r1); + FieldRotation r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(), + r1.getQ1().getReal(), + r1.getQ2().getReal(), + r1.getQ3().getReal(), + false)); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS u = createVector(x, y, z); + FieldVector3D u = createVector(x, y, z); checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u)); checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u)); } @@ -503,15 +507,19 @@ public class RotationDSTest { @Test public void testComposeInverse() throws MathIllegalArgumentException { - RotationDS r1 = new RotationDS(createVector(2, -3, 5), createAngle(1.7)); - RotationDS r2 = new RotationDS(createVector(-1, 3, 2), createAngle(0.3)); - RotationDS r3 = r2.applyInverseTo(r1); - RotationDS r3Double = r2.applyInverseTo(r1.toRotation()); + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r2 = new FieldRotation(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation r3 = r2.applyInverseTo(r1); + FieldRotation r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(), + r1.getQ1().getReal(), + r1.getQ2().getReal(), + r1.getQ3().getReal(), + false)); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS u = createVector(x, y, z); + FieldVector3D u = createVector(x, y, z); checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u)); checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u)); } @@ -527,26 +535,26 @@ public class RotationDSTest { UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random); for (int i = 0; i < 10; ++i) { double[] unit = g.nextVector(); - RotationDS r = new RotationDS(createVector(unit[0], unit[1], unit[2]), + FieldRotation r = new FieldRotation(createVector(unit[0], unit[1], unit[2]), createAngle(random.nextDouble())); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS uds = createVector(x, y, z); - Vector3DDS ruds = r.applyTo(uds); - Vector3DDS rIuds = r.applyInverseTo(uds); + FieldVector3D uds = createVector(x, y, z); + FieldVector3D ruds = r.applyTo(uds); + FieldVector3D rIuds = r.applyInverseTo(uds); Vector3D u = new Vector3D(x, y, z); - Vector3DDS ru = r.applyTo(u); - Vector3DDS rIu = r.applyInverseTo(u); + FieldVector3D ru = r.applyTo(u); + FieldVector3D rIu = r.applyInverseTo(u); DerivativeStructure[] ruArray = new DerivativeStructure[3]; r.applyTo(new double[] { x, y, z}, ruArray); DerivativeStructure[] rIuArray = new DerivativeStructure[3]; r.applyInverseTo(new double[] { x, y, z}, rIuArray); checkVector(ruds, ru); - checkVector(ruds, new Vector3DDS(ruArray)); + checkVector(ruds, new FieldVector3D(ruArray)); checkVector(rIuds, rIu); - checkVector(rIuds, new Vector3DDS(rIuArray)); + checkVector(rIuds, new FieldVector3D(rIuArray)); } } } @@ -563,27 +571,27 @@ public class RotationDSTest { double[] unit1 = g.nextVector(); Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]), random.nextDouble()); - RotationDS r1Prime = new RotationDS(new DerivativeStructure(4, 1, 0, r1.getQ0()), + FieldRotation r1Prime = new FieldRotation(new DerivativeStructure(4, 1, 0, r1.getQ0()), new DerivativeStructure(4, 1, 1, r1.getQ1()), new DerivativeStructure(4, 1, 2, r1.getQ2()), new DerivativeStructure(4, 1, 3, r1.getQ3()), false); double[] unit2 = g.nextVector(); - RotationDS r2 = new RotationDS(createVector(unit2[0], unit2[1], unit2[2]), + FieldRotation r2 = new FieldRotation(createVector(unit2[0], unit2[1], unit2[2]), createAngle(random.nextDouble())); - RotationDS rA = RotationDS.applyTo(r1, r2); - RotationDS rB = r1Prime.applyTo(r2); - RotationDS rC = RotationDS.applyInverseTo(r1, r2); - RotationDS rD = r1Prime.applyInverseTo(r2); + FieldRotation rA = FieldRotation.applyTo(r1, r2); + FieldRotation rB = r1Prime.applyTo(r2); + FieldRotation rC = FieldRotation.applyInverseTo(r1, r2); + FieldRotation rD = r1Prime.applyInverseTo(r2); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS uds = createVector(x, y, z); - checkVector(r1Prime.applyTo(uds), RotationDS.applyTo(r1, uds)); - checkVector(r1Prime.applyInverseTo(uds), RotationDS.applyInverseTo(r1, uds)); + FieldVector3D uds = createVector(x, y, z); + checkVector(r1Prime.applyTo(uds), FieldRotation.applyTo(r1, uds)); + checkVector(r1Prime.applyInverseTo(uds), FieldRotation.applyInverseTo(r1, uds)); checkVector(rA.applyTo(uds), rB.applyTo(uds)); checkVector(rA.applyInverseTo(uds), rB.applyInverseTo(uds)); checkVector(rC.applyTo(uds), rD.applyTo(uds)); @@ -608,7 +616,7 @@ public class RotationDSTest { double theta = 1.7; double cosTheta = FastMath.cos(theta); double sinTheta = FastMath.sin(theta); - RotationDS r = new RotationDS(createAxis(kx, ky, kz), createAngle(theta)); + FieldRotation r = new FieldRotation(createAxis(kx, ky, kz), createAngle(theta)); Vector3D a = new Vector3D(kx / n, ky / n, kz / n); // Jacobian of the normalized rotation axis a with respect to the Cartesian vector k @@ -622,7 +630,7 @@ public class RotationDSTest { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { Vector3D u = new Vector3D(x, y, z); - Vector3DDS v = r.applyTo(createVector(x, y, z)); + FieldVector3D v = r.applyTo(createVector(x, y, z)); // explicit formula for rotation of vector u around axis a with angle theta double dot = Vector3D.dotProduct(u, a); @@ -630,9 +638,9 @@ public class RotationDSTest { double c1 = 1 - cosTheta; double c2 = c1 * dot; Vector3D rt = new Vector3D(cosTheta, u, c2, a, sinTheta, cross); - Assert.assertEquals(rt.getX(), v.getX().getValue(), eps); - Assert.assertEquals(rt.getY(), v.getY().getValue(), eps); - Assert.assertEquals(rt.getZ(), v.getZ().getValue(), eps); + Assert.assertEquals(rt.getX(), v.getX().getReal(), eps); + Assert.assertEquals(rt.getY(), v.getY().getReal(), eps); + Assert.assertEquals(rt.getZ(), v.getZ().getReal(), eps); // Jacobian of the image v = r(u) with respect to rotation axis a // (analytical differentiation of the explicit formula) @@ -672,22 +680,22 @@ public class RotationDSTest { @Test public void testArray() throws MathIllegalArgumentException { - RotationDS r = new RotationDS(createAxis(2, -3, 5), createAngle(1.7)); + FieldRotation r = new FieldRotation(createAxis(2, -3, 5), createAngle(1.7)); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { for (double z = -0.9; z < 0.9; z += 0.2) { - Vector3DDS u = createVector(x, y, z); - Vector3DDS v = r.applyTo(u); + FieldVector3D u = createVector(x, y, z); + FieldVector3D v = r.applyTo(u); DerivativeStructure[] out = new DerivativeStructure[3]; r.applyTo(new DerivativeStructure[] { u.getX(), u.getY(), u.getZ() }, out); - Assert.assertEquals(v.getX().getValue(), out[0].getValue(), 1.0e-10); - Assert.assertEquals(v.getY().getValue(), out[1].getValue(), 1.0e-10); - Assert.assertEquals(v.getZ().getValue(), out[2].getValue(), 1.0e-10); + Assert.assertEquals(v.getX().getReal(), out[0].getReal(), 1.0e-10); + Assert.assertEquals(v.getY().getReal(), out[1].getReal(), 1.0e-10); + Assert.assertEquals(v.getZ().getReal(), out[2].getReal(), 1.0e-10); r.applyInverseTo(out, out); - Assert.assertEquals(u.getX().getValue(), out[0].getValue(), 1.0e-10); - Assert.assertEquals(u.getY().getValue(), out[1].getValue(), 1.0e-10); - Assert.assertEquals(u.getZ().getValue(), out[2].getValue(), 1.0e-10); + Assert.assertEquals(u.getX().getReal(), out[0].getReal(), 1.0e-10); + Assert.assertEquals(u.getY().getReal(), out[1].getReal(), 1.0e-10); + Assert.assertEquals(u.getZ().getReal(), out[2].getReal(), 1.0e-10); } } } @@ -700,10 +708,10 @@ public class RotationDSTest { DerivativeStructure[] in = new DerivativeStructure[3]; DerivativeStructure[] out = new DerivativeStructure[3]; DerivativeStructure[] rebuilt = new DerivativeStructure[3]; - RotationDS r = new RotationDS(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { - Vector3DDS u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); r.applyInverseTo(r.applyTo(u)); @@ -714,16 +722,16 @@ public class RotationDSTest { in[2] = u.getZ(); r.applyTo(in, out); r.applyInverseTo(out, rebuilt); - Assert.assertEquals(in[0].getValue(), rebuilt[0].getValue(), 1.0e-12); - Assert.assertEquals(in[1].getValue(), rebuilt[1].getValue(), 1.0e-12); - Assert.assertEquals(in[2].getValue(), rebuilt[2].getValue(), 1.0e-12); + Assert.assertEquals(in[0].getReal(), rebuilt[0].getReal(), 1.0e-12); + Assert.assertEquals(in[1].getReal(), rebuilt[1].getReal(), 1.0e-12); + Assert.assertEquals(in[2].getReal(), rebuilt[2].getReal(), 1.0e-12); } } r = createRotation(1, 0, 0, 0, false); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { - Vector3DDS u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); checkVector(u, r.applyInverseTo(r.applyTo(u))); @@ -731,10 +739,10 @@ public class RotationDSTest { } } - r = new RotationDS(createVector(0, 0, 1), createAngle(FastMath.PI)); + r = new FieldRotation(createVector(0, 0, 1), createAngle(FastMath.PI)); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { - Vector3DDS u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), FastMath.sin(lambda) * FastMath.cos(phi), FastMath.sin(phi)); checkVector(u, r.applyInverseTo(r.applyTo(u))); @@ -746,57 +754,57 @@ public class RotationDSTest { @Test public void testIssue639() throws MathArithmeticException{ - Vector3DDS u1 = createVector(-1321008684645961.0 / 268435456.0, + FieldVector3D u1 = createVector(-1321008684645961.0 / 268435456.0, -5774608829631843.0 / 268435456.0, -3822921525525679.0 / 4294967296.0); - Vector3DDS u2 =createVector( -5712344449280879.0 / 2097152.0, + FieldVector3D u2 =createVector( -5712344449280879.0 / 2097152.0, -2275058564560979.0 / 1048576.0, 4423475992255071.0 / 65536.0); - RotationDS rot = new RotationDS(u1, u2, createVector(1, 0, 0),createVector(0, 0, 1)); - Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0().getValue(), 1.0e-15); - Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1().getValue(), 1.0e-15); - Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2().getValue(), 1.0e-15); - Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3().getValue(), 1.0e-15); + FieldRotation rot = new FieldRotation(u1, u2, createVector(1, 0, 0),createVector(0, 0, 1)); + Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0().getReal(), 1.0e-15); + Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1().getReal(), 1.0e-15); + Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2().getReal(), 1.0e-15); + Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3().getReal(), 1.0e-15); } @Test public void testIssue801() throws MathArithmeticException { - Vector3DDS u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0); - Vector3DDS u2 = createVector(0.0, 0.0, 1.0); + FieldVector3D u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0); + FieldVector3D u2 = createVector(0.0, 0.0, 1.0); - Vector3DDS v1 = createVector(0.9999999999999999, 0.0, 0.0); - Vector3DDS v2 = createVector(0.0, 0.0, -1.0); + FieldVector3D v1 = createVector(0.9999999999999999, 0.0, 0.0); + FieldVector3D v2 = createVector(0.0, 0.0, -1.0); - RotationDS quat = new RotationDS(u1, u2, v1, v2); - double q2 = quat.getQ0().getValue() * quat.getQ0().getValue() + - quat.getQ1().getValue() * quat.getQ1().getValue() + - quat.getQ2().getValue() * quat.getQ2().getValue() + - quat.getQ3().getValue() * quat.getQ3().getValue(); + FieldRotation quat = new FieldRotation(u1, u2, v1, v2); + double q2 = quat.getQ0().getReal() * quat.getQ0().getReal() + + quat.getQ1().getReal() * quat.getQ1().getReal() + + quat.getQ2().getReal() * quat.getQ2().getReal() + + quat.getQ3().getReal() * quat.getQ3().getReal(); Assert.assertEquals(1.0, q2, 1.0e-14); - Assert.assertEquals(0.0, Vector3DDS.angle(v1, quat.applyTo(u1)).getValue(), 1.0e-14); - Assert.assertEquals(0.0, Vector3DDS.angle(v2, quat.applyTo(u2)).getValue(), 1.0e-14); + Assert.assertEquals(0.0, v1.angle(quat.applyTo(u1)).getReal(), 1.0e-14); + Assert.assertEquals(0.0, v2.angle(quat.applyTo(u2)).getReal(), 1.0e-14); } private void checkAngle(DerivativeStructure a1, double a2) { - Assert.assertEquals(a1.getValue(), MathUtils.normalizeAngle(a2, a1.getValue()), 1.0e-10); + Assert.assertEquals(a1.getReal(), MathUtils.normalizeAngle(a2, a1.getReal()), 1.0e-10); } - private void checkRotationDS(RotationDS r, double q0, double q1, double q2, double q3) { - RotationDS rPrime = createRotation(q0, q1, q2, q3, false); - Assert.assertEquals(0, RotationDS.distance(r, rPrime).getValue(), 1.0e-12); + private void checkRotationDS(FieldRotation r, double q0, double q1, double q2, double q3) { + FieldRotation rPrime = createRotation(q0, q1, q2, q3, false); + Assert.assertEquals(0, FieldRotation.distance(r, rPrime).getReal(), 1.0e-12); } - private RotationDS createRotation(double q0, double q1, double q2, double q3, + private FieldRotation createRotation(double q0, double q1, double q2, double q3, boolean needsNormalization) { - return new RotationDS(new DerivativeStructure(4, 1, 0, q0), + return new FieldRotation(new DerivativeStructure(4, 1, 0, q0), new DerivativeStructure(4, 1, 1, q1), new DerivativeStructure(4, 1, 2, q2), new DerivativeStructure(4, 1, 3, q3), needsNormalization); } - private RotationDS createRotation(double[][] m, double threshold) { + private FieldRotation createRotation(double[][] m, double threshold) { DerivativeStructure[][] mds = new DerivativeStructure[m.length][m[0].length]; int index = 0; for (int i = 0; i < m.length; ++i) { @@ -805,17 +813,17 @@ public class RotationDSTest { index = (index + 1) % 4; } } - return new RotationDS(mds, threshold); + return new FieldRotation(mds, threshold); } - private Vector3DDS createVector(double x, double y, double z) { - return new Vector3DDS(new DerivativeStructure(4, 1, x), + private FieldVector3D createVector(double x, double y, double z) { + return new FieldVector3D(new DerivativeStructure(4, 1, x), new DerivativeStructure(4, 1, y), new DerivativeStructure(4, 1, z)); } - private Vector3DDS createAxis(double x, double y, double z) { - return new Vector3DDS(new DerivativeStructure(4, 1, 0, x), + private FieldVector3D createAxis(double x, double y, double z) { + return new FieldVector3D(new DerivativeStructure(4, 1, 0, x), new DerivativeStructure(4, 1, 1, y), new DerivativeStructure(4, 1, 2, z)); } @@ -824,10 +832,10 @@ public class RotationDSTest { return new DerivativeStructure(4, 1, 3, alpha); } - private void checkVector(Vector3DDS u, Vector3DDS v) { - Assert.assertEquals(u.getX().getValue(), v.getX().getValue(), 1.0e-12); - Assert.assertEquals(u.getY().getValue(), v.getY().getValue(), 1.0e-12); - Assert.assertEquals(u.getZ().getValue(), v.getZ().getValue(), 1.0e-12); + private void checkVector(FieldVector3D u, FieldVector3D v) { + Assert.assertEquals(u.getX().getReal(), v.getX().getReal(), 1.0e-12); + Assert.assertEquals(u.getY().getReal(), v.getY().getReal(), 1.0e-12); + Assert.assertEquals(u.getZ().getReal(), v.getZ().getReal(), 1.0e-12); } } diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDfpTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDfpTest.java new file mode 100644 index 000000000..5946f6e8a --- /dev/null +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDfpTest.java @@ -0,0 +1,716 @@ +/* + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * The ASF licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.apache.commons.math3.geometry.euclidean.threed; + +import org.apache.commons.math3.dfp.Dfp; +import org.apache.commons.math3.dfp.DfpField; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.random.UnitSphereRandomVectorGenerator; +import org.apache.commons.math3.random.Well1024a; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; +import org.junit.Assert; +import org.junit.Test; + + +public class FieldRotationDfpTest { + + @Test + public void testIdentity() { + + FieldRotation r = createRotation(1, 0, 0, 0, false); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1)); + checkAngle(r.getAngle(), 0); + + r = createRotation(-1, 0, 0, 0, false); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1)); + checkAngle(r.getAngle(), 0); + + r = createRotation(42, 0, 0, 0, true); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(1, 0, 0)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 1, 0)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 0, 1)); + checkAngle(r.getAngle(), 0); + + } + + @Test + public void testAxisAngle() throws MathIllegalArgumentException { + + FieldRotation r = new FieldRotation(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3)); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0)); + double s = 1 / FastMath.sqrt(3); + checkVector(r.getAxis(), createVector(s, s, s)); + checkAngle(r.getAngle(), 2 * FastMath.PI / 3); + + try { + new FieldRotation(createAxis(0, 0, 0), createAngle(2 * FastMath.PI / 3)); + Assert.fail("an exception should have been thrown"); + } catch (MathIllegalArgumentException e) { + } + + r = new FieldRotation(createAxis(0, 0, 1), createAngle(1.5 * FastMath.PI)); + checkVector(r.getAxis(), createVector(0, 0, -1)); + checkAngle(r.getAngle(), 0.5 * FastMath.PI); + + r = new FieldRotation(createAxis(0, 1, 0), createAngle(FastMath.PI)); + checkVector(r.getAxis(), createVector(0, 1, 0)); + checkAngle(r.getAngle(), FastMath.PI); + + checkVector(createRotation(1, 0, 0, 0, false).getAxis(), createVector(1, 0, 0)); + + } + + @Test + public void testRevert() { + double a = 0.001; + double b = 0.36; + double c = 0.48; + double d = 0.8; + FieldRotation r = createRotation(a, b, c, d, true); + FieldRotation reverted = r.revert(); + FieldRotation rrT = r.applyTo(reverted); + checkRotationDS(rrT, 1, 0, 0, 0); + FieldRotation rTr = reverted.applyTo(r); + checkRotationDS(rTr, 1, 0, 0, 0); + Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15); + Assert.assertEquals(-1, r.getAxis().dotProduct(reverted.getAxis()).getReal(), 1.0e-15); + } + + @Test + public void testVectorOnePair() throws MathArithmeticException { + + FieldVector3D u = createVector(3, 2, 1); + FieldVector3D v = createVector(-4, 2, 2); + FieldRotation r = new FieldRotation(u, v); + checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm())); + + checkAngle(new FieldRotation(u, u.negate()).getAngle(), FastMath.PI); + + try { + new FieldRotation(u, createVector(0, 0, 0)); + Assert.fail("an exception should have been thrown"); + } catch (MathArithmeticException e) { + // expected behavior + } + + } + + @Test + public void testVectorTwoPairs() throws MathArithmeticException { + + FieldVector3D u1 = createVector(3, 0, 0); + FieldVector3D u2 = createVector(0, 5, 0); + FieldVector3D v1 = createVector(0, 0, 2); + FieldVector3D v2 = createVector(-2, 0, 2); + FieldRotation r = new FieldRotation(u1, u2, v1, v2); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0)); + + r = new FieldRotation(u1, u2, u1.negate(), u2.negate()); + FieldVector3D axis = r.getAxis(); + if (axis.dotProduct(createVector(0, 0, 1)).getReal() > 0) { + checkVector(axis, createVector(0, 0, 1)); + } else { + checkVector(axis, createVector(0, 0, -1)); + } + checkAngle(r.getAngle(), FastMath.PI); + + double sqrt = FastMath.sqrt(2) / 2; + r = new FieldRotation(createVector(1, 0, 0), createVector(0, 1, 0), + createVector(0.5, 0.5, sqrt), + createVector(0.5, 0.5, -sqrt)); + checkRotationDS(r, sqrt, 0.5, 0.5, 0); + + r = new FieldRotation(u1, u2, u1, u1.crossProduct(u2)); + checkRotationDS(r, sqrt, -sqrt, 0, 0); + + checkRotationDS(new FieldRotation(u1, u2, u1, u2), 1, 0, 0, 0); + + try { + new FieldRotation(u1, u2, createVector(0, 0, 0), v2); + Assert.fail("an exception should have been thrown"); + } catch (MathArithmeticException e) { + // expected behavior + } + + } + + @Test + public void testMatrix() + throws NotARotationMatrixException { + + try { + createRotation(new double[][] { + { 0.0, 1.0, 0.0 }, + { 1.0, 0.0, 0.0 } + }, 1.0e-7); + Assert.fail("Expecting NotARotationMatrixException"); + } catch (NotARotationMatrixException nrme) { + // expected behavior + } + + try { + createRotation(new double[][] { + { 0.445888, 0.797184, -0.407040 }, + { 0.821760, -0.184320, 0.539200 }, + { -0.354816, 0.574912, 0.737280 } + }, 1.0e-7); + Assert.fail("Expecting NotARotationMatrixException"); + } catch (NotARotationMatrixException nrme) { + // expected behavior + } + + try { + createRotation(new double[][] { + { 0.4, 0.8, -0.4 }, + { -0.4, 0.6, 0.7 }, + { 0.8, -0.2, 0.5 } + }, 1.0e-15); + Assert.fail("Expecting NotARotationMatrixException"); + } catch (NotARotationMatrixException nrme) { + // expected behavior + } + + checkRotationDS(createRotation(new double[][] { + { 0.445888, 0.797184, -0.407040 }, + { -0.354816, 0.574912, 0.737280 }, + { 0.821760, -0.184320, 0.539200 } + }, 1.0e-10), + 0.8, 0.288, 0.384, 0.36); + + checkRotationDS(createRotation(new double[][] { + { 0.539200, 0.737280, 0.407040 }, + { 0.184320, -0.574912, 0.797184 }, + { 0.821760, -0.354816, -0.445888 } + }, 1.0e-10), + 0.36, 0.8, 0.288, 0.384); + + checkRotationDS(createRotation(new double[][] { + { -0.445888, 0.797184, -0.407040 }, + { 0.354816, 0.574912, 0.737280 }, + { 0.821760, 0.184320, -0.539200 } + }, 1.0e-10), + 0.384, 0.36, 0.8, 0.288); + + checkRotationDS(createRotation(new double[][] { + { -0.539200, 0.737280, 0.407040 }, + { -0.184320, -0.574912, 0.797184 }, + { 0.821760, 0.354816, 0.445888 } + }, 1.0e-10), + 0.288, 0.384, 0.36, 0.8); + + double[][] m1 = { { 0.0, 1.0, 0.0 }, + { 0.0, 0.0, 1.0 }, + { 1.0, 0.0, 0.0 } }; + FieldRotation r = createRotation(m1, 1.0e-7); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0)); + + double[][] m2 = { { 0.83203, -0.55012, -0.07139 }, + { 0.48293, 0.78164, -0.39474 }, + { 0.27296, 0.29396, 0.91602 } }; + r = createRotation(m2, 1.0e-12); + + Dfp[][] m3 = r.getMatrix(); + double d00 = m2[0][0] - m3[0][0].getReal(); + double d01 = m2[0][1] - m3[0][1].getReal(); + double d02 = m2[0][2] - m3[0][2].getReal(); + double d10 = m2[1][0] - m3[1][0].getReal(); + double d11 = m2[1][1] - m3[1][1].getReal(); + double d12 = m2[1][2] - m3[1][2].getReal(); + double d20 = m2[2][0] - m3[2][0].getReal(); + double d21 = m2[2][1] - m3[2][1].getReal(); + double d22 = m2[2][2] - m3[2][2].getReal(); + + Assert.assertTrue(FastMath.abs(d00) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d01) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d02) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d10) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d11) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d12) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d20) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d21) < 6.0e-6); + Assert.assertTrue(FastMath.abs(d22) < 6.0e-6); + + Assert.assertTrue(FastMath.abs(d00) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d01) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d02) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d10) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d11) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d12) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d20) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d21) > 4.0e-7); + Assert.assertTrue(FastMath.abs(d22) > 4.0e-7); + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + double m3tm3 = m3[i][0].getReal() * m3[j][0].getReal() + + m3[i][1].getReal() * m3[j][1].getReal() + + m3[i][2].getReal() * m3[j][2].getReal(); + if (i == j) { + Assert.assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10); + } else { + Assert.assertTrue(FastMath.abs(m3tm3) < 1.0e-10); + } + } + } + + checkVector(r.applyTo(createVector(1, 0, 0)), + new FieldVector3D(m3[0][0], m3[1][0], m3[2][0])); + checkVector(r.applyTo(createVector(0, 1, 0)), + new FieldVector3D(m3[0][1], m3[1][1], m3[2][1])); + checkVector(r.applyTo(createVector(0, 0, 1)), + new FieldVector3D(m3[0][2], m3[1][2], m3[2][2])); + + double[][] m4 = { { 1.0, 0.0, 0.0 }, + { 0.0, -1.0, 0.0 }, + { 0.0, 0.0, -1.0 } }; + r = createRotation(m4, 1.0e-7); + checkAngle(r.getAngle(), FastMath.PI); + + try { + double[][] m5 = { { 0.0, 0.0, 1.0 }, + { 0.0, 1.0, 0.0 }, + { 1.0, 0.0, 0.0 } }; + r = createRotation(m5, 1.0e-7); + Assert.fail("got " + r + ", should have caught an exception"); + } catch (NotARotationMatrixException e) { + // expected + } + + } + + @Test + public void testAngles() + throws CardanEulerSingularityException { + + DfpField field = new DfpField(15); + + RotationOrder[] CardanOrders = { + RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, + RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX + }; + + for (int i = 0; i < CardanOrders.length; ++i) { + for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) { + for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.8) { + for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) { + FieldRotation r = new FieldRotation(CardanOrders[i], + field.newDfp(alpha1), + field.newDfp(alpha2), + field.newDfp(alpha3)); + Dfp[] angles = r.getAngles(CardanOrders[i]); + checkAngle(angles[0], alpha1); + checkAngle(angles[1], alpha2); + checkAngle(angles[2], alpha3); + } + } + } + } + + RotationOrder[] EulerOrders = { + RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, + RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ + }; + + for (int i = 0; i < EulerOrders.length; ++i) { + for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) { + for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.8) { + for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) { + FieldRotation r = new FieldRotation(EulerOrders[i], + field.newDfp(alpha1), + field.newDfp(alpha2), + field.newDfp(alpha3)); + Dfp[] angles = r.getAngles(EulerOrders[i]); + checkAngle(angles[0], alpha1); + checkAngle(angles[1], alpha2); + checkAngle(angles[2], alpha3); + } + } + } + } + + } + + @Test + public void testSingularities() { + + DfpField field = new DfpField(20); + RotationOrder[] CardanOrders = { + RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, + RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX + }; + + double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 }; + for (int i = 0; i < CardanOrders.length; ++i) { + for (int j = 0; j < singularCardanAngle.length; ++j) { + FieldRotation r = new FieldRotation(CardanOrders[i], + field.newDfp(0.1), + field.newDfp(singularCardanAngle[j]), + field.newDfp(0.3)); + try { + r.getAngles(CardanOrders[i]); + Assert.fail("an exception should have been caught"); + } catch (CardanEulerSingularityException cese) { + // expected behavior + } + } + } + + RotationOrder[] EulerOrders = { + RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, + RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ + }; + + double[] singularEulerAngle = { 0, FastMath.PI }; + for (int i = 0; i < EulerOrders.length; ++i) { + for (int j = 0; j < singularEulerAngle.length; ++j) { + FieldRotation r = new FieldRotation(EulerOrders[i], + field.newDfp(0.1), + field.newDfp(singularEulerAngle[j]), + field.newDfp(0.3)); + try { + r.getAngles(EulerOrders[i]); + Assert.fail("an exception should have been caught"); + } catch (CardanEulerSingularityException cese) { + // expected behavior + } + } + } + + + } + + @Test + public void testQuaternion() throws MathIllegalArgumentException { + + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + double n = 23.5; + FieldRotation r2 = new FieldRotation(r1.getQ0().multiply(n), r1.getQ1().multiply(n), + r1.getQ2().multiply(n), r1.getQ3().multiply(n), + true); + for (double x = -0.9; x < 0.9; x += 0.2) { + for (double y = -0.9; y < 0.9; y += 0.2) { + for (double z = -0.9; z < 0.9; z += 0.2) { + FieldVector3D u = createVector(x, y, z); + checkVector(r2.applyTo(u), r1.applyTo(u)); + } + } + } + + r1 = createRotation(0.288, 0.384, 0.36, 0.8, false); + checkRotationDS(r1, + -r1.getQ0().getReal(), -r1.getQ1().getReal(), + -r1.getQ2().getReal(), -r1.getQ3().getReal()); + + } + + @Test + public void testCompose() throws MathIllegalArgumentException { + + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r2 = new FieldRotation(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation r3 = r2.applyTo(r1); + FieldRotation r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(), + r1.getQ1().getReal(), + r1.getQ2().getReal(), + r1.getQ3().getReal(), + false)); + + for (double x = -0.9; x < 0.9; x += 0.2) { + for (double y = -0.9; y < 0.9; y += 0.2) { + for (double z = -0.9; z < 0.9; z += 0.2) { + FieldVector3D u = createVector(x, y, z); + checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u)); + checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u)); + } + } + } + + } + + @Test + public void testComposeInverse() throws MathIllegalArgumentException { + + FieldRotation r1 = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation r2 = new FieldRotation(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation r3 = r2.applyInverseTo(r1); + FieldRotation r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(), + r1.getQ1().getReal(), + r1.getQ2().getReal(), + r1.getQ3().getReal(), + false)); + + for (double x = -0.9; x < 0.9; x += 0.2) { + for (double y = -0.9; y < 0.9; y += 0.2) { + for (double z = -0.9; z < 0.9; z += 0.2) { + FieldVector3D u = createVector(x, y, z); + checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u)); + checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u)); + } + } + } + + } + + @Test + public void testDoubleVectors() throws MathIllegalArgumentException { + + Well1024a random = new Well1024a(0x180b41cfeeffaf67l); + UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random); + for (int i = 0; i < 10; ++i) { + double[] unit = g.nextVector(); + FieldRotation r = new FieldRotation(createVector(unit[0], unit[1], unit[2]), + createAngle(random.nextDouble())); + + for (double x = -0.9; x < 0.9; x += 0.4) { + for (double y = -0.9; y < 0.9; y += 0.4) { + for (double z = -0.9; z < 0.9; z += 0.4) { + FieldVector3D uds = createVector(x, y, z); + FieldVector3D ruds = r.applyTo(uds); + FieldVector3D rIuds = r.applyInverseTo(uds); + Vector3D u = new Vector3D(x, y, z); + FieldVector3D ru = r.applyTo(u); + FieldVector3D rIu = r.applyInverseTo(u); + Dfp[] ruArray = new Dfp[3]; + r.applyTo(new double[] { x, y, z}, ruArray); + Dfp[] rIuArray = new Dfp[3]; + r.applyInverseTo(new double[] { x, y, z}, rIuArray); + checkVector(ruds, ru); + checkVector(ruds, new FieldVector3D(ruArray)); + checkVector(rIuds, rIu); + checkVector(rIuds, new FieldVector3D(rIuArray)); + } + } + } + } + + } + + @Test + public void testDoubleRotations() throws MathIllegalArgumentException { + + DfpField field = new DfpField(20); + Well1024a random = new Well1024a(0x180b41cfeeffaf67l); + UnitSphereRandomVectorGenerator g = new UnitSphereRandomVectorGenerator(3, random); + for (int i = 0; i < 10; ++i) { + double[] unit1 = g.nextVector(); + Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]), + random.nextDouble()); + FieldRotation r1Prime = new FieldRotation(field.newDfp(r1.getQ0()), + field.newDfp(r1.getQ1()), + field.newDfp(r1.getQ2()), + field.newDfp(r1.getQ3()), + false); + double[] unit2 = g.nextVector(); + FieldRotation r2 = new FieldRotation(createVector(unit2[0], unit2[1], unit2[2]), + createAngle(random.nextDouble())); + + FieldRotation rA = FieldRotation.applyTo(r1, r2); + FieldRotation rB = r1Prime.applyTo(r2); + FieldRotation rC = FieldRotation.applyInverseTo(r1, r2); + FieldRotation rD = r1Prime.applyInverseTo(r2); + + for (double x = -0.9; x < 0.9; x += 0.4) { + for (double y = -0.9; y < 0.9; y += 0.4) { + for (double z = -0.9; z < 0.9; z += 0.4) { + + FieldVector3D uds = createVector(x, y, z); + checkVector(r1Prime.applyTo(uds), FieldRotation.applyTo(r1, uds)); + checkVector(r1Prime.applyInverseTo(uds), FieldRotation.applyInverseTo(r1, uds)); + checkVector(rA.applyTo(uds), rB.applyTo(uds)); + checkVector(rA.applyInverseTo(uds), rB.applyInverseTo(uds)); + checkVector(rC.applyTo(uds), rD.applyTo(uds)); + checkVector(rC.applyInverseTo(uds), rD.applyInverseTo(uds)); + + } + } + } + } + + } + + @Test + public void testArray() throws MathIllegalArgumentException { + + FieldRotation r = new FieldRotation(createAxis(2, -3, 5), createAngle(1.7)); + + for (double x = -0.9; x < 0.9; x += 0.2) { + for (double y = -0.9; y < 0.9; y += 0.2) { + for (double z = -0.9; z < 0.9; z += 0.2) { + FieldVector3D u = createVector(x, y, z); + FieldVector3D v = r.applyTo(u); + Dfp[] out = new Dfp[3]; + r.applyTo(new Dfp[] { u.getX(), u.getY(), u.getZ() }, out); + Assert.assertEquals(v.getX().getReal(), out[0].getReal(), 1.0e-10); + Assert.assertEquals(v.getY().getReal(), out[1].getReal(), 1.0e-10); + Assert.assertEquals(v.getZ().getReal(), out[2].getReal(), 1.0e-10); + r.applyInverseTo(out, out); + Assert.assertEquals(u.getX().getReal(), out[0].getReal(), 1.0e-10); + Assert.assertEquals(u.getY().getReal(), out[1].getReal(), 1.0e-10); + Assert.assertEquals(u.getZ().getReal(), out[2].getReal(), 1.0e-10); + } + } + } + + } + + @Test + public void testApplyInverseTo() throws MathIllegalArgumentException { + + Dfp[] in = new Dfp[3]; + Dfp[] out = new Dfp[3]; + Dfp[] rebuilt = new Dfp[3]; + FieldRotation r = new FieldRotation(createVector(2, -3, 5), createAngle(1.7)); + for (double lambda = 0; lambda < 6.2; lambda += 0.2) { + for (double phi = -1.55; phi < 1.55; phi += 0.2) { + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FastMath.sin(lambda) * FastMath.cos(phi), + FastMath.sin(phi)); + r.applyInverseTo(r.applyTo(u)); + checkVector(u, r.applyInverseTo(r.applyTo(u))); + checkVector(u, r.applyTo(r.applyInverseTo(u))); + in[0] = u.getX(); + in[1] = u.getY(); + in[2] = u.getZ(); + r.applyTo(in, out); + r.applyInverseTo(out, rebuilt); + Assert.assertEquals(in[0].getReal(), rebuilt[0].getReal(), 1.0e-12); + Assert.assertEquals(in[1].getReal(), rebuilt[1].getReal(), 1.0e-12); + Assert.assertEquals(in[2].getReal(), rebuilt[2].getReal(), 1.0e-12); + } + } + + r = createRotation(1, 0, 0, 0, false); + for (double lambda = 0; lambda < 6.2; lambda += 0.2) { + for (double phi = -1.55; phi < 1.55; phi += 0.2) { + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FastMath.sin(lambda) * FastMath.cos(phi), + FastMath.sin(phi)); + checkVector(u, r.applyInverseTo(r.applyTo(u))); + checkVector(u, r.applyTo(r.applyInverseTo(u))); + } + } + + r = new FieldRotation(createVector(0, 0, 1), createAngle(FastMath.PI)); + for (double lambda = 0; lambda < 6.2; lambda += 0.2) { + for (double phi = -1.55; phi < 1.55; phi += 0.2) { + FieldVector3D u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), + FastMath.sin(lambda) * FastMath.cos(phi), + FastMath.sin(phi)); + checkVector(u, r.applyInverseTo(r.applyTo(u))); + checkVector(u, r.applyTo(r.applyInverseTo(u))); + } + } + + } + + @Test + public void testIssue639() throws MathArithmeticException{ + FieldVector3D u1 = createVector(-1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -3822921525525679.0 / 4294967296.0); + FieldVector3D u2 =createVector( -5712344449280879.0 / 2097152.0, + -2275058564560979.0 / 1048576.0, + 4423475992255071.0 / 65536.0); + FieldRotation rot = new FieldRotation(u1, u2, createVector(1, 0, 0),createVector(0, 0, 1)); + Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0().getReal(), 1.0e-15); + Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1().getReal(), 1.0e-15); + Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2().getReal(), 1.0e-15); + Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3().getReal(), 1.0e-15); + } + + @Test + public void testIssue801() throws MathArithmeticException { + FieldVector3D u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0); + FieldVector3D u2 = createVector(0.0, 0.0, 1.0); + + FieldVector3D v1 = createVector(0.9999999999999999, 0.0, 0.0); + FieldVector3D v2 = createVector(0.0, 0.0, -1.0); + + FieldRotation quat = new FieldRotation(u1, u2, v1, v2); + double q2 = quat.getQ0().getReal() * quat.getQ0().getReal() + + quat.getQ1().getReal() * quat.getQ1().getReal() + + quat.getQ2().getReal() * quat.getQ2().getReal() + + quat.getQ3().getReal() * quat.getQ3().getReal(); + Assert.assertEquals(1.0, q2, 1.0e-14); + Assert.assertEquals(0.0, v1.angle(quat.applyTo(u1)).getReal(), 1.0e-14); + Assert.assertEquals(0.0, v2.angle(quat.applyTo(u2)).getReal(), 1.0e-14); + + } + + private void checkAngle(Dfp a1, double a2) { + Assert.assertEquals(a1.getReal(), MathUtils.normalizeAngle(a2, a1.getReal()), 1.0e-10); + } + + private void checkRotationDS(FieldRotation r, double q0, double q1, double q2, double q3) { + FieldRotation rPrime = createRotation(q0, q1, q2, q3, false); + Assert.assertEquals(0, FieldRotation.distance(r, rPrime).getReal(), 1.0e-12); + } + + private FieldRotation createRotation(double q0, double q1, double q2, double q3, + boolean needsNormalization) { + DfpField field = new DfpField(20); + return new FieldRotation(field.newDfp(q0), + field.newDfp(q1), + field.newDfp(q2), + field.newDfp(q3), + needsNormalization); + } + + private FieldRotation createRotation(double[][] m, double threshold) { + DfpField field = new DfpField(20); + Dfp[][] mds = new Dfp[m.length][m[0].length]; + for (int i = 0; i < m.length; ++i) { + for (int j = 0; j < m[i].length; ++j) { + mds[i][j] = field.newDfp(m[i][j]); + } + } + return new FieldRotation(mds, threshold); + } + + private FieldVector3D createVector(double x, double y, double z) { + DfpField field = new DfpField(20); + return new FieldVector3D(field.newDfp(x), field.newDfp(y), field.newDfp(z)); + } + + private FieldVector3D createAxis(double x, double y, double z) { + DfpField field = new DfpField(20); + return new FieldVector3D(field.newDfp(x), field.newDfp(y), field.newDfp(z)); + } + + private Dfp createAngle(double alpha) { + return new DfpField(20).newDfp(alpha); + } + + private void checkVector(FieldVector3D u, FieldVector3D v) { + Assert.assertEquals(u.getX().getReal(), v.getX().getReal(), 1.0e-12); + Assert.assertEquals(u.getY().getReal(), v.getY().getReal(), 1.0e-12); + Assert.assertEquals(u.getZ().getReal(), v.getZ().getReal(), 1.0e-12); + } + +} diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3DTest.java similarity index 64% rename from src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java rename to src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3DTest.java index 58ff03234..b80cb1606 100644 --- a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3DTest.java @@ -31,16 +31,17 @@ import org.apache.commons.math3.util.Precision; import org.junit.Assert; import org.junit.Test; -public class Vector3DDSTest { +public class FieldVector3DTest { + @Test public void testConstructors() throws DimensionMismatchException { double cosAlpha = 1 / 2.0; double sinAlpha = FastMath.sqrt(3) / 2.0; double cosDelta = FastMath.sqrt(2) / 2.0; double sinDelta = -FastMath.sqrt(2) / 2.0; - Vector3DDS u = new Vector3DDS(2, - new Vector3DDS(new DerivativeStructure(2, 1, 0, FastMath.PI / 3), - new DerivativeStructure(2, 1, 1, -FastMath.PI / 4))); + FieldVector3D u = new FieldVector3D(2, + new FieldVector3D(new DerivativeStructure(2, 1, 0, FastMath.PI / 3), + new DerivativeStructure(2, 1, 1, -FastMath.PI / 4))); checkVector(u, 2 * cosAlpha * cosDelta, 2 * sinAlpha * cosDelta, 2 * sinDelta); Assert.assertEquals(-2 * sinAlpha * cosDelta, u.getX().getPartialDerivative(1, 0), 1.0e-12); Assert.assertEquals(+2 * cosAlpha * cosDelta, u.getY().getPartialDerivative(1, 0), 1.0e-12); @@ -49,41 +50,41 @@ public class Vector3DDSTest { Assert.assertEquals(-2 * sinAlpha * sinDelta, u.getY().getPartialDerivative(0, 1), 1.0e-12); Assert.assertEquals(2 * cosDelta, u.getZ().getPartialDerivative(0, 1), 1.0e-12); - checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3)), + checkVector(new FieldVector3D(2, createVector(1, 0, 0, 3)), 2, 0, 0, 2, 0, 0, 0, 2, 0, 0, 0, 2); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), createVector(1, 0, 0, 4)), 2, 0, 0, 2, 0, 0, 1, 0, 2, 0, 0, 0, 0, 2, 0); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), new Vector3D(1, 0, 0)), 2, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0); - checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + checkVector(new FieldVector3D(2, createVector(1, 0, 0, 3), -3, createVector(0, 0, -1, 3)), 2, 0, 3, -1, 0, 0, 0, -1, 0, 0, 0, -1); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), createVector(1, 0, 0, 4), new DerivativeStructure(4, 1, 3, -3.0), createVector(0, 0, -1, 4)), 2, 0, 3, -1, 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, -1); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), new Vector3D(1, 0, 0), new DerivativeStructure(4, 1, 3, -3.0), new Vector3D(0, 0, -1)), 2, 0, 3, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1); - checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + checkVector(new FieldVector3D(2, createVector(1, 0, 0, 3), 5, createVector(0, 1, 0, 3), -3, createVector(0, 0, -1, 3)), 2, 5, 3, 4, 0, 0, 0, 4, 0, 0, 0, 4); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), createVector(1, 0, 0, 4), new DerivativeStructure(4, 1, 3, 5.0), createVector(0, 1, 0, 4), new DerivativeStructure(4, 1, 3, -3.0), createVector(0, 0, -1, 4)), 2, 5, 3, 4, 0, 0, 1, 0, 4, 0, 1, 0, 0, 4, -1); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), new Vector3D(1, 0, 0), new DerivativeStructure(4, 1, 3, 5.0), new Vector3D(0, 1, 0), @@ -91,12 +92,12 @@ public class Vector3DDSTest { new Vector3D(0, 0, -1)), 2, 5, 3, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, -1); - checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + checkVector(new FieldVector3D(2, createVector(1, 0, 0, 3), 5, createVector(0, 1, 0, 3), 5, createVector(0, -1, 0, 3), -3, createVector(0, 0, -1, 3)), 2, 0, 3, 9, 0, 0, 0, 9, 0, 0, 0, 9); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), createVector(1, 0, 0, 4), new DerivativeStructure(4, 1, 3, 5.0), createVector(0, 1, 0, 4), @@ -105,7 +106,7 @@ public class Vector3DDSTest { new DerivativeStructure(4, 1, 3, -3.0), createVector(0, 0, -1, 4)), 2, 0, 3, 9, 0, 0, 1, 0, 9, 0, 0, 0, 0, 9, -1); - checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + checkVector(new FieldVector3D(new DerivativeStructure(4, 1, 3, 2.0), new Vector3D(1, 0, 0), new DerivativeStructure(4, 1, 3, 5.0), new Vector3D(0, 1, 0), @@ -115,7 +116,7 @@ public class Vector3DDSTest { new Vector3D(0, 0, -1)), 2, 0, 3, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1); - checkVector(new Vector3DDS(new DerivativeStructure[] { + checkVector(new FieldVector3D(new DerivativeStructure[] { new DerivativeStructure(3, 1, 2, 2), new DerivativeStructure(3, 1, 1, 5), new DerivativeStructure(3, 1, 0, -3) @@ -126,19 +127,19 @@ public class Vector3DDSTest { @Test public void testEquals() { - Vector3DDS u1 = createVector(1, 2, 3, 3); - Vector3DDS v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); + FieldVector3D u1 = createVector(1, 2, 3, 3); + FieldVector3D v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); Assert.assertTrue(u1.equals(u1)); - Assert.assertTrue(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + Assert.assertTrue(u1.equals(new FieldVector3D(new DerivativeStructure(3, 1, 0, 1.0), new DerivativeStructure(3, 1, 1, 2.0), new DerivativeStructure(3, 1, 2, 3.0)))); - Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 1.0), + Assert.assertFalse(u1.equals(new FieldVector3D(new DerivativeStructure(3, 1, 1.0), new DerivativeStructure(3, 1, 1, 2.0), new DerivativeStructure(3, 1, 2, 3.0)))); - Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + Assert.assertFalse(u1.equals(new FieldVector3D(new DerivativeStructure(3, 1, 0, 1.0), new DerivativeStructure(3, 1, 2.0), new DerivativeStructure(3, 1, 2, 3.0)))); - Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + Assert.assertFalse(u1.equals(new FieldVector3D(new DerivativeStructure(3, 1, 0, 1.0), new DerivativeStructure(3, 1, 1, 2.0), new DerivativeStructure(3, 1, 3.0)))); Assert.assertFalse(u1.equals(v)); @@ -149,8 +150,8 @@ public class Vector3DDSTest { @Test public void testHash() { Assert.assertEquals(createVector(0, Double.NaN, 0, 3).hashCode(), createVector(0, 0, Double.NaN, 3).hashCode()); - Vector3DDS u = createVector(1, 2, 3, 3); - Vector3DDS v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); + FieldVector3D u = createVector(1, 2, 3, 3); + FieldVector3D v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); Assert.assertTrue(u.hashCode() != v.hashCode()); } @@ -181,7 +182,7 @@ public class Vector3DDSTest { @Test(expected=DimensionMismatchException.class) public void testWrongDimension() throws DimensionMismatchException { - new Vector3DDS(new DerivativeStructure[] { + new FieldVector3D(new DerivativeStructure[] { new DerivativeStructure(3, 1, 0, 2), new DerivativeStructure(3, 1, 0, 5) }); @@ -189,20 +190,20 @@ public class Vector3DDSTest { @Test public void testCoordinates() { - Vector3DDS v = createVector(1, 2, 3, 3); - Assert.assertTrue(FastMath.abs(v.getX().getValue() - 1) < 1.0e-12); - Assert.assertTrue(FastMath.abs(v.getY().getValue() - 2) < 1.0e-12); - Assert.assertTrue(FastMath.abs(v.getZ().getValue() - 3) < 1.0e-12); + FieldVector3D v = createVector(1, 2, 3, 3); + Assert.assertTrue(FastMath.abs(v.getX().getReal() - 1) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v.getY().getReal() - 2) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v.getZ().getReal() - 3) < 1.0e-12); DerivativeStructure[] coordinates = v.toArray(); - Assert.assertTrue(FastMath.abs(coordinates[0].getValue() - 1) < 1.0e-12); - Assert.assertTrue(FastMath.abs(coordinates[1].getValue() - 2) < 1.0e-12); - Assert.assertTrue(FastMath.abs(coordinates[2].getValue() - 3) < 1.0e-12); + Assert.assertTrue(FastMath.abs(coordinates[0].getReal() - 1) < 1.0e-12); + Assert.assertTrue(FastMath.abs(coordinates[1].getReal() - 2) < 1.0e-12); + Assert.assertTrue(FastMath.abs(coordinates[2].getReal() - 3) < 1.0e-12); } @Test public void testNorm1() { - Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNorm1().getValue(), 0); - Assert.assertEquals( 6.0, createVector(1, -2, 3, 3).getNorm1().getValue(), 0); + Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNorm1().getReal(), 0); + Assert.assertEquals( 6.0, createVector(1, -2, 3, 3).getNorm1().getReal(), 0); Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals(-1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(0, 0, 1), 0); @@ -211,8 +212,8 @@ public class Vector3DDSTest { @Test public void testNorm() { double r = FastMath.sqrt(14); - Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNorm().getValue(), 0); - Assert.assertEquals(r, createVector(1, 2, 3, 3).getNorm().getValue(), 1.0e-12); + Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNorm().getReal(), 0); + Assert.assertEquals(r, createVector(1, 2, 3, 3).getNorm().getReal(), 1.0e-12); Assert.assertEquals( 1.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 2.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 3.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(0, 0, 1), 0); @@ -220,8 +221,8 @@ public class Vector3DDSTest { @Test public void testNormSq() { - Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNormSq().getValue(), 0); - Assert.assertEquals(14, createVector(1, 2, 3, 3).getNormSq().getValue(), 1.0e-12); + Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNormSq().getReal(), 0); + Assert.assertEquals(14, createVector(1, 2, 3, 3).getNormSq().getReal(), 1.0e-12); Assert.assertEquals( 2, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 4, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 6, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(0, 0, 1), 0); @@ -229,28 +230,28 @@ public class Vector3DDSTest { @Test public void testNormInf() { - Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNormInf().getValue(), 0); - Assert.assertEquals( 3.0, createVector(1, -2, 3, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNormInf().getReal(), 0); + Assert.assertEquals( 3.0, createVector(1, -2, 3, 3).getNormInf().getReal(), 0); Assert.assertEquals( 0.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 0.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); - Assert.assertEquals( 3.0, createVector(2, -1, 3, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(2, -1, 3, 3).getNormInf().getReal(), 0); Assert.assertEquals( 0.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 0.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 1.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); - Assert.assertEquals( 3.0, createVector(1, -3, 2, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(1, -3, 2, 3).getNormInf().getReal(), 0); Assert.assertEquals( 0.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals(-1.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 0.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); - Assert.assertEquals( 3.0, createVector(2, -3, 1, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(2, -3, 1, 3).getNormInf().getReal(), 0); Assert.assertEquals( 0.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals(-1.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 0.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); - Assert.assertEquals( 3.0, createVector(3, -1, 2, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(3, -1, 2, 3).getNormInf().getReal(), 0); Assert.assertEquals( 1.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 0.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 0.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); - Assert.assertEquals( 3.0, createVector(3, -2, 1, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(3, -2, 1, 3).getNormInf().getReal(), 0); Assert.assertEquals( 1.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); Assert.assertEquals( 0.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); Assert.assertEquals( 0.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); @@ -258,152 +259,116 @@ public class Vector3DDSTest { @Test public void testDistance1() { - Vector3DDS v1 = createVector(1, -2, 3, 3); - Vector3DDS v2 = createVector(-4, 2, 0, 3); - Assert.assertEquals(0.0, Vector3DDS.distance1(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); - DerivativeStructure distance = Vector3DDS.distance1(v1, v2); - Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); + FieldVector3D v1 = createVector(1, -2, 3, 3); + FieldVector3D v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, createVector(-1, 0, 0, 3).distance1(createVector(-1, 0, 0, 3)).getReal(), 0); + DerivativeStructure distance = v1.distance1(v2); + Assert.assertEquals(12.0, distance.getReal(), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distance1(v1, new Vector3D(-4, 2, 0)); - Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); + distance = v1.distance1(new Vector3D(-4, 2, 0)); + Assert.assertEquals(12.0, distance.getReal(), 1.0e-12); Assert.assertEquals( 1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(-1, distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals( 1, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distance1(new Vector3D(-4, 2, 0), v1); - Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); - Assert.assertEquals( 1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); - Assert.assertEquals(-1, distance.getPartialDerivative(0, 1, 0), 1.0e-12); - Assert.assertEquals( 1, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - Assert.assertEquals(v1.subtract(v2).getNorm1().getValue(), Vector3DDS.distance1(v1, v2).getValue(), 1.0e-12); } @Test public void testDistance() { - Vector3DDS v1 = createVector(1, -2, 3, 3); - Vector3DDS v2 = createVector(-4, 2, 0, 3); - Assert.assertEquals(0.0, Vector3DDS.distance(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); - DerivativeStructure distance = Vector3DDS.distance(v1, v2); - Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); + FieldVector3D v1 = createVector(1, -2, 3, 3); + FieldVector3D v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, createVector(-1, 0, 0, 3).distance(createVector(-1, 0, 0, 3)).getReal(), 0); + DerivativeStructure distance = v1.distance(v2); + Assert.assertEquals(FastMath.sqrt(50), distance.getReal(), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distance(v1, new Vector3D(-4, 2, 0)); - Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); + distance = v1.distance(new Vector3D(-4, 2, 0)); + Assert.assertEquals(FastMath.sqrt(50), distance.getReal(), 1.0e-12); Assert.assertEquals( 5 / FastMath.sqrt(50), distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(-4 / FastMath.sqrt(50), distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals( 3 / FastMath.sqrt(50), distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distance(new Vector3D(-4, 2, 0), v1); - Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); - Assert.assertEquals( 5 / FastMath.sqrt(50), distance.getPartialDerivative(1, 0, 0), 1.0e-12); - Assert.assertEquals(-4 / FastMath.sqrt(50), distance.getPartialDerivative(0, 1, 0), 1.0e-12); - Assert.assertEquals( 3 / FastMath.sqrt(50), distance.getPartialDerivative(0, 0, 1), 1.0e-12); - Assert.assertEquals(v1.subtract(v2).getNorm().getValue(), Vector3DDS.distance(v1, v2).getValue(), 1.0e-12); } @Test public void testDistanceSq() { - Vector3DDS v1 = createVector(1, -2, 3, 3); - Vector3DDS v2 = createVector(-4, 2, 0, 3); - Assert.assertEquals(0.0, Vector3DDS.distanceSq(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); - DerivativeStructure distanceSq = Vector3DDS.distanceSq(v1, v2); - Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); + FieldVector3D v1 = createVector(1, -2, 3, 3); + FieldVector3D v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, createVector(-1, 0, 0, 3).distanceSq(createVector(-1, 0, 0, 3)).getReal(), 0); + DerivativeStructure distanceSq = v1.distanceSq(v2); + Assert.assertEquals(50.0, distanceSq.getReal(), 1.0e-12); Assert.assertEquals(0, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(0, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(0, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); - distanceSq = Vector3DDS.distanceSq(v1, new Vector3D(-4, 2, 0)); - Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); + distanceSq = v1.distanceSq(new Vector3D(-4, 2, 0)); + Assert.assertEquals(50.0, distanceSq.getReal(), 1.0e-12); Assert.assertEquals(10, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(-8, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals( 6, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); - distanceSq = Vector3DDS.distanceSq(new Vector3D(-4, 2, 0), v1); - Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); - Assert.assertEquals(10, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); - Assert.assertEquals(-8, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); - Assert.assertEquals( 6, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); - Assert.assertEquals(Vector3DDS.distance(v1, v2).multiply(Vector3DDS.distance(v1, v2)).getValue(), - Vector3DDS.distanceSq(v1, v2).getValue(), 1.0e-12); } @Test public void testDistanceInf() { - Vector3DDS v1 = createVector(1, -2, 3, 3); - Vector3DDS v2 = createVector(-4, 2, 0, 3); - Assert.assertEquals(0.0, Vector3DDS.distanceInf(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); - DerivativeStructure distance = Vector3DDS.distanceInf(v1, v2); - Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); + FieldVector3D v1 = createVector(1, -2, 3, 3); + FieldVector3D v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, createVector(-1, 0, 0, 3).distanceInf(createVector(-1, 0, 0, 3)).getReal(), 0); + DerivativeStructure distance = v1.distanceInf(v2); + Assert.assertEquals(5.0, distance.getReal(), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distanceInf(v1, new Vector3D(-4, 2, 0)); - Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); + distance = v1.distanceInf(new Vector3D(-4, 2, 0)); + Assert.assertEquals(5.0, distance.getReal(), 1.0e-12); Assert.assertEquals(1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - distance = Vector3DDS.distanceInf(new Vector3D(-4, 2, 0), v1); - Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); - Assert.assertEquals(1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); - Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); - Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); - Assert.assertEquals(v1.subtract(v2).getNormInf().getValue(), Vector3DDS.distanceInf(v1, v2).getValue(), 1.0e-12); + Assert.assertEquals(v1.subtract(v2).getNormInf().getReal(), v1.distanceInf(v2).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector( 1, -2, 3, 3), - createVector(-4, 2, 0, 3)).getValue(), + createVector( 1, -2, 3, 3).distanceInf(createVector(-4, 2, 0, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector( 1, 3, -2, 3), - createVector(-4, 0, 2, 3)).getValue(), + createVector( 1, 3, -2, 3).distanceInf(createVector(-4, 0, 2, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(-2, 1, 3, 3), - createVector( 2, -4, 0, 3)).getValue(), + createVector(-2, 1, 3, 3).distanceInf(createVector( 2, -4, 0, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(-2, 3, 1, 3), - createVector( 2, 0, -4, 3)).getValue(), + createVector(-2, 3, 1, 3).distanceInf(createVector( 2, 0, -4, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(3, -2, 1, 3), - createVector(0, 2, -4, 3)).getValue(), + createVector(3, -2, 1, 3).distanceInf(createVector(0, 2, -4, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(3, 1, -2, 3), - createVector(0, -4, 2, 3)).getValue(), + createVector(3, 1, -2, 3).distanceInf(createVector(0, -4, 2, 3)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector( 1, -2, 3, 3), - new Vector3D(-4, 2, 0)).getValue(), + createVector( 1, -2, 3, 3).distanceInf(new Vector3D(-4, 2, 0)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector( 1, 3, -2, 3), - new Vector3D(-4, 0, 2)).getValue(), + createVector( 1, 3, -2, 3).distanceInf(new Vector3D(-4, 0, 2)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(-2, 1, 3, 3), - new Vector3D( 2, -4, 0)).getValue(), + createVector(-2, 1, 3, 3).distanceInf(new Vector3D( 2, -4, 0)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(-2, 3, 1, 3), - new Vector3D( 2, 0, -4)).getValue(), + createVector(-2, 3, 1, 3).distanceInf(new Vector3D( 2, 0, -4)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(3, -2, 1, 3), - new Vector3D(0, 2, -4)).getValue(), + createVector(3, -2, 1, 3).distanceInf(new Vector3D(0, 2, -4)).getReal(), 1.0e-12); Assert.assertEquals(5.0, - Vector3DDS.distanceInf(createVector(3, 1, -2, 3), - new Vector3D(0, -4, 2)).getValue(), + createVector(3, 1, -2, 3).distanceInf(new Vector3D(0, -4, 2)).getReal(), 1.0e-12); } @Test public void testSubtract() { - Vector3DDS v1 = createVector(1, 2, 3, 3); - Vector3DDS v2 = createVector(-3, -2, -1, 3); + FieldVector3D v1 = createVector(1, 2, 3, 3); + FieldVector3D v2 = createVector(-3, -2, -1, 3); v1 = v1.subtract(v2); checkVector(v1, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0); @@ -425,8 +390,8 @@ public class Vector3DDSTest { @Test public void testAdd() { - Vector3DDS v1 = createVector(1, 2, 3, 3); - Vector3DDS v2 = createVector(-3, -2, -1, 3); + FieldVector3D v1 = createVector(1, 2, 3, 3); + FieldVector3D v2 = createVector(-3, -2, -1, 3); v1 = v1.add(v2); checkVector(v1, -2, 0, 2, 2, 0, 0, 0, 2, 0, 0, 0, 2); @@ -448,7 +413,7 @@ public class Vector3DDSTest { @Test public void testScalarProduct() { - Vector3DDS v = createVector(1, 2, 3, 3); + FieldVector3D v = createVector(1, 2, 3, 3); v = v.scalarMultiply(3); checkVector(v, 3, 6, 9); @@ -457,58 +422,58 @@ public class Vector3DDSTest { @Test public void testVectorialProducts() { - Vector3DDS v1 = createVector(2, 1, -4, 3); - Vector3DDS v2 = createVector(3, 1, -1, 3); + FieldVector3D v1 = createVector(2, 1, -4, 3); + FieldVector3D v2 = createVector(3, 1, -1, 3); - Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v1, v2).getValue() - 11) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v1.dotProduct(v2).getReal() - 11) < 1.0e-12); - Vector3DDS v3 = Vector3DDS.crossProduct(v1, v2); + FieldVector3D v3 = v1.crossProduct(v2); checkVector(v3, 3, -10, -1); - Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v1, v3).getValue()) < 1.0e-12); - Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v2, v3).getValue()) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v1.dotProduct(v3).getReal()) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v2.dotProduct(v3).getReal()) < 1.0e-12); } @Test public void testCrossProductCancellation() { - Vector3DDS v1 = createVector(9070467121.0, 4535233560.0, 1, 3); - Vector3DDS v2 = createVector(9070467123.0, 4535233561.0, 1, 3); - checkVector(Vector3DDS.crossProduct(v1, v2), -1, 2, 1); + FieldVector3D v1 = createVector(9070467121.0, 4535233560.0, 1, 3); + FieldVector3D v2 = createVector(9070467123.0, 4535233561.0, 1, 3); + checkVector(v1.crossProduct(v2), -1, 2, 1); double scale = FastMath.scalb(1.0, 100); - Vector3DDS big1 = new Vector3DDS(scale, v1); - Vector3DDS small2 = new Vector3DDS(1 / scale, v2); - checkVector(Vector3DDS.crossProduct(big1, small2), -1, 2, 1); + FieldVector3D big1 = new FieldVector3D(scale, v1); + FieldVector3D small2 = new FieldVector3D(1 / scale, v2); + checkVector(big1.crossProduct(small2), -1, 2, 1); } @Test public void testAngular() { - Assert.assertEquals(0, createVector(1, 0, 0, 3).getAlpha().getValue(), 1.0e-10); - Assert.assertEquals(0, createVector(1, 0, 0, 3).getDelta().getValue(), 1.0e-10); - Assert.assertEquals(FastMath.PI / 2, createVector(0, 1, 0, 3).getAlpha().getValue(), 1.0e-10); - Assert.assertEquals(0, createVector(0, 1, 0, 3).getDelta().getValue(), 1.0e-10); - Assert.assertEquals(FastMath.PI / 2, createVector(0, 0, 1, 3).getDelta().getValue(), 1.0e-10); + Assert.assertEquals(0, createVector(1, 0, 0, 3).getAlpha().getReal(), 1.0e-10); + Assert.assertEquals(0, createVector(1, 0, 0, 3).getDelta().getReal(), 1.0e-10); + Assert.assertEquals(FastMath.PI / 2, createVector(0, 1, 0, 3).getAlpha().getReal(), 1.0e-10); + Assert.assertEquals(0, createVector(0, 1, 0, 3).getDelta().getReal(), 1.0e-10); + Assert.assertEquals(FastMath.PI / 2, createVector(0, 0, 1, 3).getDelta().getReal(), 1.0e-10); - Vector3DDS u = createVector(-1, 1, -1, 3); - Assert.assertEquals(3 * FastMath.PI /4, u.getAlpha().getValue(), 1.0e-10); - Assert.assertEquals(-1.0 / FastMath.sqrt(3), u.getDelta().sin().getValue(), 1.0e-10); + FieldVector3D u = createVector(-1, 1, -1, 3); + Assert.assertEquals(3 * FastMath.PI /4, u.getAlpha().getReal(), 1.0e-10); + Assert.assertEquals(-1.0 / FastMath.sqrt(3), u.getDelta().sin().getReal(), 1.0e-10); } @Test public void testAngularSeparation() throws MathArithmeticException { - Vector3DDS v1 = createVector(2, -1, 4, 3); + FieldVector3D v1 = createVector(2, -1, 4, 3); - Vector3DDS k = v1.normalize(); - Vector3DDS i = k.orthogonal(); - Vector3DDS v2 = k.scalarMultiply(FastMath.cos(1.2)).add(i.scalarMultiply(FastMath.sin(1.2))); + FieldVector3D k = v1.normalize(); + FieldVector3D i = k.orthogonal(); + FieldVector3D v2 = k.scalarMultiply(FastMath.cos(1.2)).add(i.scalarMultiply(FastMath.sin(1.2))); - Assert.assertTrue(FastMath.abs(Vector3DDS.angle(v1, v2).getValue() - 1.2) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v1.angle(v2).getReal() - 1.2) < 1.0e-12); } @Test public void testNormalize() throws MathArithmeticException { - Assert.assertEquals(1.0, createVector(5, -4, 2, 3).normalize().getNorm().getValue(), 1.0e-12); + Assert.assertEquals(1.0, createVector(5, -4, 2, 3).normalize().getNorm().getReal(), 1.0e-12); try { createVector(0, 0, 0, 3).normalize(); Assert.fail("an exception should have been thrown"); @@ -525,14 +490,14 @@ public class Vector3DDSTest { @Test public void testOrthogonal() throws MathArithmeticException { - Vector3DDS v1 = createVector(0.1, 2.5, 1.3, 3); - Assert.assertEquals(0.0, Vector3DDS.dotProduct(v1, v1.orthogonal()).getValue(), 1.0e-12); - Vector3DDS v2 = createVector(2.3, -0.003, 7.6, 3); - Assert.assertEquals(0.0, Vector3DDS.dotProduct(v2, v2.orthogonal()).getValue(), 1.0e-12); - Vector3DDS v3 = createVector(-1.7, 1.4, 0.2, 3); - Assert.assertEquals(0.0, Vector3DDS.dotProduct(v3, v3.orthogonal()).getValue(), 1.0e-12); - Vector3DDS v4 = createVector(4.2, 0.1, -1.8, 3); - Assert.assertEquals(0.0, Vector3DDS.dotProduct(v4, v4.orthogonal()).getValue(), 1.0e-12); + FieldVector3D v1 = createVector(0.1, 2.5, 1.3, 3); + Assert.assertEquals(0.0, v1.dotProduct(v1.orthogonal()).getReal(), 1.0e-12); + FieldVector3D v2 = createVector(2.3, -0.003, 7.6, 3); + Assert.assertEquals(0.0, v2.dotProduct(v2.orthogonal()).getReal(), 1.0e-12); + FieldVector3D v3 = createVector(-1.7, 1.4, 0.2, 3); + Assert.assertEquals(0.0, v3.dotProduct(v3.orthogonal()).getReal(), 1.0e-12); + FieldVector3D v4 = createVector(4.2, 0.1, -1.8, 3); + Assert.assertEquals(0.0, v4.dotProduct(v4.orthogonal()).getReal(), 1.0e-12); try { createVector(0, 0, 0, 3).orthogonal(); Assert.fail("an exception should have been thrown"); @@ -544,16 +509,16 @@ public class Vector3DDSTest { @Test public void testAngle() throws MathArithmeticException { Assert.assertEquals(0.22572612855273393616, - Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(4, 5, 6, 3)).getValue(), + createVector(1, 2, 3, 3).angle(createVector(4, 5, 6, 3)).getReal(), 1.0e-12); Assert.assertEquals(7.98595620686106654517199e-8, - Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(2, 4, 6.000001, 3)).getValue(), + createVector(1, 2, 3, 3).angle(createVector(2, 4, 6.000001, 3)).getReal(), 1.0e-12); Assert.assertEquals(3.14159257373023116985197793156, - Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(-2, -4, -6.000001, 3)).getValue(), + createVector(1, 2, 3, 3).angle(createVector(-2, -4, -6.000001, 3)).getReal(), 1.0e-12); try { - Vector3DDS.angle(createVector(0, 0, 0, 3), createVector(1, 0, 0, 3)); + createVector(0, 0, 0, 3).angle(createVector(1, 0, 0, 3)); Assert.fail("an exception should have been thrown"); } catch (MathArithmeticException ae) { // expected behavior @@ -565,16 +530,16 @@ public class Vector3DDSTest { // 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... - Vector3DDS u1 = createVector(-1321008684645961.0 / 268435456.0, + FieldVector3D u1 = createVector(-1321008684645961.0 / 268435456.0, -5774608829631843.0 / 268435456.0, -7645843051051357.0 / 8589934592.0, 3); - Vector3DDS u2 = createVector(-5712344449280879.0 / 2097152.0, + FieldVector3D u2 = createVector(-5712344449280879.0 / 2097152.0, -4550117129121957.0 / 2097152.0, 8846951984510141.0 / 131072.0, 3); DerivativeStructure sNaive = u1.getX().multiply(u2.getX()).add(u1.getY().multiply(u2.getY())).add(u1.getZ().multiply(u2.getZ())); DerivativeStructure sAccurate = u1.dotProduct(u2); - Assert.assertEquals(0.0, sNaive.getValue(), 1.0e-30); - Assert.assertEquals(-2088690039198397.0 / 1125899906842624.0, sAccurate.getValue(), 1.0e-16); + Assert.assertEquals(0.0, sNaive.getReal(), 1.0e-30); + Assert.assertEquals(-2088690039198397.0 / 1125899906842624.0, sAccurate.getReal(), 1.0e-16); } @Test @@ -591,24 +556,18 @@ public class Vector3DDSTest { double vz = 10000 * random.nextDouble(); double sNaive = ux * vx + uy * vy + uz * vz; - Vector3DDS uds = createVector(ux, uy, uz, 3); - Vector3DDS vds = createVector(vx, vy, vz, 3); + FieldVector3D uds = createVector(ux, uy, uz, 3); + FieldVector3D vds = createVector(vx, vy, vz, 3); Vector3D v = new Vector3D(vx, vy, vz); - DerivativeStructure sAccurate = Vector3DDS.dotProduct(uds, vds); - Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); + DerivativeStructure sAccurate = uds.dotProduct(vds); + Assert.assertEquals(sNaive, sAccurate.getReal(), 2.5e-16 * sNaive); Assert.assertEquals(ux + vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); Assert.assertEquals(uy + vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); Assert.assertEquals(uz + vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); - sAccurate = Vector3DDS.dotProduct(uds, v); - Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); - Assert.assertEquals(vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); - Assert.assertEquals(vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); - Assert.assertEquals(vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); - - sAccurate = Vector3DDS.dotProduct(v, uds); - Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); + sAccurate = uds.dotProduct(v); + Assert.assertEquals(sNaive, sAccurate.getReal(), 2.5e-16 * sNaive); Assert.assertEquals(vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); Assert.assertEquals(vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); Assert.assertEquals(vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); @@ -623,21 +582,21 @@ public class Vector3DDSTest { // 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 Vector3DDS u1 = createVector(-1321008684645961.0 / 268435456.0, + final FieldVector3D u1 = createVector(-1321008684645961.0 / 268435456.0, -5774608829631843.0 / 268435456.0, -7645843051051357.0 / 8589934592.0, 3); - final Vector3DDS u2 = createVector( 1796571811118507.0 / 2147483648.0, + final FieldVector3D u2 = createVector( 1796571811118507.0 / 2147483648.0, 7853468008299307.0 / 2147483648.0, 2599586637357461.0 / 17179869184.0, 3); - final Vector3DDS u3 = createVector(12753243807587107.0 / 18446744073709551616.0, + final FieldVector3D u3 = createVector(12753243807587107.0 / 18446744073709551616.0, -2313766922703915.0 / 18446744073709551616.0, -227970081415313.0 / 288230376151711744.0, 3); - Vector3DDS cNaive = new Vector3DDS(u1.getY().multiply(u2.getZ()).subtract(u1.getZ().multiply(u2.getY())), + FieldVector3D cNaive = new FieldVector3D(u1.getY().multiply(u2.getZ()).subtract(u1.getZ().multiply(u2.getY())), u1.getZ().multiply(u2.getX()).subtract(u1.getX().multiply(u2.getZ())), u1.getX().multiply(u2.getY()).subtract(u1.getY().multiply(u2.getX()))); - Vector3DDS cAccurate = u1.crossProduct(u2); - Assert.assertTrue(u3.distance(cNaive).getValue() > 2.9 * u3.getNorm().getValue()); - Assert.assertEquals(0.0, u3.distance(cAccurate).getValue(), 1.0e-30 * cAccurate.getNorm().getValue()); + FieldVector3D cAccurate = u1.crossProduct(u2); + Assert.assertTrue(u3.distance(cNaive).getReal() > 2.9 * u3.getNorm().getReal()); + Assert.assertEquals(0.0, u3.distance(cAccurate).getReal(), 1.0e-30 * cAccurate.getNorm().getReal()); } @Test @@ -654,50 +613,44 @@ public class Vector3DDSTest { double vz = random.nextDouble(); Vector3D cNaive = new Vector3D(uy * vz - uz * vy, uz * vx - ux * vz, ux * vy - uy * vx); - Vector3DDS uds = createVector(ux, uy, uz, 3); - Vector3DDS vds = createVector(vx, vy, vz, 3); + FieldVector3D uds = createVector(ux, uy, uz, 3); + FieldVector3D vds = createVector(vx, vy, vz, 3); Vector3D v = new Vector3D(vx, vy, vz); - checkVector(Vector3DDS.crossProduct(uds, vds), + checkVector(uds.crossProduct(vds), cNaive.getX(), cNaive.getY(), cNaive.getZ(), 0, vz - uz, uy - vy, uz - vz, 0, vx - ux, vy - uy, ux - vx, 0); - checkVector(Vector3DDS.crossProduct(uds, v), + checkVector(uds.crossProduct(v), cNaive.getX(), cNaive.getY(), cNaive.getZ(), 0, vz, -vy, -vz, 0, vx, vy, -vx, 0); - checkVector(Vector3DDS.crossProduct(v, uds), - -cNaive.getX(), -cNaive.getY(), -cNaive.getZ(), - 0, -vz, vy, - vz, 0, -vx, - -vy, vx, 0); - } } - private Vector3DDS createVector(double x, double y, double z, int params) { - return new Vector3DDS(new DerivativeStructure(params, 1, 0, x), + private FieldVector3D createVector(double x, double y, double z, int params) { + return new FieldVector3D(new DerivativeStructure(params, 1, 0, x), new DerivativeStructure(params, 1, 1, y), new DerivativeStructure(params, 1, 2, z)); } - private void checkVector(Vector3DDS v, double x, double y, double z) { - Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); - Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); - Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + private void checkVector(FieldVector3D v, double x, double y, double z) { + Assert.assertEquals(x, v.getX().getReal(), 1.0e-12); + Assert.assertEquals(y, v.getY().getReal(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getReal(), 1.0e-12); } - private void checkVector(Vector3DDS v, double x, double y, double z, + private void checkVector(FieldVector3D v, double x, double y, double z, double dxdx, double dxdy, double dxdz, double dydx, double dydy, double dydz, double dzdx, double dzdy, double dzdz) { - Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); - Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); - Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + Assert.assertEquals(x, v.getX().getReal(), 1.0e-12); + Assert.assertEquals(y, v.getY().getReal(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getReal(), 1.0e-12); Assert.assertEquals(dxdx, v.getX().getPartialDerivative(1, 0, 0), 1.0e-12); Assert.assertEquals(dxdy, v.getX().getPartialDerivative(0, 1, 0), 1.0e-12); Assert.assertEquals(dxdz, v.getX().getPartialDerivative(0, 0, 1), 1.0e-12); @@ -709,13 +662,13 @@ public class Vector3DDSTest { Assert.assertEquals(dzdz, v.getZ().getPartialDerivative(0, 0, 1), 1.0e-12); } - private void checkVector(Vector3DDS v, double x, double y, double z, + private void checkVector(FieldVector3D v, double x, double y, double z, double dxdx, double dxdy, double dxdz, double dxdt, double dydx, double dydy, double dydz, double dydt, double dzdx, double dzdy, double dzdz, double dzdt) { - Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); - Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); - Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + Assert.assertEquals(x, v.getX().getReal(), 1.0e-12); + Assert.assertEquals(y, v.getY().getReal(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getReal(), 1.0e-12); Assert.assertEquals(dxdx, v.getX().getPartialDerivative(1, 0, 0, 0), 1.0e-12); Assert.assertEquals(dxdy, v.getX().getPartialDerivative(0, 1, 0, 0), 1.0e-12); Assert.assertEquals(dxdz, v.getX().getPartialDerivative(0, 0, 1, 0), 1.0e-12); diff --git a/src/test/java/org/apache/commons/math3/util/MathArraysTest.java b/src/test/java/org/apache/commons/math3/util/MathArraysTest.java index dcb5a8b96..e2e9ec35f 100644 --- a/src/test/java/org/apache/commons/math3/util/MathArraysTest.java +++ b/src/test/java/org/apache/commons/math3/util/MathArraysTest.java @@ -15,17 +15,15 @@ package org.apache.commons.math3.util; import java.util.Arrays; -import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; -import org.apache.commons.math3.exception.NonMonotonicSequenceException; +import org.apache.commons.math3.TestUtils; import org.apache.commons.math3.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.NonMonotonicSequenceException; import org.apache.commons.math3.exception.NotPositiveException; import org.apache.commons.math3.exception.NotStrictlyPositiveException; import org.apache.commons.math3.exception.NullArgumentException; -import org.apache.commons.math3.exception.MathArithmeticException; -import org.apache.commons.math3.exception.MathIllegalArgumentException; import org.apache.commons.math3.random.Well1024a; -import org.apache.commons.math3.TestUtils; - import org.junit.Assert; import org.junit.Test; @@ -720,149 +718,6 @@ public class MathArraysTest { Assert.assertTrue(Double.isNaN(MathArrays.linearCombination(a[7], b[7]))); } - @Test - public void testLinearCombination1DSDS() { - final DerivativeStructure[] a = new DerivativeStructure[] { - new DerivativeStructure(6, 1, 0, -1321008684645961.0 / 268435456.0), - new DerivativeStructure(6, 1, 1, -5774608829631843.0 / 268435456.0), - new DerivativeStructure(6, 1, 2, -7645843051051357.0 / 8589934592.0) - }; - final DerivativeStructure[] b = new DerivativeStructure[] { - new DerivativeStructure(6, 1, 3, -5712344449280879.0 / 2097152.0), - new DerivativeStructure(6, 1, 4, -4550117129121957.0 / 2097152.0), - new DerivativeStructure(6, 1, 5, 8846951984510141.0 / 131072.0) - }; - - final DerivativeStructure abSumInline = MathArrays.linearCombination(a[0], b[0], - a[1], b[1], - a[2], b[2]); - final DerivativeStructure abSumArray = MathArrays.linearCombination(a, b); - - Assert.assertEquals(abSumInline.getValue(), abSumArray.getValue(), 0); - Assert.assertEquals(-1.8551294182586248737720779899, abSumInline.getValue(), 1.0e-15); - Assert.assertEquals(b[0].getValue(), abSumInline.getPartialDerivative(1, 0, 0, 0, 0, 0), 1.0e-15); - Assert.assertEquals(b[1].getValue(), abSumInline.getPartialDerivative(0, 1, 0, 0, 0, 0), 1.0e-15); - Assert.assertEquals(b[2].getValue(), abSumInline.getPartialDerivative(0, 0, 1, 0, 0, 0), 1.0e-15); - Assert.assertEquals(a[0].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 1, 0, 0), 1.0e-15); - Assert.assertEquals(a[1].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 0, 1, 0), 1.0e-15); - Assert.assertEquals(a[2].getValue(), abSumInline.getPartialDerivative(0, 0, 0, 0, 0, 1), 1.0e-15); - - } - - @Test - public void testLinearCombination1DoubleDS() { - final double[] a = new double[] { - -1321008684645961.0 / 268435456.0, - -5774608829631843.0 / 268435456.0, - -7645843051051357.0 / 8589934592.0 - }; - final DerivativeStructure[] b = new DerivativeStructure[] { - new DerivativeStructure(3, 1, 0, -5712344449280879.0 / 2097152.0), - new DerivativeStructure(3, 1, 1, -4550117129121957.0 / 2097152.0), - new DerivativeStructure(3, 1, 2, 8846951984510141.0 / 131072.0) - }; - - final DerivativeStructure abSumInline = MathArrays.linearCombination(a[0], b[0], - a[1], b[1], - a[2], b[2]); - final DerivativeStructure abSumArray = MathArrays.linearCombination(a, b); - - Assert.assertEquals(abSumInline.getValue(), abSumArray.getValue(), 0); - Assert.assertEquals(-1.8551294182586248737720779899, abSumInline.getValue(), 1.0e-15); - Assert.assertEquals(a[0], abSumInline.getPartialDerivative(1, 0, 0), 1.0e-15); - Assert.assertEquals(a[1], abSumInline.getPartialDerivative(0, 1, 0), 1.0e-15); - Assert.assertEquals(a[2], abSumInline.getPartialDerivative(0, 0, 1), 1.0e-15); - - } - - @Test - public void testLinearCombination2DSDS() { - // 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(0xc6af886975069f11l); - - for (int i = 0; i < 10000; ++i) { - final DerivativeStructure[] u = new DerivativeStructure[4]; - final DerivativeStructure[] v = new DerivativeStructure[4]; - for (int j = 0; j < u.length; ++j) { - u[j] = new DerivativeStructure(u.length, 1, j, 1e17 * random.nextDouble()); - v[j] = new DerivativeStructure(u.length, 1, 1e17 * random.nextDouble()); - } - - DerivativeStructure lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1]); - double ref = u[0].getValue() * v[0].getValue() + - u[1].getValue() * v[1].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - - lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1], u[2], v[2]); - ref = u[0].getValue() * v[0].getValue() + - u[1].getValue() * v[1].getValue() + - u[2].getValue() * v[2].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - Assert.assertEquals(v[2].getValue(), lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); - - lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1], u[2], v[2], u[3], v[3]); - ref = u[0].getValue() * v[0].getValue() + - u[1].getValue() * v[1].getValue() + - u[2].getValue() * v[2].getValue() + - u[3].getValue() * v[3].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(v[0].getValue(), lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(v[1].getValue(), lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - Assert.assertEquals(v[2].getValue(), lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); - Assert.assertEquals(v[3].getValue(), lin.getPartialDerivative(0, 0, 0, 1), 1.0e-15 * FastMath.abs(v[3].getValue())); - - } - } - - @Test - public void testLinearCombination2DoubleDS() { - // 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(0xc6af886975069f11l); - - for (int i = 0; i < 10000; ++i) { - final double[] u = new double[4]; - final DerivativeStructure[] v = new DerivativeStructure[4]; - for (int j = 0; j < u.length; ++j) { - u[j] = 1e17 * random.nextDouble(); - v[j] = new DerivativeStructure(u.length, 1, j, 1e17 * random.nextDouble()); - } - - DerivativeStructure lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1]); - double ref = u[0] * v[0].getValue() + - u[1] * v[1].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - - lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1], u[2], v[2]); - ref = u[0] * v[0].getValue() + - u[1] * v[1].getValue() + - u[2] * v[2].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - Assert.assertEquals(u[2], lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); - - lin = MathArrays.linearCombination(u[0], v[0], u[1], v[1], u[2], v[2], u[3], v[3]); - ref = u[0] * v[0].getValue() + - u[1] * v[1].getValue() + - u[2] * v[2].getValue() + - u[3] * v[3].getValue(); - Assert.assertEquals(ref, lin.getValue(), 1.0e-15 * FastMath.abs(ref)); - Assert.assertEquals(u[0], lin.getPartialDerivative(1, 0, 0, 0), 1.0e-15 * FastMath.abs(v[0].getValue())); - Assert.assertEquals(u[1], lin.getPartialDerivative(0, 1, 0, 0), 1.0e-15 * FastMath.abs(v[1].getValue())); - Assert.assertEquals(u[2], lin.getPartialDerivative(0, 0, 1, 0), 1.0e-15 * FastMath.abs(v[2].getValue())); - Assert.assertEquals(u[3], lin.getPartialDerivative(0, 0, 0, 1), 1.0e-15 * FastMath.abs(v[3].getValue())); - - } - } - @Test public void testArrayEquals() { Assert.assertFalse(MathArrays.equals(new double[] { 1d }, null)); diff --git a/src/test/java/org/apache/commons/math3/util/MathUtilsTest.java b/src/test/java/org/apache/commons/math3/util/MathUtilsTest.java index 30274933b..89bbd0549 100644 --- a/src/test/java/org/apache/commons/math3/util/MathUtilsTest.java +++ b/src/test/java/org/apache/commons/math3/util/MathUtilsTest.java @@ -13,13 +13,13 @@ */ package org.apache.commons.math3.util; +import org.apache.commons.math3.distribution.RealDistribution; +import org.apache.commons.math3.distribution.UniformRealDistribution; import org.apache.commons.math3.exception.MathArithmeticException; import org.apache.commons.math3.exception.NotFiniteNumberException; import org.apache.commons.math3.exception.NullArgumentException; import org.apache.commons.math3.exception.util.LocalizedFormats; import org.apache.commons.math3.random.RandomDataImpl; -import org.apache.commons.math3.distribution.RealDistribution; -import org.apache.commons.math3.distribution.UniformRealDistribution; import org.junit.Assert; import org.junit.Test;