You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@commons.apache.org by lu...@apache.org on 2013/02/18 14:58:22 UTC
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/
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 (α) around Z
+ * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y)
+ * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/2
+ * @see #getAlpha()
+ * @see #getDelta()
+ */
+ public Vector3DDS(final DerivativeStructure alpha, final DerivativeStructure delta) {
+ DerivativeStructure cosDelta = delta.cos();
+ this.x = alpha.cos().multiply(cosDelta);
+ this.y = alpha.sin().multiply(cosDelta);
+ this.z = delta.sin();
+ }
+
+ /** Multiplicative constructor
+ * Build a vector from another one and a scale factor.
+ * The vector built will be a * u
+ * @param a scale factor
+ * @param u base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a, final Vector3DDS u) {
+ this.x = a.multiply(u.x);
+ this.y = a.multiply(u.y);
+ this.z = a.multiply(u.z);
+ }
+
+ /** Multiplicative constructor
+ * Build a vector from another one and a scale factor.
+ * The vector built will be a * u
+ * @param a scale factor
+ * @param u base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a, final Vector3D u) {
+ this.x = a.multiply(u.getX());
+ this.y = a.multiply(u.getY());
+ this.z = a.multiply(u.getZ());
+ }
+
+ /** Multiplicative constructor
+ * Build a vector from another one and a scale factor.
+ * The vector built will be a * u
+ * @param a scale factor
+ * @param u base (unscaled) vector
+ */
+ public Vector3DDS(final double a, final Vector3DDS u) {
+ this.x = u.x.multiply(a);
+ this.y = u.y.multiply(a);
+ this.z = u.z.multiply(a);
+ }
+
+ /** Linear constructor
+ * Build a vector from two other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+ final DerivativeStructure a2, final Vector3DDS u2) {
+ this.x = a1.multiply(u1.x).add(a2.multiply(u2.x));
+ this.y = a1.multiply(u1.y).add(a2.multiply(u2.y));
+ this.z = a1.multiply(u1.z).add(a2.multiply(u2.z));
+ }
+
+ /** Linear constructor
+ * Build a vector from two other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+ final DerivativeStructure a2, final Vector3D u2) {
+ this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX()));
+ this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY()));
+ this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ()));
+ }
+
+ /** Linear constructor
+ * Build a vector from two other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ */
+ public Vector3DDS(final double a1, final Vector3DDS u1,
+ final double a2, final Vector3DDS u2) {
+ this.x = u1.x.multiply(a1).add(u2.x.multiply(a2));
+ this.y = u1.y.multiply(a1).add(u2.y.multiply(a2));
+ this.z = u1.z.multiply(a1).add(u2.z.multiply(a2));
+ }
+
+ /** Linear constructor
+ * Build a vector from three other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+ final DerivativeStructure a2, final Vector3DDS u2,
+ final DerivativeStructure a3, final Vector3DDS u3) {
+ this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x));
+ this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y));
+ this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z));
+ }
+
+ /** Linear constructor
+ * Build a vector from three other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+ final DerivativeStructure a2, final Vector3D u2,
+ final DerivativeStructure a3, final Vector3D u3) {
+ this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX()));
+ this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY()));
+ this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ()));
+ }
+
+ /** Linear constructor
+ * Build a vector from three other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ */
+ public Vector3DDS(final double a1, final Vector3DDS u1,
+ final double a2, final Vector3DDS u2,
+ final double a3, final Vector3DDS u3) {
+ this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3));
+ this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3));
+ this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3));
+ }
+
+ /** Linear constructor
+ * Build a vector from four other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ * @param a4 fourth scale factor
+ * @param u4 fourth base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3DDS u1,
+ final DerivativeStructure a2, final Vector3DDS u2,
+ final DerivativeStructure a3, final Vector3DDS u3,
+ final DerivativeStructure a4, final Vector3DDS u4) {
+ this.x = a1.multiply(u1.x).add(a2.multiply(u2.x)).add(a3.multiply(u3.x)).add(a4.multiply(u4.x));
+ this.y = a1.multiply(u1.y).add(a2.multiply(u2.y)).add(a3.multiply(u3.y)).add(a4.multiply(u4.y));
+ this.z = a1.multiply(u1.z).add(a2.multiply(u2.z)).add(a3.multiply(u3.z)).add(a4.multiply(u4.z));
+ }
+
+ /** Linear constructor
+ * Build a vector from four other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ * @param a4 fourth scale factor
+ * @param u4 fourth base (unscaled) vector
+ */
+ public Vector3DDS(final DerivativeStructure a1, final Vector3D u1,
+ final DerivativeStructure a2, final Vector3D u2,
+ final DerivativeStructure a3, final Vector3D u3,
+ final DerivativeStructure a4, final Vector3D u4) {
+ this.x = a1.multiply(u1.getX()).add(a2.multiply(u2.getX())).add(a3.multiply(u3.getX())).add(a4.multiply(u4.getX()));
+ this.y = a1.multiply(u1.getY()).add(a2.multiply(u2.getY())).add(a3.multiply(u3.getY())).add(a4.multiply(u4.getY()));
+ this.z = a1.multiply(u1.getZ()).add(a2.multiply(u2.getZ())).add(a3.multiply(u3.getZ())).add(a4.multiply(u4.getZ()));
+ }
+
+ /** Linear constructor
+ * Build a vector from four other ones and corresponding scale factors.
+ * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4
+ * @param a1 first scale factor
+ * @param u1 first base (unscaled) vector
+ * @param a2 second scale factor
+ * @param u2 second base (unscaled) vector
+ * @param a3 third scale factor
+ * @param u3 third base (unscaled) vector
+ * @param a4 fourth scale factor
+ * @param u4 fourth base (unscaled) vector
+ */
+ public Vector3DDS(final double a1, final Vector3DDS u1,
+ final double a2, final Vector3DDS u2,
+ final double a3, final Vector3DDS u3,
+ final double a4, final Vector3DDS u4) {
+ this.x = u1.x.multiply(a1).add(u2.x.multiply(a2)).add(u3.x.multiply(a3)).add(u4.x.multiply(a4));
+ this.y = u1.y.multiply(a1).add(u2.y.multiply(a2)).add(u3.y.multiply(a3)).add(u4.y.multiply(a4));
+ this.z = u1.z.multiply(a1).add(u2.z.multiply(a2)).add(u3.z.multiply(a3)).add(u4.z.multiply(a4));
+ }
+
+ /** Get the abscissa of the vector.
+ * @return abscissa of the vector
+ * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+ */
+ public DerivativeStructure getX() {
+ return x;
+ }
+
+ /** Get the ordinate of the vector.
+ * @return ordinate of the vector
+ * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+ */
+ public DerivativeStructure getY() {
+ return y;
+ }
+
+ /** Get the height of the vector.
+ * @return height of the vector
+ * @see #Vector3D(DerivativeStructure, DerivativeStructure, DerivativeStructure)
+ */
+ public DerivativeStructure getZ() {
+ return z;
+ }
+
+ /** Get the vector coordinates as a dimension 3 array.
+ * @return vector coordinates
+ * @see #Vector3D(DerivativeStructure[])
+ */
+ public DerivativeStructure[] toArray() {
+ return new DerivativeStructure[] { x, y, z };
+ }
+
+ /** Convert to a constant vector without derivatives.
+ * @return a constant vector
+ */
+ public Vector3D toVector3D() {
+ return new Vector3D(x.getValue(), y.getValue(), z.getValue());
+ }
+
+ /** Get the 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>∞</sub> norm for the vector.
+ * @return L<sub>∞</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 (α) of the vector, between -π and +π
+ * @see #Vector3D(DerivativeStructure, DerivativeStructure)
+ */
+ public DerivativeStructure getAlpha() {
+ return DerivativeStructure.atan2(y, x);
+ }
+
+ /** Get the elevation of the vector.
+ * @return elevation (δ) of the vector, between -π/2 and +π/2
+ * @see #Vector3D(DerivativeStructure, DerivativeStructure)
+ */
+ public DerivativeStructure getDelta() {
+ return z.divide(getNorm()).asin();
+ }
+
+ /** Add a vector to the instance.
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final Vector3DDS v) {
+ return new Vector3DDS(x.add(v.x), y.add(v.y), z.add(v.z));
+ }
+
+ /** Add a vector to the instance.
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final Vector3D v) {
+ return new Vector3DDS(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ()));
+ }
+
+ /** Add a scaled vector to the instance.
+ * @param factor scale factor to apply to v before adding it
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final DerivativeStructure factor, final Vector3DDS v) {
+ return new Vector3DDS(x.add(factor.multiply(v.x)),
+ y.add(factor.multiply(v.y)),
+ z.add(factor.multiply(v.z)));
+ }
+
+ /** Add a scaled vector to the instance.
+ * @param factor scale factor to apply to v before adding it
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final DerivativeStructure factor, final Vector3D v) {
+ return new Vector3DDS(x.add(factor.multiply(v.getX())),
+ y.add(factor.multiply(v.getY())),
+ z.add(factor.multiply(v.getZ())));
+ }
+
+ /** Add a scaled vector to the instance.
+ * @param factor scale factor to apply to v before adding it
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final double factor, final Vector3DDS v) {
+ return new Vector3DDS(x.add(v.x.multiply(factor)),
+ y.add(v.y.multiply(factor)),
+ z.add(v.z.multiply(factor)));
+ }
+
+ /** Add a scaled vector to the instance.
+ * @param factor scale factor to apply to v before adding it
+ * @param v vector to add
+ * @return a new vector
+ */
+ public Vector3DDS add(final double factor, final Vector3D v) {
+ return new Vector3DDS(x.add(factor * v.getX()),
+ y.add(factor * v.getY()),
+ z.add(factor * v.getZ()));
+ }
+
+ /** Subtract a vector from the instance.
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final Vector3DDS v) {
+ return new Vector3DDS(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z));
+ }
+
+ /** Subtract a vector from the instance.
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final Vector3D v) {
+ return new Vector3DDS(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ()));
+ }
+
+ /** Subtract a scaled vector from the instance.
+ * @param factor scale factor to apply to v before subtracting it
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final DerivativeStructure factor, final Vector3DDS v) {
+ return new Vector3DDS(x.subtract(factor.multiply(v.x)),
+ y.subtract(factor.multiply(v.y)),
+ z.subtract(factor.multiply(v.z)));
+ }
+
+ /** Subtract a scaled vector from the instance.
+ * @param factor scale factor to apply to v before subtracting it
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final DerivativeStructure factor, final Vector3D v) {
+ return new Vector3DDS(x.subtract(factor.multiply(v.getX())),
+ y.subtract(factor.multiply(v.getY())),
+ z.subtract(factor.multiply(v.getZ())));
+ }
+
+ /** Subtract a scaled vector from the instance.
+ * @param factor scale factor to apply to v before subtracting it
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final double factor, final Vector3DDS v) {
+ return new Vector3DDS(x.subtract(v.x.multiply(factor)),
+ y.subtract(v.y.multiply(factor)),
+ z.subtract(v.z.multiply(factor)));
+ }
+
+ /** Subtract a scaled vector from the instance.
+ * @param factor scale factor to apply to v before subtracting it
+ * @param v vector to subtract
+ * @return a new vector
+ */
+ public Vector3DDS subtract(final double factor, final Vector3D v) {
+ return new Vector3DDS(x.subtract(factor * v.getX()),
+ y.subtract(factor * v.getY()),
+ z.subtract(factor * v.getZ()));
+ }
+
+ /** Get a normalized vector aligned with the instance.
+ * @return a new normalized vector
+ * @exception MathArithmeticException if the norm is zero
+ */
+ public Vector3DDS normalize() throws MathArithmeticException {
+ final DerivativeStructure s = getNorm();
+ if (s.getValue() == 0) {
+ throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR);
+ }
+ return scalarMultiply(s.reciprocal());
+ }
+
+ /** Get a vector orthogonal to the instance.
+ * <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>∞</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>∞</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>∞</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>∞</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>∞</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>∞</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>∞</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>∞</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>∞</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>∞</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"