commons-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From l..@apache.org
Subject svn commit: r1447263 [2/3] - in /commons/proper/math/trunk/src: changes/ main/java/org/apache/commons/math3/geometry/euclidean/threed/ test/java/org/apache/commons/math3/geometry/euclidean/threed/
Date Mon, 18 Feb 2013 13:58:22 GMT
Added: commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java?rev=1447263&view=auto
==============================================================================
--- commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java (added)
+++ commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java Mon Feb 18 13:58:21 2013
@@ -0,0 +1,1087 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.
+ * The ASF licenses this file to You under the Apache License, Version 2.0
+ * (the "License"); you may not use this file except in compliance with
+ * the License.  You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package org.apache.commons.math3.geometry.euclidean.threed;
+
+import java.io.Serializable;
+import java.text.NumberFormat;
+
+import org.apache.commons.math3.analysis.differentiation.DerivativeStructure;
+import org.apache.commons.math3.exception.DimensionMismatchException;
+import org.apache.commons.math3.exception.MathArithmeticException;
+import org.apache.commons.math3.exception.util.LocalizedFormats;
+import org.apache.commons.math3.util.FastMath;
+import org.apache.commons.math3.util.MathArrays;
+
+/**
+ * This class is a re-implementation of {@link Vector3D} using {@link DerivativeStructure}.
+ * <p>Instance of this class are guaranteed to be immutable.</p>
+ * @version $Id$
+ * @since 3.2
+ */
+public class Vector3DDS implements Serializable {
+
+    /** Serializable version identifier. */
+    private static final long serialVersionUID = 20130214L;
+
+    /** Abscissa. */
+    private final DerivativeStructure x;
+
+    /** Ordinate. */
+    private final DerivativeStructure y;
+
+    /** Height. */
+    private final DerivativeStructure z;
+
+    /** Simple constructor.
+     * Build a vector from its coordinates
+     * @param x abscissa
+     * @param y ordinate
+     * @param z height
+     * @see #getX()
+     * @see #getY()
+     * @see #getZ()
+     */
+    public Vector3DDS(final DerivativeStructure x,
+                      final DerivativeStructure y,
+                      final DerivativeStructure z) {
+        this.x = x;
+        this.y = y;
+        this.z = z;
+    }
+
+    /** Simple constructor.
+     * Build a vector from its coordinates
+     * @param v coordinates array
+     * @exception DimensionMismatchException if array does not have 3 elements
+     * @see #toArray()
+     */
+    public Vector3DDS(final DerivativeStructure[] v) throws DimensionMismatchException {
+        if (v.length != 3) {
+            throw new DimensionMismatchException(v.length, 3);
+        }
+        this.x = v[0];
+        this.y = v[1];
+        this.z = v[2];
+    }
+
+    /** Simple constructor.
+     * Build a vector from its azimuthal coordinates
+     * @param alpha azimuth (&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 Vector3DDS(final DerivativeStructure alpha, final DerivativeStructure delta) {
+        DerivativeStructure cosDelta = delta.cos();
+        this.x = alpha.cos().multiply(cosDelta);
+        this.y = alpha.sin().multiply(cosDelta);
+        this.z = delta.sin();
+    }
+
+    /** Multiplicative constructor
+     * Build a vector from another one and a scale factor.
+     * The vector built will be a * u
+     * @param a scale factor
+     * @param u base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a, final Vector3DDS u) {
+        this.x = a.multiply(u.x);
+        this.y = a.multiply(u.y);
+        this.z = a.multiply(u.z);
+    }
+
+    /** Multiplicative constructor
+     * Build a vector from another one and a scale factor.
+     * The vector built will be a * u
+     * @param a scale factor
+     * @param u base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a, final Vector3D u) {
+        this.x = a.multiply(u.getX());
+        this.y = a.multiply(u.getY());
+        this.z = a.multiply(u.getZ());
+    }
+
+    /** Multiplicative constructor
+     * Build a vector from another one and a scale factor.
+     * The vector built will be a * u
+     * @param a scale factor
+     * @param u base (unscaled) vector
+     */
+    public Vector3DDS(final double a, final Vector3DDS u) {
+        this.x = u.x.multiply(a);
+        this.y = u.y.multiply(a);
+        this.z = u.z.multiply(a);
+    }
+
+    /** Linear constructor
+     * Build a vector from two other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+                      final DerivativeStructure a2, final Vector3DDS u2) {
+        this.x = a1.multiply(u1.x).add(a2.multiply(u2.x));
+        this.y = a1.multiply(u1.y).add(a2.multiply(u2.y));
+        this.z = a1.multiply(u1.z).add(a2.multiply(u2.z));
+    }
+
+    /** Linear constructor
+     * Build a vector from two other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+                      final DerivativeStructure a2, final Vector3D u2) {
+        this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX()));
+        this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY()));
+        this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ()));
+    }
+
+    /** Linear constructor
+     * Build a vector from two other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     */
+    public Vector3DDS(final double a1, final Vector3DDS u1,
+                      final double a2, final Vector3DDS u2) {
+        this.x = u1.x.multiply(a1).add(u2.x.multiply(a2));
+        this.y = u1.y.multiply(a1).add(u2.y.multiply(a2));
+        this.z = u1.z.multiply(a1).add(u2.z.multiply(a2));
+    }
+
+    /** Linear constructor
+     * Build a vector from three other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+                      final DerivativeStructure a2, final Vector3DDS u2,
+                      final DerivativeStructure a3, final Vector3DDS u3) {
+        this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x));
+        this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y));
+        this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z));
+    }
+
+    /** Linear constructor
+     * Build a vector from three other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+                      final DerivativeStructure a2, final Vector3D u2,
+                      final DerivativeStructure a3, final Vector3D u3) {
+        this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX()));
+        this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY()));
+        this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ()));
+    }
+
+    /** Linear constructor
+     * Build a vector from three other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     */
+    public Vector3DDS(final double a1, final Vector3DDS u1,
+                      final double a2, final Vector3DDS u2,
+                      final double a3, final Vector3DDS u3) {
+        this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3));
+        this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3));
+        this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3));
+    }
+
+    /** Linear constructor
+     * Build a vector from four other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     * @param a4 fourth scale factor
+     * @param u4 fourth base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+                      final DerivativeStructure a2, final Vector3DDS u2,
+                      final DerivativeStructure a3, final Vector3DDS u3,
+                      final DerivativeStructure a4, final Vector3DDS u4) {
+        this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x)).add(a4.multiply(u4.x));
+        this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y)).add(a4.multiply(u4.y));
+        this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z)).add(a4.multiply(u4.z));
+    }
+
+    /** Linear constructor
+     * Build a vector from four other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     * @param a4 fourth scale factor
+     * @param u4 fourth base (unscaled) vector
+     */
+    public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+                      final DerivativeStructure a2, final Vector3D u2,
+                      final DerivativeStructure a3, final Vector3D u3,
+                      final DerivativeStructure a4, final Vector3D u4) {
+        this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX())).add(a4.multiply(u4.getX()));
+        this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY())).add(a4.multiply(u4.getY()));
+        this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ())).add(a4.multiply(u4.getZ()));
+    }
+
+    /** Linear constructor
+     * Build a vector from four other ones and corresponding scale factors.
+     * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+     * @param a1 first scale factor
+     * @param u1 first base (unscaled) vector
+     * @param a2 second scale factor
+     * @param u2 second base (unscaled) vector
+     * @param a3 third scale factor
+     * @param u3 third base (unscaled) vector
+     * @param a4 fourth scale factor
+     * @param u4 fourth base (unscaled) vector
+     */
+    public Vector3DDS(final double a1, final Vector3DDS u1,
+                      final double a2, final Vector3DDS u2,
+                      final double a3, final Vector3DDS u3,
+                      final double a4, final Vector3DDS u4) {
+        this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3)).add(u4.x.multiply(a4));
+        this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3)).add(u4.y.multiply(a4));
+        this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3)).add(u4.z.multiply(a4));
+    }
+
+    /** Get the abscissa of the vector.
+     * @return abscissa of the vector
+     * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+     */
+    public DerivativeStructure getX() {
+        return x;
+    }
+
+    /** Get the ordinate of the vector.
+     * @return ordinate of the vector
+     * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+     */
+    public DerivativeStructure getY() {
+        return y;
+    }
+
+    /** Get the height of the vector.
+     * @return height of the vector
+     * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+     */
+    public DerivativeStructure getZ() {
+        return z;
+    }
+
+    /** Get the vector coordinates as a dimension 3 array.
+     * @return vector coordinates
+     * @see #Vector3D(DerivativeStructure[])
+     */
+    public DerivativeStructure[] toArray() {
+        return new DerivativeStructure[] { x, y, z };
+    }
+
+    /** Convert to a constant vector without derivatives.
+     * @return a constant vector
+     */
+    public Vector3D toVector3D() {
+        return new Vector3D(x.getValue(), y.getValue(), z.getValue());
+    }
+
+    /** Get the L<sub>1</sub> norm for the vector.
+     * @return L<sub>1</sub> norm for the vector
+     */
+    public DerivativeStructure 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 DerivativeStructure getNorm() {
+        // there are no cancellation problems here, so we use the straightforward formula
+        return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)).sqrt();
+    }
+
+    /** Get the square of the norm for the vector.
+     * @return square of the Euclidean norm for the vector
+     */
+    public DerivativeStructure getNormSq() {
+        // there are no cancellation problems here, so we use the straightforward formula
+        return x.multiply(x).add(y.multiply(y)).add(z.multiply(z));
+    }
+
+    /** Get the L<sub>&infin;</sub> norm for the vector.
+     * @return L<sub>&infin;</sub> norm for the vector
+     */
+    public DerivativeStructure getNormInf() {
+        final DerivativeStructure xAbs = x.abs();
+        final DerivativeStructure yAbs = y.abs();
+        final DerivativeStructure zAbs = z.abs();
+        if (xAbs.getValue() <= yAbs.getValue()) {
+            if (yAbs.getValue() <= zAbs.getValue()) {
+                return zAbs;
+            } else {
+                return yAbs;
+            }
+        } else {
+            if (xAbs.getValue() <= zAbs.getValue()) {
+                return zAbs;
+            } else {
+                return xAbs;
+            }
+        }
+    }
+
+    /** Get the azimuth of the vector.
+     * @return azimuth (&alpha;) of the vector, between -&pi; and +&pi;
+     * @see #Vector3D(DerivativeStructure, DerivativeStructure)
+     */
+    public DerivativeStructure getAlpha() {
+        return DerivativeStructure.atan2(y, x);
+    }
+
+    /** Get the elevation of the vector.
+     * @return elevation (&delta;) of the vector, between -&pi;/2 and +&pi;/2
+     * @see #Vector3D(DerivativeStructure, DerivativeStructure)
+     */
+    public DerivativeStructure getDelta() {
+        return z.divide(getNorm()).asin();
+    }
+
+    /** Add a vector to the instance.
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final Vector3DDS v) {
+        return new Vector3DDS(x.add(v.x), y.add(v.y), z.add(v.z));
+    }
+
+    /** Add a vector to the instance.
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final Vector3D v) {
+        return new Vector3DDS(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ()));
+    }
+
+    /** Add a scaled vector to the instance.
+     * @param factor scale factor to apply to v before adding it
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final DerivativeStructure factor, final Vector3DDS v) {
+        return new Vector3DDS(x.add(factor.multiply(v.x)),
+                              y.add(factor.multiply(v.y)),
+                              z.add(factor.multiply(v.z)));
+    }
+
+    /** Add a scaled vector to the instance.
+     * @param factor scale factor to apply to v before adding it
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final DerivativeStructure factor, final Vector3D v) {
+        return new Vector3DDS(x.add(factor.multiply(v.getX())),
+                              y.add(factor.multiply(v.getY())),
+                              z.add(factor.multiply(v.getZ())));
+    }
+
+    /** Add a scaled vector to the instance.
+     * @param factor scale factor to apply to v before adding it
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final double factor, final Vector3DDS v) {
+        return new Vector3DDS(x.add(v.x.multiply(factor)),
+                              y.add(v.y.multiply(factor)),
+                              z.add(v.z.multiply(factor)));
+    }
+
+    /** Add a scaled vector to the instance.
+     * @param factor scale factor to apply to v before adding it
+     * @param v vector to add
+     * @return a new vector
+     */
+    public Vector3DDS add(final double factor, final Vector3D v) {
+        return new Vector3DDS(x.add(factor * v.getX()),
+                              y.add(factor * v.getY()),
+                              z.add(factor * v.getZ()));
+    }
+
+    /** Subtract a vector from the instance.
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final Vector3DDS v) {
+        return new Vector3DDS(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z));
+    }
+
+    /** Subtract a vector from the instance.
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final Vector3D v) {
+        return new Vector3DDS(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ()));
+    }
+
+    /** Subtract a scaled vector from the instance.
+     * @param factor scale factor to apply to v before subtracting it
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final DerivativeStructure factor, final Vector3DDS v) {
+        return new Vector3DDS(x.subtract(factor.multiply(v.x)),
+                              y.subtract(factor.multiply(v.y)),
+                              z.subtract(factor.multiply(v.z)));
+    }
+
+    /** Subtract a scaled vector from the instance.
+     * @param factor scale factor to apply to v before subtracting it
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final DerivativeStructure factor, final Vector3D v) {
+        return new Vector3DDS(x.subtract(factor.multiply(v.getX())),
+                              y.subtract(factor.multiply(v.getY())),
+                              z.subtract(factor.multiply(v.getZ())));
+    }
+
+    /** Subtract a scaled vector from the instance.
+     * @param factor scale factor to apply to v before subtracting it
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final double factor, final Vector3DDS v) {
+        return new Vector3DDS(x.subtract(v.x.multiply(factor)),
+                              y.subtract(v.y.multiply(factor)),
+                              z.subtract(v.z.multiply(factor)));
+    }
+
+    /** Subtract a scaled vector from the instance.
+     * @param factor scale factor to apply to v before subtracting it
+     * @param v vector to subtract
+     * @return a new vector
+     */
+    public Vector3DDS subtract(final double factor, final Vector3D v) {
+        return new Vector3DDS(x.subtract(factor * v.getX()),
+                              y.subtract(factor * v.getY()),
+                              z.subtract(factor * v.getZ()));
+    }
+
+    /** Get a normalized vector aligned with the instance.
+     * @return a new normalized vector
+     * @exception MathArithmeticException if the norm is zero
+     */
+    public Vector3DDS normalize() throws MathArithmeticException {
+        final DerivativeStructure s = getNorm();
+        if (s.getValue() == 0) {
+            throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR);
+        }
+        return scalarMultiply(s.reciprocal());
+    }
+
+    /** Get a vector orthogonal to the instance.
+     * <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 Vector3DDS orthogonal() throws MathArithmeticException {
+
+        final double threshold = 0.6 * getNorm().getValue();
+        if (threshold == 0) {
+            throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
+        }
+
+        if (FastMath.abs(x.getValue()) <= threshold) {
+            final DerivativeStructure inverse  = y.multiply(y).add(z.multiply(z)).sqrt().reciprocal();
+            return new Vector3DDS(inverse.getField().getZero(), inverse.multiply(z), inverse.multiply(y).negate());
+        } else if (FastMath.abs(y.getValue()) <= threshold) {
+            final DerivativeStructure inverse  = x.multiply(x).add(z.multiply(z)).sqrt().reciprocal();
+            return new Vector3DDS(inverse.multiply(z).negate(), inverse.getField().getZero(), inverse.multiply(x));
+        } else {
+            final DerivativeStructure inverse  = x.multiply(x).add(y.multiply(y)).sqrt().reciprocal();
+            return new Vector3DDS(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero());
+        }
+
+    }
+
+    /** Compute the angular separation between two vectors.
+     * <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 v1 first vector
+     * @param v2 second vector
+     * @return angular separation between v1 and v2
+     * @exception MathArithmeticException if either vector has a null norm
+     */
+    public static DerivativeStructure angle(Vector3DDS v1, Vector3DDS v2) throws MathArithmeticException {
+
+        final DerivativeStructure normProduct = v1.getNorm().multiply(v2.getNorm());
+        if (normProduct.getValue() == 0) {
+            throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
+        }
+
+        final DerivativeStructure dot = v1.dotProduct(v2);
+        final double threshold = normProduct.getValue() * 0.9999;
+        if ((dot.getValue() < -threshold) || (dot.getValue() > threshold)) {
+            // the vectors are almost aligned, compute using the sine
+            Vector3DDS v3 = crossProduct(v1, v2);
+            if (dot.getValue() >= 0) {
+                return v3.getNorm().divide(normProduct).asin();
+            }
+            return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate();
+        }
+
+        // the vectors are sufficiently separated to use the cosine
+        return dot.divide(normProduct).acos();
+
+    }
+
+    /** Get the opposite of the instance.
+     * @return a new vector which is opposite to the instance
+     */
+    public Vector3DDS negate() {
+        return new Vector3DDS(x.negate(), y.negate(), z.negate());
+    }
+
+    /** Multiply the instance by a scalar.
+     * @param a scalar
+     * @return a new vector
+     */
+    public Vector3DDS scalarMultiply(final DerivativeStructure a) {
+        return new Vector3DDS(x.multiply(a), y.multiply(a), z.multiply(a));
+    }
+
+    /** Multiply the instance by a scalar.
+     * @param a scalar
+     * @return a new vector
+     */
+    public Vector3DDS scalarMultiply(final double a) {
+        return new Vector3DDS(x.multiply(a), y.multiply(a), z.multiply(a));
+    }
+
+    /**
+     * Returns true if any coordinate of this vector is NaN; false otherwise
+     * @return  true if any coordinate of this vector is NaN; false otherwise
+     */
+    public boolean isNaN() {
+        return Double.isNaN(x.getValue()) || Double.isNaN(y.getValue()) || Double.isNaN(z.getValue());
+    }
+
+    /**
+     * Returns true if any coordinate of this vector is infinite and none are NaN;
+     * false otherwise
+     * @return  true if any coordinate of this vector is infinite and none are NaN;
+     * false otherwise
+     */
+    public boolean isInfinite() {
+        return !isNaN() && (Double.isInfinite(x.getValue()) || Double.isInfinite(y.getValue()) || Double.isInfinite(z.getValue()));
+    }
+
+    /**
+     * Test for the equality of two 3D vectors.
+     * <p>
+     * If all coordinates of two 3D vectors are exactly the same, and none are
+     * <code>DerivativeStructure.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>DerivativeStructure.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 Vector3DDS) {
+            final Vector3DDS rhs = (Vector3DDS)other;
+            if (rhs.isNaN()) {
+                return this.isNaN();
+            }
+
+            return MathArrays.equals(x.getAllDerivatives(), rhs.x.getAllDerivatives()) &&
+                   MathArrays.equals(y.getAllDerivatives(), rhs.y.getAllDerivatives()) &&
+                   MathArrays.equals(z.getAllDerivatives(), rhs.z.getAllDerivatives());
+
+        }
+        return false;
+    }
+
+    /**
+     * Get a hashCode for the 3D vector.
+     * <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 DerivativeStructure dotProduct(final Vector3DDS v) {
+        return MathArrays.linearCombination(x, v.x, y, v.y, z, v.z);
+    }
+
+    /** Compute the dot-product of the instance and another vector.
+     * <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 DerivativeStructure dotProduct(final Vector3D v) {
+        return MathArrays.linearCombination(v.getX(), x, v.getY(), y, v.getZ(), z);
+    }
+
+    /** Compute the cross-product of the instance with another vector.
+     * @param v other vector
+     * @return the cross product this ^ v as a new Vector3D
+     */
+    public Vector3DDS crossProduct(final Vector3DDS v) {
+        return new Vector3DDS(MathArrays.linearCombination(y, v.z, z.negate(), v.y),
+                              MathArrays.linearCombination(z, v.x, x.negate(), v.z),
+                              MathArrays.linearCombination(x, v.y, y.negate(), v.x));
+    }
+
+    /** Compute the cross-product of the instance with another vector.
+     * @param v other vector
+     * @return the cross product this ^ v as a new Vector3D
+     */
+    public Vector3DDS crossProduct(final Vector3D v) {
+        return new Vector3DDS(MathArrays.linearCombination(v.getZ(), y, v.getY(), z.negate()),
+                              MathArrays.linearCombination(v.getX(), z, v.getZ(), x.negate()),
+                              MathArrays.linearCombination(v.getY(), x, v.getX(), y.negate()));
+    }
+
+    /** Compute the distance between the instance and another vector according to the 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 DerivativeStructure distance1(final Vector3DDS v) {
+        final DerivativeStructure dx = v.x.subtract(x).abs();
+        final DerivativeStructure dy = v.y.subtract(y).abs();
+        final DerivativeStructure dz = v.z.subtract(z).abs();
+        return dx.add(dy).add(dz);
+    }
+
+    /** Compute the distance between the instance and another vector according to the 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 DerivativeStructure distance1(final Vector3D v) {
+        final DerivativeStructure dx = x.subtract(v.getX()).abs();
+        final DerivativeStructure dy = y.subtract(v.getY()).abs();
+        final DerivativeStructure dz = z.subtract(v.getZ()).abs();
+        return dx.add(dy).add(dz);
+    }
+
+    /** Compute the distance between the instance and another vector according to the 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 DerivativeStructure distance(final Vector3DDS v) {
+        final DerivativeStructure dx = v.x.subtract(x);
+        final DerivativeStructure dy = v.y.subtract(y);
+        final DerivativeStructure dz = v.z.subtract(z);
+        return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt();
+    }
+
+    /** Compute the distance between the instance and another vector according to the 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 DerivativeStructure distance(final Vector3D v) {
+        final DerivativeStructure dx = x.subtract(v.getX());
+        final DerivativeStructure dy = y.subtract(v.getY());
+        final DerivativeStructure dz = z.subtract(v.getZ());
+        return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt();
+    }
+
+    /** Compute the distance between the instance and another vector according to the L<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 DerivativeStructure distanceInf(final Vector3DDS v) {
+        final DerivativeStructure dx = v.x.subtract(x).abs();
+        final DerivativeStructure dy = v.y.subtract(y).abs();
+        final DerivativeStructure dz = v.z.subtract(z).abs();
+        if (dx.getValue() <= dy.getValue()) {
+            if (dy.getValue() <= dz.getValue()) {
+                return dz;
+            } else {
+                return dy;
+            }
+        } else {
+            if (dx.getValue() <= dz.getValue()) {
+                return dz;
+            } else {
+                return dx;
+            }
+        }
+    }
+
+    /** Compute the distance between the instance and another vector according to the L<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 DerivativeStructure distanceInf(final Vector3D v) {
+        final DerivativeStructure dx = x.subtract(v.getX()).abs();
+        final DerivativeStructure dy = y.subtract(v.getY()).abs();
+        final DerivativeStructure dz = z.subtract(v.getZ()).abs();
+        if (dx.getValue() <= dy.getValue()) {
+            if (dy.getValue() <= dz.getValue()) {
+                return dz;
+            } else {
+                return dy;
+            }
+        } else {
+            if (dx.getValue() <= dz.getValue()) {
+                return dz;
+            } else {
+                return dx;
+            }
+        }
+    }
+
+    /** Compute the square of the distance between the instance and another vector.
+     * <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 DerivativeStructure distanceSq(final Vector3DDS v) {
+        final DerivativeStructure dx = v.x.subtract(x);
+        final DerivativeStructure dy = v.y.subtract(y);
+        final DerivativeStructure dz = v.z.subtract(z);
+        return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz));
+    }
+
+    /** Compute the square of the distance between the instance and another vector.
+     * <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 DerivativeStructure distanceSq(final Vector3D v) {
+        final DerivativeStructure dx = x.subtract(v.getX());
+        final DerivativeStructure dy = y.subtract(v.getY());
+        final DerivativeStructure dz = z.subtract(v.getZ());
+        return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz));
+    }
+
+    /** Compute the dot-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the dot product v1.v2
+     */
+    public static DerivativeStructure dotProduct(Vector3DDS v1, Vector3DDS v2) {
+        return v1.dotProduct(v2);
+    }
+
+    /** Compute the dot-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the dot product v1.v2
+     */
+    public static DerivativeStructure dotProduct(Vector3DDS v1, Vector3D v2) {
+        return v1.dotProduct(v2);
+    }
+
+    /** Compute the dot-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the dot product v1.v2
+     */
+    public static DerivativeStructure dotProduct(Vector3D v1, Vector3DDS v2) {
+        return v2.dotProduct(v1);
+    }
+
+    /** Compute the cross-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the cross product v1 ^ v2 as a new Vector
+     */
+    public static Vector3DDS crossProduct(final Vector3DDS v1, final Vector3DDS v2) {
+        return v1.crossProduct(v2);
+    }
+
+    /** Compute the cross-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the cross product v1 ^ v2 as a new Vector
+     */
+    public static Vector3DDS crossProduct(final Vector3DDS v1, final Vector3D v2) {
+        return v1.crossProduct(v2);
+    }
+
+    /** Compute the cross-product of two vectors.
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the cross product v1 ^ v2 as a new Vector
+     */
+    public static Vector3DDS crossProduct(final Vector3D v1, final Vector3DDS v2) {
+        return v2.crossProduct(v1).negate();
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
+     */
+    public static DerivativeStructure distance1(Vector3DDS v1, Vector3DDS v2) {
+        return v1.distance1(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
+     */
+    public static DerivativeStructure distance1(Vector3DDS v1, Vector3D v2) {
+        return v1.distance1(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>1</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>1</sub> norm
+     */
+    public static DerivativeStructure distance1(Vector3D v1, Vector3DDS v2) {
+        return v2.distance1(v1);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
+     */
+    public static DerivativeStructure distance(Vector3DDS v1, Vector3DDS v2) {
+        return v1.distance(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
+     */
+    public static DerivativeStructure distance(Vector3DDS v1, Vector3D v2) {
+        return v1.distance(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>2</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNorm()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>2</sub> norm
+     */
+    public static DerivativeStructure distance(Vector3D v1, Vector3DDS v2) {
+        return v2.distance(v1);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
+     */
+    public static DerivativeStructure distanceInf(Vector3DDS v1, Vector3DDS v2) {
+        return v1.distanceInf(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
+     */
+    public static DerivativeStructure distanceInf(Vector3DDS v1, Vector3D v2) {
+        return v1.distanceInf(v2);
+    }
+
+    /** Compute the distance between two vectors according to the L<sub>&infin;</sub> norm.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the distance between v1 and v2 according to the L<sub>&infin;</sub> norm
+     */
+    public static DerivativeStructure distanceInf(Vector3D v1, Vector3DDS v2) {
+        return v2.distanceInf(v1);
+    }
+
+    /** Compute the square of the distance between two vectors.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the square of the distance between v1 and v2
+     */
+    public static DerivativeStructure distanceSq(Vector3DDS v1, Vector3DDS v2) {
+        return v1.distanceSq(v2);
+    }
+
+    /** Compute the square of the distance between two vectors.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the square of the distance between v1 and v2
+     */
+    public static DerivativeStructure distanceSq(Vector3DDS v1, Vector3D v2) {
+        return v1.distanceSq(v2);
+    }
+
+    /** Compute the square of the distance between two vectors.
+     * <p>Calling this method is equivalent to calling:
+     * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate
+     * vector is built</p>
+     * @param v1 first vector
+     * @param v2 second vector
+     * @return the square of the distance between v1 and v2
+     */
+    public static DerivativeStructure distanceSq(Vector3D v1, Vector3DDS v2) {
+        return v2.distanceSq(v1);
+    }
+
+    /** Get a string representation of this vector.
+     * @return a string representation of this vector
+     */
+    @Override
+    public String toString() {
+        return Vector3DFormat.getInstance().format(toVector3D());
+    }
+
+    /** {@inheritDoc} */
+    public String toString(final NumberFormat format) {
+        return new Vector3DFormat(format).format(toVector3D());
+    }
+
+}

Propchange: commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java
------------------------------------------------------------------------------
    svn:eol-style = native

Propchange: commons/proper/math/trunk/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java
------------------------------------------------------------------------------
    svn:keywords = "Author Date Id Revision"

Added: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java
URL: http://svn.apache.org/viewvc/commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java?rev=1447263&view=auto
==============================================================================
--- commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java (added)
+++ commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java Mon Feb 18 13:58:21 2013
@@ -0,0 +1,841 @@
+/*
+ * 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.analysis.differentiation.DerivativeStructure;
+import org.apache.commons.math3.exception.MathArithmeticException;
+import org.apache.commons.math3.exception.MathIllegalArgumentException;
+import org.apache.commons.math3.linear.MatrixUtils;
+import org.apache.commons.math3.linear.RealMatrix;
+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 RotationDSTest {
+
+    @Test
+    public void testIdentity() {
+
+        RotationDS 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 {
+
+        RotationDS r = new RotationDS(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 RotationDS(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));
+        checkVector(r.getAxis(), createVector(0, 0, -1));
+        checkAngle(r.getAngle(), 0.5 * FastMath.PI);
+
+        r = new RotationDS(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;
+        RotationDS r = createRotation(a, b, c, d, true);
+        double a2 = a * a;
+        double b2 = b * b;
+        double c2 = c * c;
+        double d2 = d * d;
+        double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
+        Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        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);
+        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);
+        Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        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);
+        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);
+        Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+        Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+        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);
+    }
+
+    @Test
+    public void testVectorOnePair() throws MathArithmeticException {
+
+        Vector3DDS u = createVector(3, 2, 1);
+        Vector3DDS v = createVector(-4, 2, 2);
+        RotationDS r = new RotationDS(u, v);
+        checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
+
+        checkAngle(new RotationDS(u, u.negate()).getAngle(), FastMath.PI);
+
+        try {
+            new RotationDS(u, createVector(0, 0, 0));
+            Assert.fail("an exception should have been thrown");
+        } catch (MathArithmeticException e) {
+            // expected behavior
+        }
+
+    }
+
+    @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);
+        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) {
+            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 RotationDS(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));
+        checkRotationDS(r, sqrt, -sqrt, 0, 0);
+
+        checkRotationDS(new RotationDS(u1, u2, u1, u2), 1, 0, 0, 0);
+
+        try {
+            new RotationDS(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 } };
+        RotationDS 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);
+
+        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();
+
+        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].getValue() * m3[j][0].getValue() +
+                               m3[i][1].getValue() * m3[j][1].getValue() +
+                               m3[i][2].getValue() * m3[j][2].getValue();
+                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 Vector3DDS(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]));
+        checkVector(r.applyTo(createVector(0, 0, 1)),
+                    new Vector3DDS(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 {
+
+        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 += 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],
+                                                      new DerivativeStructure(3, 1, 0, alpha1),
+                                                      new DerivativeStructure(3, 1, 1, alpha2),
+                                                      new DerivativeStructure(3, 1, 2, alpha3));
+                        DerivativeStructure[] 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 += 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],
+                                                      new DerivativeStructure(3, 1, 0, alpha1),
+                                                      new DerivativeStructure(3, 1, 1, alpha2),
+                                                      new DerivativeStructure(3, 1, 2, alpha3));
+                        DerivativeStructure[] angles = r.getAngles(EulerOrders[i]);
+                        checkAngle(angles[0], alpha1);
+                        checkAngle(angles[1], alpha2);
+                        checkAngle(angles[2], alpha3);
+                    }
+                }
+            }
+        }
+
+    }
+
+    @Test
+    public void testSingularities() {
+
+        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) {
+                RotationDS r = new RotationDS(CardanOrders[i],
+                                              new DerivativeStructure(3, 1, 0, 0.1),
+                                              new DerivativeStructure(3, 1, 1, singularCardanAngle[j]),
+                                              new DerivativeStructure(3, 1, 2, 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) {
+                RotationDS r = new RotationDS(EulerOrders[i],
+                                              new DerivativeStructure(3, 1, 0, 0.1),
+                                              new DerivativeStructure(3, 1, 1, singularEulerAngle[j]),
+                                              new DerivativeStructure(3, 1, 2, 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 {
+
+        RotationDS r1 = new RotationDS(createVector(2, -3, 5), createAngle(1.7));
+        double n = 23.5;
+        RotationDS r2 = new RotationDS(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);
+                    checkVector(r2.applyTo(u), r1.applyTo(u));
+                }
+            }
+        }
+
+        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());
+
+    }
+
+    @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(new Rotation(r1.getQ0().getValue(),
+                                                      r1.getQ1().getValue(),
+                                                      r1.getQ2().getValue(),
+                                                      r1.getQ3().getValue(),
+                                                      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);
+                    checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
+                    checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
+                }
+            }
+        }
+
+    }
+
+    @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(new Rotation(r1.getQ0().getValue(),
+                                                             r1.getQ1().getValue(),
+                                                             r1.getQ2().getValue(),
+                                                             r1.getQ3().getValue(),
+                                                             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);
+                    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();
+            RotationDS r = new RotationDS(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);
+                        Vector3D   u     = new Vector3D(x, y, z);
+                        Vector3DDS ru    = r.applyTo(u);
+                        Vector3DDS 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(rIuds, rIu);
+                        checkVector(rIuds, new Vector3DDS(rIuArray));
+                    }
+                }
+            }
+        }
+
+    }
+
+    @Test
+    public void testDoubleRotations() throws MathIllegalArgumentException {
+
+        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());
+            RotationDS r1Prime = new RotationDS(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]),
+                                           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);
+
+            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));
+                        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 testDerivatives() {
+
+        double eps      = 5.0e-16;
+        double kx       = 2;
+        double ky       = -3;
+        double kz       = 5;
+        double n2       = kx * kx + ky * ky + kz * kz;
+        double n        = FastMath.sqrt(n2);
+        double theta    = 1.7;
+        double cosTheta = FastMath.cos(theta);
+        double sinTheta = FastMath.sin(theta);
+        RotationDS r    = new RotationDS(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
+        RealMatrix dadk = MatrixUtils.createRealMatrix(new double[][] {
+            { (ky * ky + kz * kz) / ( n * n2),            -kx * ky / ( n * n2),            -kx * kz / ( n * n2) },
+            {            -kx * ky / ( n * n2), (kx * kx + kz * kz) / ( n * n2),            -ky * kz / ( n * n2) },
+            {            -kx * kz / ( n * n2),            -ky * kz / ( n * n2), (kx * kx + ky * ky) / ( n * n2) }
+        });
+
+        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) {
+                    Vector3D   u = new Vector3D(x, y, z);
+                    Vector3DDS 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);
+                    Vector3D cross = Vector3D.crossProduct(a, u);
+                    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);
+
+                    // Jacobian of the image v = r(u) with respect to rotation axis a
+                    // (analytical differentiation of the explicit formula)
+                    RealMatrix dvda = MatrixUtils.createRealMatrix(new double[][] {
+                        { c1 * x * a.getX() + c2,           c1 * y * a.getX() + sinTheta * z, c1 * z * a.getX() - sinTheta * y },
+                        { c1 * x * a.getY() - sinTheta * z, c1 * y * a.getY() + c2,           c1 * z * a.getY() + sinTheta * x },
+                        { c1 * x * a.getZ() + sinTheta * y, c1 * y * a.getZ() - sinTheta * x, c1 * z * a.getZ() + c2           }
+                    });
+
+                    // compose Jacobians
+                    RealMatrix dvdk = dvda.multiply(dadk);
+
+                    // derivatives with respect to un-normalized axis
+                    Assert.assertEquals(dvdk.getEntry(0, 0), v.getX().getPartialDerivative(1, 0, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(0, 1), v.getX().getPartialDerivative(0, 1, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(0, 2), v.getX().getPartialDerivative(0, 0, 1, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(1, 0), v.getY().getPartialDerivative(1, 0, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(1, 1), v.getY().getPartialDerivative(0, 1, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(1, 2), v.getY().getPartialDerivative(0, 0, 1, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(2, 0), v.getZ().getPartialDerivative(1, 0, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(2, 1), v.getZ().getPartialDerivative(0, 1, 0, 0), eps);
+                    Assert.assertEquals(dvdk.getEntry(2, 2), v.getZ().getPartialDerivative(0, 0, 1, 0), eps);
+
+                    // derivative with respect to rotation angle
+                    // (analytical differentiation of the explicit formula)
+                    Vector3D dvdTheta =
+                            new Vector3D(-sinTheta, u, sinTheta * dot, a, cosTheta, cross);
+                    Assert.assertEquals(dvdTheta.getX(), v.getX().getPartialDerivative(0, 0, 0, 1), eps);
+                    Assert.assertEquals(dvdTheta.getY(), v.getY().getPartialDerivative(0, 0, 0, 1), eps);
+                    Assert.assertEquals(dvdTheta.getZ(), v.getZ().getPartialDerivative(0, 0, 0, 1), eps);
+
+                }
+            }
+        }
+     }
+
+    @Test
+    public void testArray() throws MathIllegalArgumentException {
+
+        RotationDS r = new RotationDS(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);
+                    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);
+                    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);
+                }
+            }
+        }
+
+    }
+
+    @Test
+    public void testApplyInverseTo() throws MathIllegalArgumentException {
+
+        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));
+        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),
+                                          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].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);
+            }
+        }
+
+        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),
+                                          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 RotationDS(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),
+                                          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{
+        Vector3DDS u1 = createVector(-1321008684645961.0 /  268435456.0,
+                                   -5774608829631843.0 /  268435456.0,
+                                   -3822921525525679.0 / 4294967296.0);
+        Vector3DDS 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);
+    }
+
+    @Test
+    public void testIssue801() throws MathArithmeticException {
+        Vector3DDS u1 = createVector(0.9999988431610581, -0.0015210774290851095, 0.0);
+        Vector3DDS 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);
+
+        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();
+        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);
+
+    }
+
+    private void checkAngle(DerivativeStructure a1, double a2) {
+        Assert.assertEquals(a1.getValue(), MathUtils.normalizeAngle(a2, a1.getValue()), 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 RotationDS createRotation(double q0, double q1, double q2, double q3,
+                                      boolean needsNormalization) {
+        return new RotationDS(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) {
+        DerivativeStructure[][] mds = new DerivativeStructure[m.length][m[0].length];
+        int index = 0;
+        for (int i = 0; i < m.length; ++i) {
+            for (int j = 0; j < m[i].length; ++j) {
+                mds[i][j] = new DerivativeStructure(4, 1, index, m[i][j]);
+                index = (index + 1) % 4;
+            }
+        }
+        return new RotationDS(mds, threshold);
+    }
+
+    private Vector3DDS createVector(double x, double y, double z) {
+        return new Vector3DDS(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),
+                              new DerivativeStructure(4, 1, 1, y),
+                              new DerivativeStructure(4, 1, 2, z));
+    }
+
+    private DerivativeStructure createAngle(double alpha) {
+        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);
+    }
+
+}

Propchange: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java
------------------------------------------------------------------------------
    svn:eol-style = native

Propchange: commons/proper/math/trunk/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java
------------------------------------------------------------------------------
    svn:keywords = "Author Date Id Revision"



Mime
View raw message