Apache
/
commons-math
mirror of https://github.com/apache/commons-math.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Luc Maisonobe 2013-02-24 19:13:17 +00:00
parent 3ffdf6246a
commit 49d94ad78c
18 changed files with 3904 additions and 2533 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/changes/changes.xml
View File

@ -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.
">
<action dev="luc" type="add" >
Added ExtendFieldElement interface to represent anything that is
real number like, implemented by both Decimal64, Dfp and DerivativeStructure.
</action>
<action dev="luc" type="add" >
Added partial derivatives computation for 3D vectors and rotations.
</action>
<action dev="luc" type="fix" issue="MATH-935" >
<action dev="luc" type="fix" issue="MATH-935" >
Fixed DerivativeStructure.atan2 for special cases when both arguments are +/-0.
</action>
<action dev="luc" type="add" >

490
src/main/java/org/apache/commons/math3/ExtendedFieldElement.java Normal file
View File

@ -0,0 +1,490 @@
package org.apache.commons.math3;
import org.apache.commons.math3.exception.DimensionMismatchException;
/**
* Interface representing a <a href="http://mathworld.wolfram.com/RealNumber.html">real</a>
* <a href="http://mathworld.wolfram.com/Field.html">field</a>.
* <p>
* Classes implementing this interface will often be singletons.
* </p>
* @param <T> the type of the field elements
* @see FieldElement
* @version $Id$
* @since 3.2
*/
public interface ExtendedFieldElement<T> extends FieldElement<T> {
/** 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);
/** '&times;' operator.
* @param a right hand side parameter of the operator
* @return this&times;a
*/
T multiply(double a);
/** '&divides;' operator.
* @param a right hand side parameter of the operator
* @return this&divides;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 &times; 2<sup>n</sup>
*/
T scalb(int n);
/**
* Returns the hypotenuse of a triangle with sides {@code this} and {@code y}
* - sqrt(<i>this</i><sup>2</sup>&nbsp;+<i>y</i><sup>2</sup>)<br/>
* avoiding intermediate overflow or underflow.
*
* <ul>
* <li> If either argument is infinite, then the result is positive infinity.</li>
* <li> else, if either argument is NaN then the result is NaN.</li>
* </ul>
*
* @param y a value
* @return sqrt(<i>this</i><sup>2</sup>&nbsp;+<i>y</i><sup>2</sup>)
* @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();
/** N<sup>th</sup> root.
* @param n order of the root
* @return n<sup>th</sup> root of the instance
*/
T rootN(int n);
/** Power operation.
* @param p power to apply
* @return this<sup>p</sup>
*/
T pow(double p);
/** Integer power operation.
* @param n power to apply
* @return this<sup>n</sup>
*/
T pow(int n);
/** Power operation.
* @param e exponent
* @return this<sup>e</sup>
* @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
* <code>a<sub>i</sub> b<sub>i</sub></code> to high accuracy.
* It does so by using specific multiplication and addition algorithms to
* preserve accuracy and reduce cancellation effects.
* <br/>
* It is based on the 2005 paper
* <a href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita, Siegfried M. Rump,
* and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @param a Factors.
* @param b Factors.
* @return <code>&Sigma;<sub>i</sub> a<sub>i</sub> b<sub>i</sub></code>.
* @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
* <code>a<sub>i</sub> b<sub>i</sub></code> to high accuracy.
* It does so by using specific multiplication and addition algorithms to
* preserve accuracy and reduce cancellation effects.
* <br/>
* It is based on the 2005 paper
* <a href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita, Siegfried M. Rump,
* and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @param a Factors.
* @param b Factors.
* @return <code>&Sigma;<sub>i</sub> a<sub>i</sub> b<sub>i</sub></code>.
* @throws DimensionMismatchException if arrays dimensions don't match
*/
public T linearCombination(double[] a, T[] b)
throws DimensionMismatchException;
/**
* Compute a linear combination accurately.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* <p>
* 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.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* @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);
}

515
src/main/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructure.java
View File

@ -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.
* <p>This class is the workhorse of the differentiation package.</p>
@ -55,7 +58,7 @@ import org.apache.commons.math3.util.FastMath;
* @version $Id$
* @since 3.1
*/
public class DerivativeStructure implements FieldElement<DerivativeStructure>, Serializable {
public class DerivativeStructure implements ExtendedFieldElement<DerivativeStructure>, Serializable {
/** Serializable UID. */
private static final long serialVersionUID = 20120730L;
@ -223,6 +226,11 @@ public class DerivativeStructure implements FieldElement<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, S
return multiply((double) n);
}
/** '&times;' operator.
* @param a right hand side parameter of the operator
* @return this&times;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<DerivativeStructure>, S
return ds;
}
/** '&times;' operator.
* @param a right hand side parameter of the operator
* @return this&times;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<DerivativeStructure>, S
return result;
}
/** '&divides;' operator.
* @param a right hand side parameter of the operator
* @return this&divides;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<DerivativeStructure>, S
return ds;
}
/** '&divides;' operator.
* @param a right hand side parameter of the operator
* @return this&divides;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<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, S
return FastMath.getExponent(data[0]);
}
/**
* Multiply the instance by a power of 2.
* @param n power of 2
* @return this &times; 2<sup>n</sup>
*/
/** {@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<DerivativeStructure>, 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(<i>x</i><sup>2</sup>&nbsp;+<i>y</i><sup>2</sup>)<br/>
@ -501,46 +494,7 @@ public class DerivativeStructure implements FieldElement<DerivativeStructure>, 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<DerivativeStructure>, 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);
}
/** N<sup>th</sup> root.
* @param n order of the root
* @return n<sup>th</sup> 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<DerivativeStructure>, S
};
}
/** Power operation.
* @param p power to apply
* @return this<sup>p</sup>
*/
/** {@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 this<sup>n</sup>
*/
/** {@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 this<sup>e</sup>
* @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<DerivativeStructure>, 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<DerivativeStructure>, 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<DerivativeStructure>, 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.
* <p>
* Derivative structures are considered equal if they have the same number
* of free parameters, the same derivation order, and the same derivatives.
* </p>
* @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

354
src/main/java/org/apache/commons/math3/dfp/Dfp.java
View File

@ -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<Dfp> {
public class Dfp implements ExtendedFieldElement<Dfp> {
/** 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<Dfp> {
/** 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<Dfp> {
/* 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<Dfp> {
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));
}
}

4
src/main/java/org/apache/commons/math3/dfp/DfpDec.java
View File

@ -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)) {

667
src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java → src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java
View File

File diff suppressed because it is too large Load Diff

891
src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java Normal file
View File

@ -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}.
* <p>Instance of this class are guaranteed to be immutable.</p>
* @param <T> the type of the field elements
* @version $Id$
* @since 3.2
*/
public class FieldVector3D<T extends ExtendedFieldElement<T>> 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 (&alpha;) around Z
* (0 is +X, &pi;/2 is +Y, &pi; is -X and 3&pi;/2 is -Y)
* @param delta elevation (&delta;) above (XY) plane, from -&pi;/2 to +&pi;/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 FieldVector3D<T>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 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<T> 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<T> u1,
final T a2, final FieldVector3D<T> 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<T> u1,
final double a2, final FieldVector3D<T> 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<T> u1,
final T a2, final FieldVector3D<T> u2,
final T a3, final FieldVector3D<T> 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<T> u1,
final double a2, final FieldVector3D<T> u2,
final double a3, final FieldVector3D<T> 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<T> u1,
final T a2, final FieldVector3D<T> u2,
final T a3, final FieldVector3D<T> u3,
final T a4, final FieldVector3D<T> 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<T> u1,
final double a2, final FieldVector3D<T> u2,
final double a3, final FieldVector3D<T> u3,
final double a4, final FieldVector3D<T> 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 L<sub>1</sub> norm for the vector.
* @return L<sub>1</sub> norm for the vector
*/
public T getNorm1() {
return x.abs().add(y.abs()).add(z.abs());
}
/** Get the L<sub>2</sub> 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<sub>&infin;</sub> norm for the vector.
* @return L<sub>&infin;</sub> 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 (&alpha;) of the vector, between -&pi; and +&pi;
* @see #Vector3D(T, T)
*/
public T getAlpha() {
return y.atan2(x);
}
/** Get the elevation of the vector.
* @return elevation (&delta;) of the vector, between -&pi;/2 and +&pi;/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<T> add(final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> add(final Vector3D v) {
return new FieldVector3D<T>(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<T> add(final T factor, final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> add(final T factor, final Vector3D v) {
return new FieldVector3D<T>(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<T> add(final double factor, final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> add(final double factor, final Vector3D v) {
return new FieldVector3D<T>(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<T> subtract(final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> subtract(final Vector3D v) {
return new FieldVector3D<T>(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<T> subtract(final T factor, final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> subtract(final T factor, final Vector3D v) {
return new FieldVector3D<T>(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<T> subtract(final double factor, final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> subtract(final double factor, final Vector3D v) {
return new FieldVector3D<T>(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<T> 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.
* <p>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 :
* <pre><code>
* Vector3D k = u.normalize();
* Vector3D i = k.orthogonal();
* Vector3D j = Vector3D.crossProduct(k, i);
* </code></pre></p>
* @return a new normalized vector orthogonal to the instance
* @exception MathArithmeticException if the norm of the instance is null
*/
public FieldVector3D<T> 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<T>(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<T>(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<T>(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero());
}
}
/** Compute the angular separation between the instance and another vector.
* <p>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.</p>
* @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<T> 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<T> 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<T> negate() {
return new FieldVector3D<T>(x.negate(), y.negate(), z.negate());
}
/** Multiply the instance by a scalar.
* @param a scalar
* @return a new vector
*/
public FieldVector3D<T> scalarMultiply(final T a) {
return new FieldVector3D<T>(x.multiply(a), y.multiply(a), z.multiply(a));
}
/** Multiply the instance by a scalar.
* @param a scalar
* @return a new vector
*/
public FieldVector3D<T> scalarMultiply(final double a) {
return new FieldVector3D<T>(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.
* <p>
* If all coordinates of two 3D vectors are exactly the same, and none are
* <code>T.NaN</code>, the two 3D vectors are considered to be equal.
* </p>
* <p>
* <code>NaN</code> 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 <code>T.NaN</code>, the 3D vector is equal to
* {@link #NaN}.
* </p>
*
* @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<T> rhs = (FieldVector3D<T>) 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.
* <p>
* All NaN values have the same hash code.</p>
*
* @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.
* <p>
* 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.
* </p>
* @see MathArrays#linearCombination(double, double, double, double, double, double)
* @param v second vector
* @return the dot product this.v
*/
public T dotProduct(final FieldVector3D<T> v) {
return x.linearCombination(x, v.x, y, v.y, z, v.z);
}
/** Compute the dot-product of the instance and another vector.
* <p>
* 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.
* </p>
* @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<T> crossProduct(final FieldVector3D<T> v) {
return new FieldVector3D<T>(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<T> crossProduct(final Vector3D v) {
return new FieldVector3D<T>(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 L<sub>1</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNorm1()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>1</sub> norm
*/
public T distance1(final FieldVector3D<T> 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 L<sub>1</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNorm1()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>1</sub> 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 L<sub>2</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNorm()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>2</sub> norm
*/
public T distance(final FieldVector3D<T> 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 L<sub>2</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNorm()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>2</sub> 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<sub>&infin;</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNormInf()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>&infin;</sub> norm
*/
public T distanceInf(final FieldVector3D<T> 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<sub>&infin;</sub> norm.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNormInf()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the distance between the instance and p according to the L<sub>&infin;</sub> 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.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNormSq()</code> except that no intermediate
* vector is built</p>
* @param v second vector
* @return the square of the distance between the instance and p
*/
public T distanceSq(final FieldVector3D<T> 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.
* <p>Calling this method is equivalent to calling:
* <code>q.subtract(p).getNormSq()</code> except that no intermediate
* vector is built</p>
* @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());
}
}

1087
src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java
View File

File diff suppressed because it is too large Load Diff

18
src/main/java/org/apache/commons/math3/linear/SparseFieldVector.java
View File

@ -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<T extends FieldElement<T>> implements FieldVector
/** {@inheritDoc} */
public T[] toArray() {
T[] res = buildArray(virtualSize);
T[] res = MathArrays.buildArray(field, virtualSize);
OpenIntToFieldHashMap<T>.Iterator iter = entries.iterator();
while (iter.hasNext()) {
iter.advance();
@ -529,18 +529,6 @@ public class SparseFieldVector<T extends FieldElement<T>> 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() {

292
src/main/java/org/apache/commons/math3/util/Decimal64.java
View File

@ -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<Decimal64>,
Comparable<Decimal64> {
public class Decimal64 extends Number
implements ExtendedFieldElement<Decimal64>, Comparable<Decimal64> {
/** The constant value of {@code 0d} as a {@code Decimal64}. */
public static final Decimal64 ZERO;
@ -301,4 +302,287 @@ Comparable<Decimal64> {
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));
}
}

405
src/main/java/org/apache/commons/math3/util/MathArrays.java
View File

@ -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
* <code>a<sub>i</sub> b<sub>i</sub></code> to high accuracy.
* It does so by using specific multiplication and addition algorithms to
* preserve accuracy and reduce cancellation effects.
* <br/>
* It is based on the 2005 paper
* <a href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita, Siegfried M. Rump,
* and Shin'ichi Oishi published in SIAM J. Sci. Comput.
*
* @param a Factors.
* @param b Factors.
* @return <code>&Sigma;<sub>i</sub> a<sub>i</sub> b<sub>i</sub></code>.
* @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
* <code>a<sub>i</sub> b<sub>i</sub></code> to high accuracy.
* It does so by using specific multiplication and addition algorithms to
* preserve accuracy and reduce cancellation effects.
* <br/>
* It is based on the 2005 paper
* <a href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita, Siegfried M. Rump,
* and Shin'ichi Oishi published in SIAM J. Sci. Comput.
*
* @param a Factors.
* @param b Factors.
* @return <code>&Sigma;<sub>i</sub> a<sub>i</sub> b<sub>i</sub></code>.
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* @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.
* <p>
* This method computes a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* 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 <a
* href="http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.2.1547">
* Accurate Sum and Dot Product</a> by Takeshi Ogita,
* Siegfried M. Rump, and Shin'ichi Oishi published in SIAM J. Sci. Comput.
* </p>
* @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 a<sub>1</sub>&times;b<sub>1</sub> +
* a<sub>2</sub>&times;b<sub>2</sub> + a<sub>3</sub>&times;b<sub>3</sub> +
* a<sub>4</sub>&times;b<sub>4</sub>
* @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.
* <p>
* Arrays are filled with field.getZero()
* </p>
* @param <T> 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> T[] buildArray(final Field<T> 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.
* <p>
* Arrays are filled with field.getZero()
* </p>
* @param <T> 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 <code>new Field[rows][]</code> works)
* @return a new array
*/
@SuppressWarnings("unchecked")
public static <T> T[][] buildArray(final Field<T> 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;
}
}

142
src/test/java/org/apache/commons/math3/analysis/differentiation/DerivativeStructureTest.java
View File

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

32
src/test/java/org/apache/commons/math3/dfp/DfpTest.java
View File

@ -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

282
src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java → src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java
View File

@ -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<DerivativeStructure> 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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> reverted = r.revert();
FieldRotation<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> u = createVector(3, 2, 1);
FieldVector3D<DerivativeStructure> v = createVector(-4, 2, 2);
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure>(u, u.negate()).getAngle(), FastMath.PI);
try {
new RotationDS(u, createVector(0, 0, 0));
new FieldRotation<DerivativeStructure>(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<DerivativeStructure> u1 = createVector(3, 0, 0);
FieldVector3D<DerivativeStructure> u2 = createVector(0, 5, 0);
FieldVector3D<DerivativeStructure> v1 = createVector(0, 0, 2);
FieldVector3D<DerivativeStructure> v2 = createVector(-2, 0, 2);
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure>(u1, u2, u1.negate(), u2.negate());
FieldVector3D<DerivativeStructure> 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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(u1, u2, u1, u2), 1, 0, 0, 0);
try {
new RotationDS(u1, u2, createVector(0, 0, 0), v2);
new FieldRotation<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
double n = 23.5;
RotationDS r2 = new RotationDS(r1.getQ0().multiply(n), r1.getQ1().multiply(n),
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3));
FieldRotation<DerivativeStructure> r3 = r2.applyTo(r1);
FieldRotation<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3));
FieldRotation<DerivativeStructure> r3 = r2.applyInverseTo(r1);
FieldRotation<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> uds = createVector(x, y, z);
FieldVector3D<DerivativeStructure> ruds = r.applyTo(uds);
FieldVector3D<DerivativeStructure> rIuds = r.applyInverseTo(uds);
Vector3D u = new Vector3D(x, y, z);
Vector3DDS ru = r.applyTo(u);
Vector3DDS rIu = r.applyInverseTo(u);
FieldVector3D<DerivativeStructure> ru = r.applyTo(u);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure>(ruArray));
checkVector(rIuds, rIu);
checkVector(rIuds, new Vector3DDS(rIuArray));
checkVector(rIuds, new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> r1Prime = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2);
FieldRotation<DerivativeStructure> rB = r1Prime.applyTo(r2);
FieldRotation<DerivativeStructure> rC = FieldRotation.applyInverseTo(r1, r2);
FieldRotation<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> u = createVector(x, y, z);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> u1 = createVector(-1321008684645961.0 / 268435456.0,
-5774608829631843.0 / 268435456.0,
-3822921525525679.0 / 4294967296.0);
Vector3DDS u2 =createVector( -5712344449280879.0 / 2097152.0,
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> rot = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(0.9999999999999999, 0.0, 0.0);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> quat = new FieldRotation<DerivativeStructure>(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<DerivativeStructure> r, double q0, double q1, double q2, double q3) {
FieldRotation<DerivativeStructure> 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<DerivativeStructure> createRotation(double q0, double q1, double q2, double q3,
boolean needsNormalization) {
return new RotationDS(new DerivativeStructure(4, 1, 0, q0),
return new FieldRotation<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure>(mds, threshold);
}
private Vector3DDS createVector(double x, double y, double z) {
return new Vector3DDS(new DerivativeStructure(4, 1, x),
private FieldVector3D<DerivativeStructure> createVector(double x, double y, double z) {
return new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> createAxis(double x, double y, double z) {
return new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> u, FieldVector3D<DerivativeStructure> 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);
}
}

716
src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDfpTest.java Normal file
View File

@ -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<Dfp> 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<Dfp> r = new FieldRotation<Dfp>(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<Dfp>(createAxis(0, 0, 0), createAngle(2 * FastMath.PI / 3));
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new FieldRotation<Dfp>(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<Dfp>(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<Dfp> r = createRotation(a, b, c, d, true);
FieldRotation<Dfp> reverted = r.revert();
FieldRotation<Dfp> rrT = r.applyTo(reverted);
checkRotationDS(rrT, 1, 0, 0, 0);
FieldRotation<Dfp> 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<Dfp> u = createVector(3, 2, 1);
FieldVector3D<Dfp> v = createVector(-4, 2, 2);
FieldRotation<Dfp> r = new FieldRotation<Dfp>(u, v);
checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
checkAngle(new FieldRotation<Dfp>(u, u.negate()).getAngle(), FastMath.PI);
try {
new FieldRotation<Dfp>(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<Dfp> u1 = createVector(3, 0, 0);
FieldVector3D<Dfp> u2 = createVector(0, 5, 0);
FieldVector3D<Dfp> v1 = createVector(0, 0, 2);
FieldVector3D<Dfp> v2 = createVector(-2, 0, 2);
FieldRotation<Dfp> r = new FieldRotation<Dfp>(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<Dfp>(u1, u2, u1.negate(), u2.negate());
FieldVector3D<Dfp> 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<Dfp>(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<Dfp>(u1, u2, u1, u1.crossProduct(u2));
checkRotationDS(r, sqrt, -sqrt, 0, 0);
checkRotationDS(new FieldRotation<Dfp>(u1, u2, u1, u2), 1, 0, 0, 0);
try {
new FieldRotation<Dfp>(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<Dfp> 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<Dfp>(m3[0][0], m3[1][0], m3[2][0]));
checkVector(r.applyTo(createVector(0, 1, 0)),
new FieldVector3D<Dfp>(m3[0][1], m3[1][1], m3[2][1]));
checkVector(r.applyTo(createVector(0, 0, 1)),
new FieldVector3D<Dfp>(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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
double n = 23.5;
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(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<Dfp> 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<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2), createAngle(0.3));
FieldRotation<Dfp> r3 = r2.applyTo(r1);
FieldRotation<Dfp> 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<Dfp> 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<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2), createAngle(0.3));
FieldRotation<Dfp> r3 = r2.applyInverseTo(r1);
FieldRotation<Dfp> 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<Dfp> 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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> uds = createVector(x, y, z);
FieldVector3D<Dfp> ruds = r.applyTo(uds);
FieldVector3D<Dfp> rIuds = r.applyInverseTo(uds);
Vector3D u = new Vector3D(x, y, z);
FieldVector3D<Dfp> ru = r.applyTo(u);
FieldVector3D<Dfp> 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<Dfp>(ruArray));
checkVector(rIuds, rIu);
checkVector(rIuds, new FieldVector3D<Dfp>(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<Dfp> r1Prime = new FieldRotation<Dfp>(field.newDfp(r1.getQ0()),
field.newDfp(r1.getQ1()),
field.newDfp(r1.getQ2()),
field.newDfp(r1.getQ3()),
false);
double[] unit2 = g.nextVector();
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(unit2[0], unit2[1], unit2[2]),
createAngle(random.nextDouble()));
FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2);
FieldRotation<Dfp> rB = r1Prime.applyTo(r2);
FieldRotation<Dfp> rC = FieldRotation.applyInverseTo(r1, r2);
FieldRotation<Dfp> 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<Dfp> 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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> u = createVector(x, y, z);
FieldVector3D<Dfp> 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<Dfp> r = new FieldRotation<Dfp>(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<Dfp> 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<Dfp> 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<Dfp>(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<Dfp> 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<Dfp> u1 = createVector(-1321008684645961.0 / 268435456.0,
-5774608829631843.0 / 268435456.0,
-3822921525525679.0 / 4294967296.0);
FieldVector3D<Dfp> u2 =createVector( -5712344449280879.0 / 2097152.0,
-2275058564560979.0 / 1048576.0,
4423475992255071.0 / 65536.0);
FieldRotation<Dfp> rot = new FieldRotation<Dfp>(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<Dfp> u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0);
FieldVector3D<Dfp> u2 = createVector(0.0, 0.0, 1.0);
FieldVector3D<Dfp> v1 = createVector(0.9999999999999999, 0.0, 0.0);
FieldVector3D<Dfp> v2 = createVector(0.0, 0.0, -1.0);
FieldRotation<Dfp> quat = new FieldRotation<Dfp>(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<Dfp> r, double q0, double q1, double q2, double q3) {
FieldRotation<Dfp> rPrime = createRotation(q0, q1, q2, q3, false);
Assert.assertEquals(0, FieldRotation.distance(r, rPrime).getReal(), 1.0e-12);
}
private FieldRotation<Dfp> createRotation(double q0, double q1, double q2, double q3,
boolean needsNormalization) {
DfpField field = new DfpField(20);
return new FieldRotation<Dfp>(field.newDfp(q0),
field.newDfp(q1),
field.newDfp(q2),
field.newDfp(q3),
needsNormalization);
}
private FieldRotation<Dfp> 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<Dfp>(mds, threshold);
}
private FieldVector3D<Dfp> createVector(double x, double y, double z) {
DfpField field = new DfpField(20);
return new FieldVector3D<Dfp>(field.newDfp(x), field.newDfp(y), field.newDfp(z));
}
private FieldVector3D<Dfp> createAxis(double x, double y, double z) {
DfpField field = new DfpField(20);
return new FieldVector3D<Dfp>(field.newDfp(x), field.newDfp(y), field.newDfp(z));
}
private Dfp createAngle(double alpha) {
return new DfpField(20).newDfp(alpha);
}
private void checkVector(FieldVector3D<Dfp> u, FieldVector3D<Dfp> 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);
}
}

379
src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java → src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3DTest.java
View File

@ -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<DerivativeStructure> u = new FieldVector3D<DerivativeStructure>(2,
new FieldVector3D<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure> u1 = createVector(1, 2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure>(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<DerivativeStructure> u = createVector(1, 2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, -2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, -2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, -2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, -2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, 2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(1, 2, 3, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(2, 1, -4, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(9070467121.0, 4535233560.0, 1, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> big1 = new FieldVector3D<DerivativeStructure>(scale, v1);
FieldVector3D<DerivativeStructure> small2 = new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> k = v1.normalize();
FieldVector3D<DerivativeStructure> i = k.orthogonal();
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> v1 = createVector(0.1, 2.5, 1.3, 3);
Assert.assertEquals(0.0, v1.dotProduct(v1.orthogonal()).getReal(), 1.0e-12);
FieldVector3D<DerivativeStructure> v2 = createVector(2.3, -0.003, 7.6, 3);
Assert.assertEquals(0.0, v2.dotProduct(v2.orthogonal()).getReal(), 1.0e-12);
FieldVector3D<DerivativeStructure> v3 = createVector(-1.7, 1.4, 0.2, 3);
Assert.assertEquals(0.0, v3.dotProduct(v3.orthogonal()).getReal(), 1.0e-12);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> u1 = createVector(-1321008684645961.0 / 268435456.0,
-5774608829631843.0 / 268435456.0,
-7645843051051357.0 / 8589934592.0, 3);
Vector3DDS u2 = createVector(-5712344449280879.0 / 2097152.0,
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> uds = createVector(ux, uy, uz, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> cNaive = new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> uds = createVector(ux, uy, uz, 3);
FieldVector3D<DerivativeStructure> 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<DerivativeStructure> createVector(double x, double y, double z, int params) {
return new FieldVector3D<DerivativeStructure>(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<DerivativeStructure> 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<DerivativeStructure> 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<DerivativeStructure> 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);

153
src/test/java/org/apache/commons/math3/util/MathArraysTest.java
View File

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

4
src/test/java/org/apache/commons/math3/util/MathUtilsTest.java
View File

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