From b1a4e8f2f962a51d075e341b3305553b1a2cf6bc Mon Sep 17 00:00:00 2001 From: Luc Maisonobe Date: Mon, 18 Feb 2013 13:58:21 +0000 Subject: [PATCH] Added partial derivatives computation for 3D vectors and rotations. git-svn-id: https://svn.apache.org/repos/asf/commons/proper/math/trunk@1447263 13f79535-47bb-0310-9956-ffa450edef68 --- src/changes/changes.xml | 5 +- .../geometry/euclidean/threed/RotationDS.java | 1211 +++++++++++++++++ .../geometry/euclidean/threed/Vector3DDS.java | 1087 +++++++++++++++ .../euclidean/threed/RotationDSTest.java | 841 ++++++++++++ .../euclidean/threed/Vector3DDSTest.java | 733 ++++++++++ 5 files changed, 3876 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java create mode 100644 src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java create mode 100644 src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java create mode 100644 src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 3820d35b7..06e9e1736 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -55,7 +55,10 @@ This is a minor release: It combines bug fixes and new features. Changes to existing features were made in a backwards-compatible way such as to allow drop-in replacement of the v3.1[.1] JAR file. "> - + + Added partial derivatives computation for 3D vectors and rotations. + + Fixed DerivativeStructure.atan2 for special cases when both arguments are +/-0. diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java new file mode 100644 index 000000000..485dca054 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDS.java @@ -0,0 +1,1211 @@ +/* + * 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 org.apache.commons.math3.Field; +import org.apache.commons.math3.analysis.differentiation.DerivativeStructure; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; + +/** + * This class is a re-implementation of {@link Rotation} using {@link DerivativeStructure}. + *

Instance of this class are guaranteed to be immutable.

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

A rotation can be built from a normalized quaternion, + * i.e. a quaternion for which q02 + + * q12 + q22 + + * q32 = 1. If the quaternion is not normalized, + * the constructor can normalize it in a preprocessing step.

+ *

Note that some conventions put the scalar part of the quaternion + * as the 4th component and the vector part as the first three + * components. This is not our convention. We put the scalar part + * as the first component.

+ * @param q0 scalar part of the quaternion + * @param q1 first coordinate of the vectorial part of the quaternion + * @param q2 second coordinate of the vectorial part of the quaternion + * @param q3 third coordinate of the vectorial part of the quaternion + * @param needsNormalization if true, the coordinates are considered + * not to be normalized, a normalization preprocessing step is performed + * before using them + */ + public RotationDS(final DerivativeStructure q0, final DerivativeStructure q1, + final DerivativeStructure q2, final DerivativeStructure q3, + final boolean needsNormalization) { + + if (needsNormalization) { + // normalization preprocessing + final DerivativeStructure inv = + q0.multiply(q0).add(q1.multiply(q1)).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().reciprocal(); + this.q0 = inv.multiply(q0); + this.q1 = inv.multiply(q1); + this.q2 = inv.multiply(q2); + this.q3 = inv.multiply(q3); + } else { + this.q0 = q0; + this.q1 = q1; + this.q2 = q2; + this.q3 = q3; + } + + } + + /** Build a rotation from an axis and an angle. + *

We use the convention that angles are oriented according to + * the effect of the rotation on vectors around the axis. That means + * that if (i, j, k) is a direct frame and if we first provide +k as + * the axis and π/2 as the angle to this constructor, and then + * {@link #applyTo(Vector3DDS) apply} the instance to +i, we will get + * +j.

+ *

Another way to represent our convention is to say that a rotation + * of angle θ about the unit vector (x, y, z) is the same as the + * rotation build from quaternion components { cos(-θ/2), + * x * sin(-θ/2), y * sin(-θ/2), z * sin(-θ/2) }. + * Note the minus sign on the angle!

+ *

On the one hand this convention is consistent with a vectorial + * perspective (moving vectors in fixed frames), on the other hand it + * is different from conventions with a frame perspective (fixed vectors + * viewed from different frames) like the ones used for example in spacecraft + * attitude community or in the graphics community.

+ * @param axis axis around which to rotate + * @param angle rotation angle. + * @exception MathIllegalArgumentException if the axis norm is zero + */ + public RotationDS(final Vector3DDS axis, final DerivativeStructure angle) + throws MathIllegalArgumentException { + + final DerivativeStructure norm = axis.getNorm(); + if (norm.getValue() == 0) { + throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); + } + + final DerivativeStructure halfAngle = angle.multiply(-0.5); + final DerivativeStructure coeff = halfAngle.sin().divide(norm); + + q0 = halfAngle.cos(); + q1 = coeff.multiply(axis.getX()); + q2 = coeff.multiply(axis.getY()); + q3 = coeff.multiply(axis.getZ()); + + } + + /** Build a rotation from a 3X3 matrix. + + *

Rotation matrices are orthogonal matrices, i.e. unit matrices + * (which are matrices for which m.mT = I) with real + * coefficients. The module of the determinant of unit matrices is + * 1, among the orthogonal 3X3 matrices, only the ones having a + * positive determinant (+1) are rotation matrices.

+ + *

When a rotation is defined by a matrix with truncated values + * (typically when it is extracted from a technical sheet where only + * four to five significant digits are available), the matrix is not + * orthogonal anymore. This constructor handles this case + * transparently by using a copy of the given matrix and applying a + * correction to the copy in order to perfect its orthogonality. If + * the Frobenius norm of the correction needed is above the given + * threshold, then the matrix is considered to be too far from a + * true rotation matrix and an exception is thrown.

+ + * @param m rotation matrix + * @param threshold convergence threshold for the iterative + * orthogonality correction (convergence is reached when the + * difference between two steps of the Frobenius norm of the + * correction is below this threshold) + + * @exception NotARotationMatrixException if the matrix is not a 3X3 + * matrix, or if it cannot be transformed into an orthogonal matrix + * with the given threshold, or if the determinant of the resulting + * orthogonal matrix is negative + + */ + public RotationDS(final DerivativeStructure[][] m, final double threshold) + throws NotARotationMatrixException { + + // dimension check + if ((m.length != 3) || (m[0].length != 3) || + (m[1].length != 3) || (m[2].length != 3)) { + throw new NotARotationMatrixException( + LocalizedFormats.ROTATION_MATRIX_DIMENSIONS, + m.length, m[0].length); + } + + // compute a "close" orthogonal matrix + final DerivativeStructure[][] ort = orthogonalizeMatrix(m, threshold); + + // check the sign of the determinant + final DerivativeStructure d0 = ort[1][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[1][2])); + final DerivativeStructure d1 = ort[0][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[0][2])); + final DerivativeStructure d2 = ort[0][1].multiply(ort[1][2]).subtract(ort[1][1].multiply(ort[0][2])); + final DerivativeStructure det = + ort[0][0].multiply(d0).subtract(ort[1][0].multiply(d1)).add(ort[2][0].multiply(d2)); + if (det.getValue() < 0.0) { + throw new NotARotationMatrixException( + LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, + det); + } + + final DerivativeStructure[] quat = mat2quat(ort); + q0 = quat[0]; + q1 = quat[1]; + q2 = quat[2]; + q3 = quat[3]; + + } + + /** Build the rotation that transforms a pair of vector into another pair. + + *

Except for possible scale factors, if the instance were applied to + * the pair (u1, u2) it will produce the pair + * (v1, v2).

+ + *

If the angular separation between u1 and u2 is + * not the same as the angular separation between v1 and + * v2, then a corrected v'2 will be used rather than + * v2, the corrected vector will be in the (v1, + * v2) plane.

+ + * @param u1 first vector of the origin pair + * @param u2 second vector of the origin pair + * @param v1 desired image of u1 by the rotation + * @param v2 desired image of u2 by the rotation + * @exception MathArithmeticException if the norm of one of the vectors is zero, + * or if one of the pair is degenerated (i.e. the vectors of the pair are colinear) + */ + public RotationDS(Vector3DDS u1, Vector3DDS u2, Vector3DDS v1, Vector3DDS v2) + throws MathArithmeticException { + + // build orthonormalized base from u1, u2 + // this fails when vectors are null or colinear, which is forbidden to define a rotation + final Vector3DDS u3 = u1.crossProduct(u2).normalize(); + u2 = u3.crossProduct(u1).normalize(); + u1 = u1.normalize(); + + // build an orthonormalized base from v1, v2 + // this fails when vectors are null or colinear, which is forbidden to define a rotation + final Vector3DDS v3 = v1.crossProduct(v2).normalize(); + v2 = v3.crossProduct(v1).normalize(); + v1 = v1.normalize(); + + // buid a matrix transforming the first base into the second one + final DerivativeStructure[][] m = new DerivativeStructure[][] { + { + MathArrays.linearCombination(u1.getX(), v1.getX(), u2.getX(), v2.getX(), u3.getX(), v3.getX()), + MathArrays.linearCombination(u1.getY(), v1.getX(), u2.getY(), v2.getX(), u3.getY(), v3.getX()), + MathArrays.linearCombination(u1.getZ(), v1.getX(), u2.getZ(), v2.getX(), u3.getZ(), v3.getX()) + }, + { + MathArrays.linearCombination(u1.getX(), v1.getY(), u2.getX(), v2.getY(), u3.getX(), v3.getY()), + MathArrays.linearCombination(u1.getY(), v1.getY(), u2.getY(), v2.getY(), u3.getY(), v3.getY()), + MathArrays.linearCombination(u1.getZ(), v1.getY(), u2.getZ(), v2.getY(), u3.getZ(), v3.getY()) + }, + { + MathArrays.linearCombination(u1.getX(), v1.getZ(), u2.getX(), v2.getZ(), u3.getX(), v3.getZ()), + MathArrays.linearCombination(u1.getY(), v1.getZ(), u2.getY(), v2.getZ(), u3.getY(), v3.getZ()), + MathArrays.linearCombination(u1.getZ(), v1.getZ(), u2.getZ(), v2.getZ(), u3.getZ(), v3.getZ()) + } + }; + + DerivativeStructure[] quat = mat2quat(m); + q0 = quat[0]; + q1 = quat[1]; + q2 = quat[2]; + q3 = quat[3]; + + } + + /** Build one of the rotations that transform one vector into another one. + + *

Except for a possible scale factor, if the instance were + * applied to the vector u it will produce the vector v. There is an + * infinite number of such rotations, this constructor choose the + * one with the smallest associated angle (i.e. the one whose axis + * is orthogonal to the (u, v) plane). If u and v are colinear, an + * arbitrary rotation axis is chosen.

+ + * @param u origin vector + * @param v desired image of u by the rotation + * @exception MathArithmeticException if the norm of one of the vectors is zero + */ + public RotationDS(final Vector3DDS u, final Vector3DDS v) throws MathArithmeticException { + + final DerivativeStructure normProduct = u.getNorm().multiply(v.getNorm()); + if (normProduct.getValue() == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); + } + + final DerivativeStructure dot = u.dotProduct(v); + + if (dot.getValue() < ((2.0e-15 - 1.0) * normProduct.getValue())) { + // special case u = -v: we select a PI angle rotation around + // an arbitrary vector orthogonal to u + final Vector3DDS w = u.orthogonal(); + q0 = normProduct.getField().getZero(); + q1 = w.getX().negate(); + q2 = w.getY().negate(); + q3 = w.getZ().negate(); + } else { + // general case: (u, v) defines a plane, we select + // the shortest possible rotation: axis orthogonal to this plane + q0 = dot.divide(normProduct).add(1.0).multiply(0.5).sqrt(); + final DerivativeStructure coeff = q0.multiply(normProduct).multiply(2.0).reciprocal(); + final Vector3DDS q = v.crossProduct(u); + q1 = coeff.multiply(q.getX()); + q2 = coeff.multiply(q.getY()); + q3 = coeff.multiply(q.getZ()); + } + + } + + /** Build a rotation from three Cardan or Euler elementary rotations. + + *

Cardan rotations are three successive rotations around the + * canonical axes X, Y and Z, each axis being used once. There are + * 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler + * rotations are three successive rotations around the canonical + * axes X, Y and Z, the first and last rotations being around the + * same axis. There are 6 such sets of rotations (XYX, XZX, YXY, + * YZY, ZXZ and ZYZ), the most popular one being ZXZ.

+ *

Beware that many people routinely use the term Euler angles even + * for what really are Cardan angles (this confusion is especially + * widespread in the aerospace business where Roll, Pitch and Yaw angles + * are often wrongly tagged as Euler angles).

+ + * @param order order of rotations to use + * @param alpha1 angle of the first elementary rotation + * @param alpha2 angle of the second elementary rotation + * @param alpha3 angle of the third elementary rotation + */ + public RotationDS(final RotationOrder order, final DerivativeStructure alpha1, + final DerivativeStructure alpha2, final DerivativeStructure alpha3) { + final int p = alpha1.getFreeParameters(); + final int o = alpha1.getOrder(); + final RotationDS r1 = + new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA1().getX()), + new DerivativeStructure(p, o, order.getA1().getY()), + new DerivativeStructure(p, o, order.getA1().getZ())), + alpha1); + final RotationDS r2 = + new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA2().getX()), + new DerivativeStructure(p, o, order.getA2().getY()), + new DerivativeStructure(p, o, order.getA2().getZ())), + alpha2); + final RotationDS r3 = + new RotationDS(new Vector3DDS(new DerivativeStructure(p, o, order.getA3().getX()), + new DerivativeStructure(p, o, order.getA3().getY()), + new DerivativeStructure(p, o, order.getA3().getZ())), + alpha3); + final RotationDS composed = r1.applyTo(r2.applyTo(r3)); + q0 = composed.q0; + q1 = composed.q1; + q2 = composed.q2; + q3 = composed.q3; + } + + /** Convert an orthogonal rotation matrix to a quaternion. + * @param ort orthogonal rotation matrix + * @return quaternion corresponding to the matrix + */ + private static DerivativeStructure[] mat2quat(final DerivativeStructure[][] ort) { + + final DerivativeStructure[] quat = new DerivativeStructure[4]; + + // There are different ways to compute the quaternions elements + // from the matrix. They all involve computing one element from + // the diagonal of the matrix, and computing the three other ones + // using a formula involving a division by the first element, + // which unfortunately can be zero. Since the norm of the + // quaternion is 1, we know at least one element has an absolute + // value greater or equal to 0.5, so it is always possible to + // select the right formula and avoid division by zero and even + // numerical inaccuracy. Checking the elements in turn and using + // the first one greater than 0.45 is safe (this leads to a simple + // test since qi = 0.45 implies 4 qi^2 - 1 = -0.19) + DerivativeStructure s = ort[0][0].add(ort[1][1]).add(ort[2][2]); + if (s.getValue() > -0.19) { + // compute q0 and deduce q1, q2 and q3 + quat[0] = s.add(1.0).sqrt().multiply(0.5); + DerivativeStructure inv = quat[0].reciprocal().multiply(0.25); + quat[1] = inv.multiply(ort[1][2].subtract(ort[2][1])); + quat[2] = inv.multiply(ort[2][0].subtract(ort[0][2])); + quat[3] = inv.multiply(ort[0][1].subtract(ort[1][0])); + } else { + s = ort[0][0].subtract(ort[1][1]).subtract(ort[2][2]); + if (s.getValue() > -0.19) { + // compute q1 and deduce q0, q2 and q3 + quat[1] = s.add(1.0).sqrt().multiply(0.5); + DerivativeStructure inv = quat[1].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[1][2].subtract(ort[2][1])); + quat[2] = inv.multiply(ort[0][1].add(ort[1][0])); + quat[3] = inv.multiply(ort[0][2].add(ort[2][0])); + } else { + s = ort[1][1].subtract(ort[0][0]).subtract(ort[2][2]); + if (s.getValue() > -0.19) { + // compute q2 and deduce q0, q1 and q3 + quat[2] = s.add(1.0).sqrt().multiply(0.5); + DerivativeStructure inv = quat[2].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[2][0].subtract(ort[0][2])); + quat[1] = inv.multiply(ort[0][1].add(ort[1][0])); + quat[3] = inv.multiply(ort[2][1].add(ort[1][2])); + } else { + // compute q3 and deduce q0, q1 and q2 + s = ort[2][2].subtract(ort[0][0]).subtract(ort[1][1]); + quat[3] = s.add(1.0).sqrt().multiply(0.5); + DerivativeStructure inv = quat[3].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[0][1].subtract(ort[1][0])); + quat[1] = inv.multiply(ort[0][2].add(ort[2][0])); + quat[2] = inv.multiply(ort[2][1].add(ort[1][2])); + } + } + } + + return quat; + + } + + /** Revert a rotation. + * Build a rotation which reverse the effect of another + * rotation. This means that if r(u) = v, then r.revert(v) = u. The + * instance is not changed. + * @return a new rotation whose effect is the reverse of the effect + * of the instance + */ + public RotationDS revert() { + return new RotationDS(q0.negate(), q1, q2, q3, false); + } + + /** Get the scalar coordinate of the quaternion. + * @return scalar coordinate of the quaternion + */ + public DerivativeStructure getQ0() { + return q0; + } + + /** Get the first coordinate of the vectorial part of the quaternion. + * @return first coordinate of the vectorial part of the quaternion + */ + public DerivativeStructure getQ1() { + return q1; + } + + /** Get the second coordinate of the vectorial part of the quaternion. + * @return second coordinate of the vectorial part of the quaternion + */ + public DerivativeStructure getQ2() { + return q2; + } + + /** Get the third coordinate of the vectorial part of the quaternion. + * @return third coordinate of the vectorial part of the quaternion + */ + public DerivativeStructure getQ3() { + return q3; + } + + /** Get the normalized axis of the rotation. + * @return normalized axis of the rotation + * @see #Rotation(Vector3DDS, DerivativeStructure) + */ + public Vector3DDS getAxis() { + final DerivativeStructure squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)); + if (squaredSine.getValue() == 0) { + final Field field = squaredSine.getField(); + return new Vector3DDS(field.getOne(), field.getZero(), field.getZero()); + } else if (q0.getValue() < 0) { + DerivativeStructure inverse = squaredSine.sqrt().reciprocal(); + return new Vector3DDS(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + } + final DerivativeStructure inverse = squaredSine.sqrt().reciprocal().negate(); + return new Vector3DDS(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + } + + /** Get the angle of the rotation. + * @return angle of the rotation (between 0 and π) + * @see #Rotation(Vector3DDS, DerivativeStructure) + */ + public DerivativeStructure getAngle() { + if ((q0.getValue() < -0.1) || (q0.getValue() > 0.1)) { + return q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().asin().multiply(2); + } else if (q0.getValue() < 0) { + return q0.negate().acos().multiply(2); + } + return q0.acos().multiply(2); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + *

The equations show that each rotation can be defined by two + * different values of the Cardan or Euler angles set. For example + * if Cardan angles are used, the rotation defined by the angles + * a1, a2 and a3 is the same as + * the rotation defined by the angles π + a1, π + * - a2 and π + a3. This method implements + * the following arbitrary choices:

+ *
    + *
  • for Cardan angles, the chosen set is the one for which the + * second angle is between -π/2 and π/2 (i.e its cosine is + * positive),
    • + *
  • for Euler angles, the chosen set is the one for which the + * second angle is between 0 and π (i.e its sine is positive).
    • + *
+ + *

Cardan and Euler angle have a very disappointing drawback: all + * of them have singularities. This means that if the instance is + * too close to the singularities corresponding to the given + * rotation order, it will be impossible to retrieve the angles. For + * Cardan angles, this is often called gimbal lock. There is + * nothing to do to prevent this, it is an intrinsic problem + * with Cardan and Euler representation (but not a problem with the + * rotation itself, which is perfectly well defined). For Cardan + * angles, singularities occur when the second angle is close to + * -π/2 or +π/2, for Euler angle singularities occur when the + * second angle is close to 0 or π, this implies that the identity + * rotation is always singular for Euler angles!

+ + * @param order rotation order to use + * @return an array of three angles, in the order specified by the set + * @exception CardanEulerSingularityException if the rotation is + * singular with respect to the angles set specified + */ + public DerivativeStructure[] getAngles(final RotationOrder order) + throws CardanEulerSingularityException { + + if (order == RotationOrder.XYZ) { + + // r (+K) coordinates are : + // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (+I) coordinates are : + // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) + final // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + Vector3DDS v1 = applyTo(vector(0, 0, 1)); + final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getY().negate(), v1.getZ()), + v2.getZ().asin(), + DerivativeStructure.atan2(v2.getY().negate(), v2.getX()) + }; + + } else if (order == RotationOrder.XZY) { + + // r (+J) coordinates are : + // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) + // (-r) (+I) coordinates are : + // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + final Vector3DDS v1 = applyTo(vector(0, 1, 0)); + final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getZ(), v1.getY()), + v2.getY().asin().negate(), + DerivativeStructure.atan2(v2.getZ(), v2.getX()) + }; + + } else if (order == RotationOrder.YXZ) { + + // r (+K) coordinates are : + // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) + // (-r) (+J) coordinates are : + // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + final Vector3DDS v1 = applyTo(vector(0, 0, 1)); + final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getX(), v1.getZ()), + v2.getZ().asin().negate(), + DerivativeStructure.atan2(v2.getX(), v2.getY()) + }; + + } else if (order == RotationOrder.YZX) { + + // r (+I) coordinates are : + // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) + // (-r) (+J) coordinates are : + // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + final Vector3DDS v1 = applyTo(vector(1, 0, 0)); + final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getZ().negate(), v1.getX()), + v2.getX().asin(), + DerivativeStructure.atan2(v2.getZ().negate(), v2.getY()) + }; + + } else if (order == RotationOrder.ZXY) { + + // r (+J) coordinates are : + // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) + // (-r) (+K) coordinates are : + // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + final Vector3DDS v1 = applyTo(vector(0, 1, 0)); + final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getX().negate(), v1.getY()), + v2.getY().asin(), + DerivativeStructure.atan2(v2.getX().negate(), v2.getZ()) + }; + + } else if (order == RotationOrder.ZYX) { + + // r (+I) coordinates are : + // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) + // (-r) (+K) coordinates are : + // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + final Vector3DDS v1 = applyTo(vector(1, 0, 0)); + final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getY(), v1.getX()), + v2.getX().asin().negate(), + DerivativeStructure.atan2(v2.getY(), v2.getZ()) + }; + + } else if (order == RotationOrder.XYX) { + + // r (+I) coordinates are : + // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) + // (-r) (+I) coordinates are : + // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) + // and we can choose to have theta in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(1, 0, 0)); + final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getY(), v1.getZ().negate()), + v2.getX().acos(), + DerivativeStructure.atan2(v2.getY(), v2.getZ()) + }; + + } else if (order == RotationOrder.XZX) { + + // r (+I) coordinates are : + // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) + // (-r) (+I) coordinates are : + // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) + // and we can choose to have psi in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(1, 0, 0)); + final Vector3DDS v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getValue() < -0.9999999999) || (v2.getX().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getZ(), v1.getY()), + v2.getX().acos(), + DerivativeStructure.atan2(v2.getZ(), v2.getY().negate()) + }; + + } else if (order == RotationOrder.YXY) { + + // r (+J) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // (-r) (+J) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // and we can choose to have phi in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(0, 1, 0)); + final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getX(), v1.getZ()), + v2.getY().acos(), + DerivativeStructure.atan2(v2.getX(), v2.getZ().negate()) + }; + + } else if (order == RotationOrder.YZY) { + + // r (+J) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // (-r) (+J) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // and we can choose to have psi in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(0, 1, 0)); + final Vector3DDS v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getValue() < -0.9999999999) || (v2.getY().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getZ(), v1.getX().negate()), + v2.getY().acos(), + DerivativeStructure.atan2(v2.getZ(), v2.getX()) + }; + + } else if (order == RotationOrder.ZXZ) { + + // r (+K) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // (-r) (+K) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(0, 0, 1)); + final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getX(), v1.getY().negate()), + v2.getZ().acos(), + DerivativeStructure.atan2(v2.getX(), v2.getY()) + }; + + } else { // last possibility is ZYZ + + // r (+K) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // (-r) (+K) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + final Vector3DDS v1 = applyTo(vector(0, 0, 1)); + final Vector3DDS v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getValue() < -0.9999999999) || (v2.getZ().getValue() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new DerivativeStructure[] { + DerivativeStructure.atan2(v1.getY(), v1.getX()), + v2.getZ().acos(), + DerivativeStructure.atan2(v2.getY(), v2.getX().negate()) + }; + + } + + } + + /** Create a constant vector with appropriate derivation parameters. + * @param x abscissa + * @param y ordinate + * @param z height + * @return a constant vector + */ + private Vector3DDS vector(final double x, final double y, final double z) { + final int parameters = q0.getFreeParameters(); + final int order = q0.getOrder(); + return new Vector3DDS(new DerivativeStructure(parameters, order, x), + new DerivativeStructure(parameters, order, y), + new DerivativeStructure(parameters, order, z)); + } + + /** Get the 3X3 matrix corresponding to the instance + * @return the matrix corresponding to the instance + */ + public DerivativeStructure[][] getMatrix() { + + // products + final DerivativeStructure q0q0 = q0.multiply(q0); + final DerivativeStructure q0q1 = q0.multiply(q1); + final DerivativeStructure q0q2 = q0.multiply(q2); + final DerivativeStructure q0q3 = q0.multiply(q3); + final DerivativeStructure q1q1 = q1.multiply(q1); + final DerivativeStructure q1q2 = q1.multiply(q2); + final DerivativeStructure q1q3 = q1.multiply(q3); + final DerivativeStructure q2q2 = q2.multiply(q2); + final DerivativeStructure q2q3 = q2.multiply(q3); + final DerivativeStructure q3q3 = q3.multiply(q3); + + // create the matrix + final DerivativeStructure[][] m = new DerivativeStructure[3][]; + m[0] = new DerivativeStructure[3]; + m[1] = new DerivativeStructure[3]; + m[2] = new DerivativeStructure[3]; + + m [0][0] = q0q0.add(q1q1).multiply(2).subtract(1); + m [1][0] = q1q2.subtract(q0q3).multiply(2); + m [2][0] = q1q3.add(q0q2).multiply(2); + + m [0][1] = q1q2.add(q0q3).multiply(2); + m [1][1] = q0q0.add(q2q2).multiply(2).subtract(1); + m [2][1] = q2q3.subtract(q0q1).multiply(2); + + m [0][2] = q1q3.subtract(q0q2).multiply(2); + m [1][2] = q2q3.add(q0q1).multiply(2); + m [2][2] = q0q0.add(q3q3).multiply(2).subtract(1); + + return m; + + } + + /** Apply the rotation to a vector. + * @param u vector to apply the rotation to + * @return a new vector which is the image of u by the rotation + */ + public Vector3DDS applyTo(final Vector3DDS u) { + + final DerivativeStructure x = u.getX(); + final DerivativeStructure y = u.getY(); + final DerivativeStructure z = u.getZ(); + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + return new Vector3DDS(q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + q0.multiply(z.multiply(q0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + + } + + /** Apply the rotation to a vector. + * @param u vector to apply the rotation to + * @return a new vector which is the image of u by the rotation + */ + public Vector3DDS applyTo(final Vector3D u) { + + final double x = u.getX(); + final double y = u.getY(); + final double z = u.getZ(); + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + return new Vector3DDS(q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + q0.multiply(q0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + + } + + /** Apply the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to (it can be the same + * array as in) + */ + public void applyTo(final DerivativeStructure[] in, final DerivativeStructure[] out) { + + final DerivativeStructure x = in[0]; + final DerivativeStructure y = in[1]; + final DerivativeStructure z = in[2]; + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + out[0] = q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + out[2] = q0.multiply(z.multiply(q0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z); + + } + + /** Apply the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to + */ + public void applyTo(final double[] in, final DerivativeStructure[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + out[0] = q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + out[2] = q0.multiply(q0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z); + + } + + /** Apply a rotation to a vector. + * @param r rotation to apply + * @param u vector to apply the rotation to + * @return a new vector which is the image of u by the rotation + */ + public static Vector3DDS applyTo(final Rotation r, final Vector3DDS u) { + + final DerivativeStructure x = u.getX(); + final DerivativeStructure y = u.getY(); + final DerivativeStructure z = u.getZ(); + + final DerivativeStructure s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + + return new Vector3DDS(x.multiply(r.getQ0()).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(r.getQ0()).add(s.multiply(r.getQ1())).multiply(2).subtract(x), + y.multiply(r.getQ0()).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(r.getQ0()).add(s.multiply(r.getQ2())).multiply(2).subtract(y), + z.multiply(r.getQ0()).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(r.getQ0()).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); + + } + + /** Apply the inverse of the rotation to a vector. + * @param u vector to apply the inverse of the rotation to + * @return a new vector which such that u is its image by the rotation + */ + public Vector3DDS applyInverseTo(final Vector3DDS u) { + + final DerivativeStructure x = u.getX(); + final DerivativeStructure y = u.getY(); + final DerivativeStructure z = u.getZ(); + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final DerivativeStructure m0 = q0.negate(); + + return new Vector3DDS(m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + m0.multiply(z.multiply(m0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + + } + + /** Apply the inverse of the rotation to a vector. + * @param u vector to apply the inverse of the rotation to + * @return a new vector which such that u is its image by the rotation + */ + public Vector3DDS applyInverseTo(final Vector3D u) { + + final double x = u.getX(); + final double y = u.getY(); + final double z = u.getZ(); + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final DerivativeStructure m0 = q0.negate(); + + return new Vector3DDS(m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x), + m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y), + m0.multiply(m0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z)); + + } + + /** Apply the inverse of the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to (it can be the same + * array as in) + */ + public void applyInverseTo(final DerivativeStructure[] in, final DerivativeStructure[] out) { + + final DerivativeStructure x = in[0]; + final DerivativeStructure y = in[1]; + final DerivativeStructure z = in[2]; + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final DerivativeStructure m0 = q0.negate(); + + out[0] = m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + out[2] = m0.multiply(z.multiply(m0).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z); + + } + + /** Apply the inverse of the rotation to a vector stored in an array. + * @param in an array with three items which stores vector to rotate + * @param out an array with three items to put result to + */ + public void applyInverseTo(final double[] in, final DerivativeStructure[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final DerivativeStructure s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final DerivativeStructure m0 = q0.negate(); + + out[0] = m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + out[2] = m0.multiply(m0.multiply(z).subtract(q1.multiply(y).subtract(q2.multiply(x)))).add(s.multiply(q3)).multiply(2).subtract(z); + + } + + /** Apply the inverse of a rotation to a vector. + * @param r rotation to apply + * @param u vector to apply the inverse of the rotation to + * @return a new vector which such that u is its image by the rotation + */ + public static Vector3DDS applyInverseTo(final Rotation r, final Vector3DDS u) { + + final DerivativeStructure x = u.getX(); + final DerivativeStructure y = u.getY(); + final DerivativeStructure z = u.getZ(); + + final DerivativeStructure s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + final double m0 = -r.getQ0(); + + return new Vector3DDS(x.multiply(m0).subtract(z.multiply(r.getQ2()).subtract(y.multiply(r.getQ3()))).multiply(m0).add(s.multiply(r.getQ1())).multiply(2).subtract(x), + y.multiply(m0).subtract(x.multiply(r.getQ3()).subtract(z.multiply(r.getQ1()))).multiply(m0).add(s.multiply(r.getQ2())).multiply(2).subtract(y), + z.multiply(m0).subtract(y.multiply(r.getQ1()).subtract(x.multiply(r.getQ2()))).multiply(m0).add(s.multiply(r.getQ3())).multiply(2).subtract(z)); + + } + + /** Apply the instance to another rotation. + * Applying the instance to a rotation is computing the composition + * in an order compliant with the following rule : let u be any + * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image + * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), + * where comp = applyTo(r). + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public RotationDS applyTo(final RotationDS r) { + return new RotationDS(r.q0.multiply(q0).subtract(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))), + r.q1.multiply(q0).add(r.q0.multiply(q1)).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))), + r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))), + r.q3.multiply(q0).add(r.q0.multiply(q3)).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))), + false); + } + + /** Apply the instance to another rotation. + * Applying the instance to a rotation is computing the composition + * in an order compliant with the following rule : let u be any + * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image + * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), + * where comp = applyTo(r). + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public RotationDS applyTo(final Rotation r) { + return new RotationDS(q0.multiply(r.getQ0()).subtract(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))), + q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))), + q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))), + q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))), + false); + } + + /** Apply a rotation to another rotation. + * Applying a rotation to another rotation is computing the composition + * in an order compliant with the following rule : let u be any + * vector and v its image by rInner (i.e. rInner.applyTo(u) = v), let w be the image + * of v by rOuter (i.e. rOuter.applyTo(v) = w), then w = comp.applyTo(u), + * where comp = applyTo(rOuter, rInner). + * @param r1 rotation to apply + * @param rInner rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public static RotationDS applyTo(final Rotation r1, final RotationDS rInner) { + return new RotationDS(rInner.q0.multiply(r1.getQ0()).subtract(rInner.q1.multiply(r1.getQ1()).add(rInner.q2.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ3()))), + rInner.q1.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ1())).add(rInner.q2.multiply(r1.getQ3()).subtract(rInner.q3.multiply(r1.getQ2()))), + rInner.q2.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ2())).add(rInner.q3.multiply(r1.getQ1()).subtract(rInner.q1.multiply(r1.getQ3()))), + rInner.q3.multiply(r1.getQ0()).add(rInner.q0.multiply(r1.getQ3())).add(rInner.q1.multiply(r1.getQ2()).subtract(rInner.q2.multiply(r1.getQ1()))), + false); + } + + /** Apply the inverse of the instance to another rotation. + * Applying the inverse of the instance to a rotation is computing + * the composition in an order compliant with the following rule : + * let u be any vector and v its image by r (i.e. r.applyTo(u) = v), + * let w be the inverse image of v by the instance + * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where + * comp = applyInverseTo(r). + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public RotationDS applyInverseTo(final RotationDS r) { + return new RotationDS(r.q0.multiply(q0).add(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))).negate(), + r.q0.multiply(q1).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))).subtract(r.q1.multiply(q0)), + r.q0.multiply(q2).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))).subtract(r.q2.multiply(q0)), + r.q0.multiply(q3).add(r.q1.multiply(q2).subtract(r.q2.multiply(q1))).subtract(r.q3.multiply(q0)), + false); + } + + /** Apply the inverse of the instance to another rotation. + * Applying the inverse of the instance to a rotation is computing + * the composition in an order compliant with the following rule : + * let u be any vector and v its image by r (i.e. r.applyTo(u) = v), + * let w be the inverse image of v by the instance + * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where + * comp = applyInverseTo(r). + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public RotationDS applyInverseTo(final Rotation r) { + return new RotationDS(q0.multiply(r.getQ0()).add(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))).negate(), + q1.multiply(r.getQ0()).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))).subtract(q0.multiply(r.getQ1())), + q2.multiply(r.getQ0()).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))).subtract(q0.multiply(r.getQ2())), + q3.multiply(r.getQ0()).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))).subtract(q0.multiply(r.getQ3())), + false); + } + + /** Apply the inverse of a rotation to another rotation. + * Applying the inverse of a rotation to another rotation is computing + * the composition in an order compliant with the following rule : + * let u be any vector and v its image by rInner (i.e. rInner.applyTo(u) = v), + * let w be the inverse image of v by rOuter + * (i.e. rOuter.applyInverseTo(v) = w), then w = comp.applyTo(u), where + * comp = applyInverseTo(rOuter, rInner). + * @param rOuter rotation to apply the rotation to + * @param rInner rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public static RotationDS applyInverseTo(final Rotation rOuter, final RotationDS rInner) { + return new RotationDS(rInner.q0.multiply(rOuter.getQ0()).add(rInner.q1.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ2())).add(rInner.q3.multiply(rOuter.getQ3()))).negate(), + rInner.q0.multiply(rOuter.getQ1()).add(rInner.q2.multiply(rOuter.getQ3()).subtract(rInner.q3.multiply(rOuter.getQ2()))).subtract(rInner.q1.multiply(rOuter.getQ0())), + rInner.q0.multiply(rOuter.getQ2()).add(rInner.q3.multiply(rOuter.getQ1()).subtract(rInner.q1.multiply(rOuter.getQ3()))).subtract(rInner.q2.multiply(rOuter.getQ0())), + rInner.q0.multiply(rOuter.getQ3()).add(rInner.q1.multiply(rOuter.getQ2()).subtract(rInner.q2.multiply(rOuter.getQ1()))).subtract(rInner.q3.multiply(rOuter.getQ0())), + false); + } + + /** Perfect orthogonality on a 3X3 matrix. + * @param m initial matrix (not exactly orthogonal) + * @param threshold convergence threshold for the iterative + * orthogonality correction (convergence is reached when the + * difference between two steps of the Frobenius norm of the + * correction is below this threshold) + * @return an orthogonal matrix close to m + * @exception NotARotationMatrixException if the matrix cannot be + * orthogonalized with the given threshold after 10 iterations + */ + private DerivativeStructure[][] orthogonalizeMatrix(final DerivativeStructure[][] m, + final double threshold) + throws NotARotationMatrixException { + + DerivativeStructure x00 = m[0][0]; + DerivativeStructure x01 = m[0][1]; + DerivativeStructure x02 = m[0][2]; + DerivativeStructure x10 = m[1][0]; + DerivativeStructure x11 = m[1][1]; + DerivativeStructure x12 = m[1][2]; + DerivativeStructure x20 = m[2][0]; + DerivativeStructure x21 = m[2][1]; + DerivativeStructure x22 = m[2][2]; + double fn = 0; + double fn1; + + final DerivativeStructure[][] o = new DerivativeStructure[3][3]; + + // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) + int i = 0; + while (++i < 11) { + + // Mt.Xn + final DerivativeStructure mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20)); + final DerivativeStructure mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20)); + final DerivativeStructure mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20)); + final DerivativeStructure mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21)); + final DerivativeStructure mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21)); + final DerivativeStructure mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21)); + final DerivativeStructure mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22)); + final DerivativeStructure mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22)); + final DerivativeStructure mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22)); + + // Xn+1 + o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5)); + o[0][1] = x01.subtract(x00.multiply(mx01).add(x01.multiply(mx11)).add(x02.multiply(mx21)).subtract(m[0][1]).multiply(0.5)); + o[0][2] = x02.subtract(x00.multiply(mx02).add(x01.multiply(mx12)).add(x02.multiply(mx22)).subtract(m[0][2]).multiply(0.5)); + o[1][0] = x10.subtract(x10.multiply(mx00).add(x11.multiply(mx10)).add(x12.multiply(mx20)).subtract(m[1][0]).multiply(0.5)); + o[1][1] = x11.subtract(x10.multiply(mx01).add(x11.multiply(mx11)).add(x12.multiply(mx21)).subtract(m[1][1]).multiply(0.5)); + o[1][2] = x12.subtract(x10.multiply(mx02).add(x11.multiply(mx12)).add(x12.multiply(mx22)).subtract(m[1][2]).multiply(0.5)); + o[2][0] = x20.subtract(x20.multiply(mx00).add(x21.multiply(mx10)).add(x22.multiply(mx20)).subtract(m[2][0]).multiply(0.5)); + o[2][1] = x21.subtract(x20.multiply(mx01).add(x21.multiply(mx11)).add(x22.multiply(mx21)).subtract(m[2][1]).multiply(0.5)); + o[2][2] = x22.subtract(x20.multiply(mx02).add(x21.multiply(mx12)).add(x22.multiply(mx22)).subtract(m[2][2]).multiply(0.5)); + + // correction on each elements + final double corr00 = o[0][0].getValue() - m[0][0].getValue(); + final double corr01 = o[0][1].getValue() - m[0][1].getValue(); + final double corr02 = o[0][2].getValue() - m[0][2].getValue(); + final double corr10 = o[1][0].getValue() - m[1][0].getValue(); + final double corr11 = o[1][1].getValue() - m[1][1].getValue(); + final double corr12 = o[1][2].getValue() - m[1][2].getValue(); + final double corr20 = o[2][0].getValue() - m[2][0].getValue(); + final double corr21 = o[2][1].getValue() - m[2][1].getValue(); + final double corr22 = o[2][2].getValue() - m[2][2].getValue(); + + // Frobenius norm of the correction + fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 + + corr10 * corr10 + corr11 * corr11 + corr12 * corr12 + + corr20 * corr20 + corr21 * corr21 + corr22 * corr22; + + // convergence test + if (FastMath.abs(fn1 - fn) <= threshold) { + return o; + } + + // prepare next iteration + x00 = o[0][0]; + x01 = o[0][1]; + x02 = o[0][2]; + x10 = o[1][0]; + x11 = o[1][1]; + x12 = o[1][2]; + x20 = o[2][0]; + x21 = o[2][1]; + x22 = o[2][2]; + fn = fn1; + + } + + // the algorithm did not converge after 10 iterations + throw new NotARotationMatrixException(LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX, + i - 1); + + } + + /** Compute the distance between two rotations. + *

The distance is intended here as a way to check if two + * rotations are almost similar (i.e. they transform vectors the same way) + * or very different. It is mathematically defined as the angle of + * the rotation r that prepended to one of the rotations gives the other + * one:

+ * 
+     *        r1(r) = r2
+     *
+ *

This distance is an angle between 0 and π. Its value is the smallest + * possible upper bound of the angle in radians between r1(v) + * and r2(v) for all possible vectors v. This upper bound is + * reached for some v. The distance is equal to 0 if and only if the two + * rotations are identical.

+ *

Comparing two rotations should always be done using this value rather + * than for example comparing the components of the quaternions. It is much + * more stable, and has a geometric meaning. Also comparing quaternions + * components is error prone since for example quaternions (0.36, 0.48, -0.48, -0.64) + * and (-0.36, -0.48, 0.48, 0.64) represent exactly the same rotation despite + * their components are different (they are exact opposites).

+ * @param r1 first rotation + * @param r2 second rotation + * @return distance between r1 and r2 + */ + public static DerivativeStructure distance(final RotationDS r1, final RotationDS r2) { + return r1.applyInverseTo(r2).getAngle(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java new file mode 100644 index 000000000..31e392ebb --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDS.java @@ -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}. + *

Instance of this class are guaranteed to be immutable.

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

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


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

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

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

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

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

+ *

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

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

+ * All NaN values have the same hash code.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

+ * @param v1 first vector + * @param v2 second vector + * @return the square of the distance between v1 and v2 + */ + public static DerivativeStructure distanceSq(Vector3D v1, Vector3DDS v2) { + return v2.distanceSq(v1); + } + + /** Get a string representation of this vector. + * @return a string representation of this vector + */ + @Override + public String toString() { + return Vector3DFormat.getInstance().format(toVector3D()); + } + + /** {@inheritDoc} */ + public String toString(final NumberFormat format) { + return new Vector3DFormat(format).format(toVector3D()); + } + +} diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java new file mode 100644 index 000000000..cd1497694 --- /dev/null +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/RotationDSTest.java @@ -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); + } + +} diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java new file mode 100644 index 000000000..58ff03234 --- /dev/null +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DDSTest.java @@ -0,0 +1,733 @@ +/* + * 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.text.DecimalFormat; +import java.text.DecimalFormatSymbols; +import java.text.NumberFormat; +import java.util.Locale; + +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.random.Well1024a; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.Precision; +import org.junit.Assert; +import org.junit.Test; + +public class Vector3DDSTest { + @Test + public void testConstructors() throws DimensionMismatchException { + double cosAlpha = 1 / 2.0; + double sinAlpha = FastMath.sqrt(3) / 2.0; + double cosDelta = FastMath.sqrt(2) / 2.0; + double sinDelta = -FastMath.sqrt(2) / 2.0; + Vector3DDS u = new Vector3DDS(2, + new Vector3DDS(new DerivativeStructure(2, 1, 0, FastMath.PI / 3), + new DerivativeStructure(2, 1, 1, -FastMath.PI / 4))); + checkVector(u, 2 * cosAlpha * cosDelta, 2 * sinAlpha * cosDelta, 2 * sinDelta); + Assert.assertEquals(-2 * sinAlpha * cosDelta, u.getX().getPartialDerivative(1, 0), 1.0e-12); + Assert.assertEquals(+2 * cosAlpha * cosDelta, u.getY().getPartialDerivative(1, 0), 1.0e-12); + Assert.assertEquals(0, u.getZ().getPartialDerivative(1, 0), 1.0e-12); + Assert.assertEquals(-2 * cosAlpha * sinDelta, u.getX().getPartialDerivative(0, 1), 1.0e-12); + Assert.assertEquals(-2 * sinAlpha * sinDelta, u.getY().getPartialDerivative(0, 1), 1.0e-12); + Assert.assertEquals(2 * cosDelta, u.getZ().getPartialDerivative(0, 1), 1.0e-12); + + checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3)), + 2, 0, 0, 2, 0, 0, 0, 2, 0, 0, 0, 2); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + createVector(1, 0, 0, 4)), + 2, 0, 0, 2, 0, 0, 1, 0, 2, 0, 0, 0, 0, 2, 0); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + new Vector3D(1, 0, 0)), + 2, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0); + + checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + -3, createVector(0, 0, -1, 3)), + 2, 0, 3, -1, 0, 0, 0, -1, 0, 0, 0, -1); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + createVector(1, 0, 0, 4), + new DerivativeStructure(4, 1, 3, -3.0), + createVector(0, 0, -1, 4)), + 2, 0, 3, -1, 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, -1); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + new Vector3D(1, 0, 0), + new DerivativeStructure(4, 1, 3, -3.0), + new Vector3D(0, 0, -1)), + 2, 0, 3, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1); + + checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + 5, createVector(0, 1, 0, 3), + -3, createVector(0, 0, -1, 3)), + 2, 5, 3, 4, 0, 0, 0, 4, 0, 0, 0, 4); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + createVector(1, 0, 0, 4), + new DerivativeStructure(4, 1, 3, 5.0), + createVector(0, 1, 0, 4), + new DerivativeStructure(4, 1, 3, -3.0), + createVector(0, 0, -1, 4)), + 2, 5, 3, 4, 0, 0, 1, 0, 4, 0, 1, 0, 0, 4, -1); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + new Vector3D(1, 0, 0), + new DerivativeStructure(4, 1, 3, 5.0), + new Vector3D(0, 1, 0), + new DerivativeStructure(4, 1, 3, -3.0), + new Vector3D(0, 0, -1)), + 2, 5, 3, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, -1); + + checkVector(new Vector3DDS(2, createVector(1, 0, 0, 3), + 5, createVector(0, 1, 0, 3), + 5, createVector(0, -1, 0, 3), + -3, createVector(0, 0, -1, 3)), + 2, 0, 3, 9, 0, 0, 0, 9, 0, 0, 0, 9); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + createVector(1, 0, 0, 4), + new DerivativeStructure(4, 1, 3, 5.0), + createVector(0, 1, 0, 4), + new DerivativeStructure(4, 1, 3, 5.0), + createVector(0, -1, 0, 4), + new DerivativeStructure(4, 1, 3, -3.0), + createVector(0, 0, -1, 4)), + 2, 0, 3, 9, 0, 0, 1, 0, 9, 0, 0, 0, 0, 9, -1); + checkVector(new Vector3DDS(new DerivativeStructure(4, 1, 3, 2.0), + new Vector3D(1, 0, 0), + new DerivativeStructure(4, 1, 3, 5.0), + new Vector3D(0, 1, 0), + new DerivativeStructure(4, 1, 3, 5.0), + new Vector3D(0, -1, 0), + new DerivativeStructure(4, 1, 3, -3.0), + new Vector3D(0, 0, -1)), + 2, 0, 3, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, -1); + + checkVector(new Vector3DDS(new DerivativeStructure[] { + new DerivativeStructure(3, 1, 2, 2), + new DerivativeStructure(3, 1, 1, 5), + new DerivativeStructure(3, 1, 0, -3) + }), + 2, 5, -3, 0, 0, 1, 0, 1, 0, 1, 0, 0); + + } + + @Test + public void testEquals() { + Vector3DDS u1 = createVector(1, 2, 3, 3); + Vector3DDS v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); + Assert.assertTrue(u1.equals(u1)); + Assert.assertTrue(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + new DerivativeStructure(3, 1, 1, 2.0), + new DerivativeStructure(3, 1, 2, 3.0)))); + Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 1.0), + new DerivativeStructure(3, 1, 1, 2.0), + new DerivativeStructure(3, 1, 2, 3.0)))); + Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + new DerivativeStructure(3, 1, 2.0), + new DerivativeStructure(3, 1, 2, 3.0)))); + Assert.assertFalse(u1.equals(new Vector3DDS(new DerivativeStructure(3, 1, 0, 1.0), + new DerivativeStructure(3, 1, 1, 2.0), + new DerivativeStructure(3, 1, 3.0)))); + Assert.assertFalse(u1.equals(v)); + Assert.assertFalse(u1.equals(u1.toVector3D())); + Assert.assertTrue(createVector(0, Double.NaN, 0, 3).equals(createVector(0, 0, Double.NaN, 3))); + } + + @Test + public void testHash() { + Assert.assertEquals(createVector(0, Double.NaN, 0, 3).hashCode(), createVector(0, 0, Double.NaN, 3).hashCode()); + Vector3DDS u = createVector(1, 2, 3, 3); + Vector3DDS v = createVector(1, 2, 3 + 10 * Precision.EPSILON, 3); + Assert.assertTrue(u.hashCode() != v.hashCode()); + } + + @Test + public void testInfinite() { + Assert.assertTrue(createVector(1, 1, Double.NEGATIVE_INFINITY, 3).isInfinite()); + Assert.assertTrue(createVector(1, Double.NEGATIVE_INFINITY, 1, 3).isInfinite()); + Assert.assertTrue(createVector(Double.NEGATIVE_INFINITY, 1, 1, 3).isInfinite()); + Assert.assertFalse(createVector(1, 1, 2, 3).isInfinite()); + Assert.assertFalse(createVector(1, Double.NaN, Double.NEGATIVE_INFINITY, 3).isInfinite()); + } + + @Test + public void testNaN() { + Assert.assertTrue(createVector(1, 1, Double.NaN, 3).isNaN()); + Assert.assertTrue(createVector(1, Double.NaN, 1, 3).isNaN()); + Assert.assertTrue(createVector(Double.NaN, 1, 1, 3).isNaN()); + Assert.assertFalse(createVector(1, 1, 2, 3).isNaN()); + Assert.assertFalse(createVector(1, 1, Double.NEGATIVE_INFINITY, 3).isNaN()); + } + + @Test + public void testToString() { + Assert.assertEquals("{3; 2; 1}", createVector(3, 2, 1, 3).toString()); + NumberFormat format = new DecimalFormat("0.000", new DecimalFormatSymbols(Locale.US)); + Assert.assertEquals("{3.000; 2.000; 1.000}", createVector(3, 2, 1, 3).toString(format)); + } + + @Test(expected=DimensionMismatchException.class) + public void testWrongDimension() throws DimensionMismatchException { + new Vector3DDS(new DerivativeStructure[] { + new DerivativeStructure(3, 1, 0, 2), + new DerivativeStructure(3, 1, 0, 5) + }); + } + + @Test + public void testCoordinates() { + Vector3DDS v = createVector(1, 2, 3, 3); + Assert.assertTrue(FastMath.abs(v.getX().getValue() - 1) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v.getY().getValue() - 2) < 1.0e-12); + Assert.assertTrue(FastMath.abs(v.getZ().getValue() - 3) < 1.0e-12); + DerivativeStructure[] coordinates = v.toArray(); + Assert.assertTrue(FastMath.abs(coordinates[0].getValue() - 1) < 1.0e-12); + Assert.assertTrue(FastMath.abs(coordinates[1].getValue() - 2) < 1.0e-12); + Assert.assertTrue(FastMath.abs(coordinates[2].getValue() - 3) < 1.0e-12); + } + + @Test + public void testNorm1() { + Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNorm1().getValue(), 0); + Assert.assertEquals( 6.0, createVector(1, -2, 3, 3).getNorm1().getValue(), 0); + Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals(-1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNorm1().getPartialDerivative(0, 0, 1), 0); + } + + @Test + public void testNorm() { + double r = FastMath.sqrt(14); + Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNorm().getValue(), 0); + Assert.assertEquals(r, createVector(1, 2, 3, 3).getNorm().getValue(), 1.0e-12); + Assert.assertEquals( 1.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 2.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 3.0 / r, createVector(1, 2, 3, 3).getNorm().getPartialDerivative(0, 0, 1), 0); + } + + @Test + public void testNormSq() { + Assert.assertEquals(0.0, createVector(0, 0, 0, 3).getNormSq().getValue(), 0); + Assert.assertEquals(14, createVector(1, 2, 3, 3).getNormSq().getValue(), 1.0e-12); + Assert.assertEquals( 2, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 4, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 6, createVector(1, 2, 3, 3).getNormSq().getPartialDerivative(0, 0, 1), 0); + } + + @Test + public void testNormInf() { + Assert.assertEquals( 0.0, createVector(0, 0, 0, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 3.0, createVector(1, -2, 3, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 0.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 0.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 1.0, createVector(1, -2, 3, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + Assert.assertEquals( 3.0, createVector(2, -1, 3, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 0.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 0.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 1.0, createVector(2, -1, 3, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + Assert.assertEquals( 3.0, createVector(1, -3, 2, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 0.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals(-1.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 0.0, createVector(1, -3, 2, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + Assert.assertEquals( 3.0, createVector(2, -3, 1, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 0.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals(-1.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 0.0, createVector(2, -3, 1, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + Assert.assertEquals( 3.0, createVector(3, -1, 2, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 1.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 0.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 0.0, createVector(3, -1, 2, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + Assert.assertEquals( 3.0, createVector(3, -2, 1, 3).getNormInf().getValue(), 0); + Assert.assertEquals( 1.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(1, 0, 0), 0); + Assert.assertEquals( 0.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(0, 1, 0), 0); + Assert.assertEquals( 0.0, createVector(3, -2, 1, 3).getNormInf().getPartialDerivative(0, 0, 1), 0); + } + + @Test + public void testDistance1() { + Vector3DDS v1 = createVector(1, -2, 3, 3); + Vector3DDS v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, Vector3DDS.distance1(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); + DerivativeStructure distance = Vector3DDS.distance1(v1, v2); + Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distance1(v1, new Vector3D(-4, 2, 0)); + Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); + Assert.assertEquals( 1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-1, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 1, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distance1(new Vector3D(-4, 2, 0), v1); + Assert.assertEquals(12.0, distance.getValue(), 1.0e-12); + Assert.assertEquals( 1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-1, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 1, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(v1.subtract(v2).getNorm1().getValue(), Vector3DDS.distance1(v1, v2).getValue(), 1.0e-12); + } + + @Test + public void testDistance() { + Vector3DDS v1 = createVector(1, -2, 3, 3); + Vector3DDS v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, Vector3DDS.distance(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); + DerivativeStructure distance = Vector3DDS.distance(v1, v2); + Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distance(v1, new Vector3D(-4, 2, 0)); + Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); + Assert.assertEquals( 5 / FastMath.sqrt(50), distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-4 / FastMath.sqrt(50), distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 3 / FastMath.sqrt(50), distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distance(new Vector3D(-4, 2, 0), v1); + Assert.assertEquals(FastMath.sqrt(50), distance.getValue(), 1.0e-12); + Assert.assertEquals( 5 / FastMath.sqrt(50), distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-4 / FastMath.sqrt(50), distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 3 / FastMath.sqrt(50), distance.getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(v1.subtract(v2).getNorm().getValue(), Vector3DDS.distance(v1, v2).getValue(), 1.0e-12); + } + + @Test + public void testDistanceSq() { + Vector3DDS v1 = createVector(1, -2, 3, 3); + Vector3DDS v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, Vector3DDS.distanceSq(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); + DerivativeStructure distanceSq = Vector3DDS.distanceSq(v1, v2); + Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); + Assert.assertEquals(0, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); + distanceSq = Vector3DDS.distanceSq(v1, new Vector3D(-4, 2, 0)); + Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); + Assert.assertEquals(10, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-8, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 6, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); + distanceSq = Vector3DDS.distanceSq(new Vector3D(-4, 2, 0), v1); + Assert.assertEquals(50.0, distanceSq.getValue(), 1.0e-12); + Assert.assertEquals(10, distanceSq.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(-8, distanceSq.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals( 6, distanceSq.getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(Vector3DDS.distance(v1, v2).multiply(Vector3DDS.distance(v1, v2)).getValue(), + Vector3DDS.distanceSq(v1, v2).getValue(), 1.0e-12); + } + + @Test + public void testDistanceInf() { + Vector3DDS v1 = createVector(1, -2, 3, 3); + Vector3DDS v2 = createVector(-4, 2, 0, 3); + Assert.assertEquals(0.0, Vector3DDS.distanceInf(createVector(-1, 0, 0, 3), createVector(-1, 0, 0, 3)).getValue(), 0); + DerivativeStructure distance = Vector3DDS.distanceInf(v1, v2); + Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distanceInf(v1, new Vector3D(-4, 2, 0)); + Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); + Assert.assertEquals(1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + distance = Vector3DDS.distanceInf(new Vector3D(-4, 2, 0), v1); + Assert.assertEquals(5.0, distance.getValue(), 1.0e-12); + Assert.assertEquals(1, distance.getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(0, distance.getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(v1.subtract(v2).getNormInf().getValue(), Vector3DDS.distanceInf(v1, v2).getValue(), 1.0e-12); + + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector( 1, -2, 3, 3), + createVector(-4, 2, 0, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector( 1, 3, -2, 3), + createVector(-4, 0, 2, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(-2, 1, 3, 3), + createVector( 2, -4, 0, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(-2, 3, 1, 3), + createVector( 2, 0, -4, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(3, -2, 1, 3), + createVector(0, 2, -4, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(3, 1, -2, 3), + createVector(0, -4, 2, 3)).getValue(), + 1.0e-12); + + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector( 1, -2, 3, 3), + new Vector3D(-4, 2, 0)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector( 1, 3, -2, 3), + new Vector3D(-4, 0, 2)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(-2, 1, 3, 3), + new Vector3D( 2, -4, 0)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(-2, 3, 1, 3), + new Vector3D( 2, 0, -4)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(3, -2, 1, 3), + new Vector3D(0, 2, -4)).getValue(), + 1.0e-12); + Assert.assertEquals(5.0, + Vector3DDS.distanceInf(createVector(3, 1, -2, 3), + new Vector3D(0, -4, 2)).getValue(), + 1.0e-12); + + } + + @Test + public void testSubtract() { + Vector3DDS v1 = createVector(1, 2, 3, 3); + Vector3DDS v2 = createVector(-3, -2, -1, 3); + v1 = v1.subtract(v2); + checkVector(v1, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + checkVector(v2.subtract(v1), -7, -6, -5, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.subtract(new Vector3D(4, 4, 4)), -7, -6, -5, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.subtract(3, v1), -15, -14, -13, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.subtract(3, new Vector3D(4, 4, 4)), -15, -14, -13, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.subtract(new DerivativeStructure(3, 1, 2, 3), new Vector3D(4, 4, 4)), + -15, -14, -13, 1, 0, -4, 0, 1, -4, 0, 0, -3); + + checkVector(createVector(1, 2, 3, 4).subtract(new DerivativeStructure(4, 1, 3, 5.0), + createVector(3, -2, 1, 4)), + -14, 12, -2, + -4, 0, 0, -3, + 0, -4, 0, 2, + 0, 0, -4, -1); + + } + + @Test + public void testAdd() { + Vector3DDS v1 = createVector(1, 2, 3, 3); + Vector3DDS v2 = createVector(-3, -2, -1, 3); + v1 = v1.add(v2); + checkVector(v1, -2, 0, 2, 2, 0, 0, 0, 2, 0, 0, 0, 2); + + checkVector(v2.add(v1), -5, -2, 1, 3, 0, 0, 0, 3, 0, 0, 0, 3); + checkVector(v2.add(new Vector3D(-2, 0, 2)), -5, -2, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.add(3, v1), -9, -2, 5, 7, 0, 0, 0, 7, 0, 0, 0, 7); + checkVector(v2.add(3, new Vector3D(-2, 0, 2)), -9, -2, 5, 1, 0, 0, 0, 1, 0, 0, 0, 1); + checkVector(v2.add(new DerivativeStructure(3, 1, 2, 3), new Vector3D(-2, 0, 2)), + -9, -2, 5, 1, 0, -2, 0, 1, 0, 0, 0, 3); + + checkVector(createVector(1, 2, 3, 4).add(new DerivativeStructure(4, 1, 3, 5.0), + createVector(3, -2, 1, 4)), + 16, -8, 8, + 6, 0, 0, 3, + 0, 6, 0, -2, + 0, 0, 6, 1); + + } + + @Test + public void testScalarProduct() { + Vector3DDS v = createVector(1, 2, 3, 3); + v = v.scalarMultiply(3); + checkVector(v, 3, 6, 9); + + checkVector(v.scalarMultiply(0.5), 1.5, 3, 4.5); + } + + @Test + public void testVectorialProducts() { + Vector3DDS v1 = createVector(2, 1, -4, 3); + Vector3DDS v2 = createVector(3, 1, -1, 3); + + Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v1, v2).getValue() - 11) < 1.0e-12); + + Vector3DDS v3 = Vector3DDS.crossProduct(v1, v2); + checkVector(v3, 3, -10, -1); + + Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v1, v3).getValue()) < 1.0e-12); + Assert.assertTrue(FastMath.abs(Vector3DDS.dotProduct(v2, v3).getValue()) < 1.0e-12); + } + + @Test + public void testCrossProductCancellation() { + Vector3DDS v1 = createVector(9070467121.0, 4535233560.0, 1, 3); + Vector3DDS v2 = createVector(9070467123.0, 4535233561.0, 1, 3); + checkVector(Vector3DDS.crossProduct(v1, v2), -1, 2, 1); + + double scale = FastMath.scalb(1.0, 100); + Vector3DDS big1 = new Vector3DDS(scale, v1); + Vector3DDS small2 = new Vector3DDS(1 / scale, v2); + checkVector(Vector3DDS.crossProduct(big1, small2), -1, 2, 1); + + } + + @Test + public void testAngular() { + Assert.assertEquals(0, createVector(1, 0, 0, 3).getAlpha().getValue(), 1.0e-10); + Assert.assertEquals(0, createVector(1, 0, 0, 3).getDelta().getValue(), 1.0e-10); + Assert.assertEquals(FastMath.PI / 2, createVector(0, 1, 0, 3).getAlpha().getValue(), 1.0e-10); + Assert.assertEquals(0, createVector(0, 1, 0, 3).getDelta().getValue(), 1.0e-10); + Assert.assertEquals(FastMath.PI / 2, createVector(0, 0, 1, 3).getDelta().getValue(), 1.0e-10); + + Vector3DDS u = createVector(-1, 1, -1, 3); + Assert.assertEquals(3 * FastMath.PI /4, u.getAlpha().getValue(), 1.0e-10); + Assert.assertEquals(-1.0 / FastMath.sqrt(3), u.getDelta().sin().getValue(), 1.0e-10); + } + + @Test + public void testAngularSeparation() throws MathArithmeticException { + Vector3DDS v1 = createVector(2, -1, 4, 3); + + Vector3DDS k = v1.normalize(); + Vector3DDS i = k.orthogonal(); + Vector3DDS v2 = k.scalarMultiply(FastMath.cos(1.2)).add(i.scalarMultiply(FastMath.sin(1.2))); + + Assert.assertTrue(FastMath.abs(Vector3DDS.angle(v1, v2).getValue() - 1.2) < 1.0e-12); + } + + @Test + public void testNormalize() throws MathArithmeticException { + Assert.assertEquals(1.0, createVector(5, -4, 2, 3).normalize().getNorm().getValue(), 1.0e-12); + try { + createVector(0, 0, 0, 3).normalize(); + Assert.fail("an exception should have been thrown"); + } catch (MathArithmeticException ae) { + // expected behavior + } + } + + @Test + public void testNegate() { + checkVector(createVector(0.1, 2.5, 1.3, 3).negate(), + -0.1, -2.5, -1.3, -1, 0, 0, 0, -1, 0, 0, 0, -1); + } + + @Test + public void testOrthogonal() throws MathArithmeticException { + Vector3DDS v1 = createVector(0.1, 2.5, 1.3, 3); + Assert.assertEquals(0.0, Vector3DDS.dotProduct(v1, v1.orthogonal()).getValue(), 1.0e-12); + Vector3DDS v2 = createVector(2.3, -0.003, 7.6, 3); + Assert.assertEquals(0.0, Vector3DDS.dotProduct(v2, v2.orthogonal()).getValue(), 1.0e-12); + Vector3DDS v3 = createVector(-1.7, 1.4, 0.2, 3); + Assert.assertEquals(0.0, Vector3DDS.dotProduct(v3, v3.orthogonal()).getValue(), 1.0e-12); + Vector3DDS v4 = createVector(4.2, 0.1, -1.8, 3); + Assert.assertEquals(0.0, Vector3DDS.dotProduct(v4, v4.orthogonal()).getValue(), 1.0e-12); + try { + createVector(0, 0, 0, 3).orthogonal(); + Assert.fail("an exception should have been thrown"); + } catch (MathArithmeticException ae) { + // expected behavior + } + } + + @Test + public void testAngle() throws MathArithmeticException { + Assert.assertEquals(0.22572612855273393616, + Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(4, 5, 6, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(7.98595620686106654517199e-8, + Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(2, 4, 6.000001, 3)).getValue(), + 1.0e-12); + Assert.assertEquals(3.14159257373023116985197793156, + Vector3DDS.angle(createVector(1, 2, 3, 3), createVector(-2, -4, -6.000001, 3)).getValue(), + 1.0e-12); + try { + Vector3DDS.angle(createVector(0, 0, 0, 3), createVector(1, 0, 0, 3)); + Assert.fail("an exception should have been thrown"); + } catch (MathArithmeticException ae) { + // expected behavior + } + } + + @Test + public void testAccurateDotProduct() { + // the following two vectors are nearly but not exactly orthogonal + // naive dot product (i.e. computing u1.x * u2.x + u1.y * u2.y + u1.z * u2.z + // leads to a result of 0.0, instead of the correct -1.855129... + Vector3DDS u1 = createVector(-1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -7645843051051357.0 / 8589934592.0, 3); + Vector3DDS u2 = createVector(-5712344449280879.0 / 2097152.0, + -4550117129121957.0 / 2097152.0, + 8846951984510141.0 / 131072.0, 3); + DerivativeStructure sNaive = u1.getX().multiply(u2.getX()).add(u1.getY().multiply(u2.getY())).add(u1.getZ().multiply(u2.getZ())); + DerivativeStructure sAccurate = u1.dotProduct(u2); + Assert.assertEquals(0.0, sNaive.getValue(), 1.0e-30); + Assert.assertEquals(-2088690039198397.0 / 1125899906842624.0, sAccurate.getValue(), 1.0e-16); + } + + @Test + public void testDotProduct() { + // we compare accurate versus naive dot product implementations + // on regular vectors (i.e. not extreme cases like in the previous test) + Well1024a random = new Well1024a(553267312521321234l); + for (int i = 0; i < 10000; ++i) { + double ux = 10000 * random.nextDouble(); + double uy = 10000 * random.nextDouble(); + double uz = 10000 * random.nextDouble(); + double vx = 10000 * random.nextDouble(); + double vy = 10000 * random.nextDouble(); + double vz = 10000 * random.nextDouble(); + double sNaive = ux * vx + uy * vy + uz * vz; + + Vector3DDS uds = createVector(ux, uy, uz, 3); + Vector3DDS vds = createVector(vx, vy, vz, 3); + Vector3D v = new Vector3D(vx, vy, vz); + + DerivativeStructure sAccurate = Vector3DDS.dotProduct(uds, vds); + Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); + Assert.assertEquals(ux + vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); + Assert.assertEquals(uy + vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); + Assert.assertEquals(uz + vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); + + sAccurate = Vector3DDS.dotProduct(uds, v); + Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); + Assert.assertEquals(vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); + Assert.assertEquals(vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); + Assert.assertEquals(vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); + + sAccurate = Vector3DDS.dotProduct(v, uds); + Assert.assertEquals(sNaive, sAccurate.getValue(), 2.5e-16 * sNaive); + Assert.assertEquals(vx, sAccurate.getPartialDerivative(1, 0, 0), 2.5e-16 * sNaive); + Assert.assertEquals(vy, sAccurate.getPartialDerivative(0, 1, 0), 2.5e-16 * sNaive); + Assert.assertEquals(vz, sAccurate.getPartialDerivative(0, 0, 1), 2.5e-16 * sNaive); + + } + } + + @Test + public void testAccurateCrossProduct() { + // the vectors u1 and u2 are nearly but not exactly anti-parallel + // (7.31e-16 degrees from 180 degrees) naive cross product (i.e. + // computing u1.x * u2.x + u1.y * u2.y + u1.z * u2.z + // leads to a result of [0.0009765, -0.0001220, -0.0039062], + // instead of the correct [0.0006913, -0.0001254, -0.0007909] + final Vector3DDS u1 = createVector(-1321008684645961.0 / 268435456.0, + -5774608829631843.0 / 268435456.0, + -7645843051051357.0 / 8589934592.0, 3); + final Vector3DDS u2 = createVector( 1796571811118507.0 / 2147483648.0, + 7853468008299307.0 / 2147483648.0, + 2599586637357461.0 / 17179869184.0, 3); + final Vector3DDS u3 = createVector(12753243807587107.0 / 18446744073709551616.0, + -2313766922703915.0 / 18446744073709551616.0, + -227970081415313.0 / 288230376151711744.0, 3); + Vector3DDS cNaive = new Vector3DDS(u1.getY().multiply(u2.getZ()).subtract(u1.getZ().multiply(u2.getY())), + u1.getZ().multiply(u2.getX()).subtract(u1.getX().multiply(u2.getZ())), + u1.getX().multiply(u2.getY()).subtract(u1.getY().multiply(u2.getX()))); + Vector3DDS cAccurate = u1.crossProduct(u2); + Assert.assertTrue(u3.distance(cNaive).getValue() > 2.9 * u3.getNorm().getValue()); + Assert.assertEquals(0.0, u3.distance(cAccurate).getValue(), 1.0e-30 * cAccurate.getNorm().getValue()); + } + + @Test + public void testCrossProduct() { + // we compare accurate versus naive cross product implementations + // on regular vectors (i.e. not extreme cases like in the previous test) + Well1024a random = new Well1024a(885362227452043214l); + for (int i = 0; i < 10000; ++i) { + double ux = random.nextDouble(); + double uy = random.nextDouble(); + double uz = random.nextDouble(); + double vx = random.nextDouble(); + double vy = random.nextDouble(); + double vz = random.nextDouble(); + Vector3D cNaive = new Vector3D(uy * vz - uz * vy, uz * vx - ux * vz, ux * vy - uy * vx); + + Vector3DDS uds = createVector(ux, uy, uz, 3); + Vector3DDS vds = createVector(vx, vy, vz, 3); + Vector3D v = new Vector3D(vx, vy, vz); + + checkVector(Vector3DDS.crossProduct(uds, vds), + cNaive.getX(), cNaive.getY(), cNaive.getZ(), + 0, vz - uz, uy - vy, + uz - vz, 0, vx - ux, + vy - uy, ux - vx, 0); + + checkVector(Vector3DDS.crossProduct(uds, v), + cNaive.getX(), cNaive.getY(), cNaive.getZ(), + 0, vz, -vy, + -vz, 0, vx, + vy, -vx, 0); + + checkVector(Vector3DDS.crossProduct(v, uds), + -cNaive.getX(), -cNaive.getY(), -cNaive.getZ(), + 0, -vz, vy, + vz, 0, -vx, + -vy, vx, 0); + + } + } + + private Vector3DDS createVector(double x, double y, double z, int params) { + return new Vector3DDS(new DerivativeStructure(params, 1, 0, x), + new DerivativeStructure(params, 1, 1, y), + new DerivativeStructure(params, 1, 2, z)); + } + + private void checkVector(Vector3DDS v, double x, double y, double z) { + Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); + Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + } + + private void checkVector(Vector3DDS v, double x, double y, double z, + double dxdx, double dxdy, double dxdz, + double dydx, double dydy, double dydz, + double dzdx, double dzdy, double dzdz) { + Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); + Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + Assert.assertEquals(dxdx, v.getX().getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(dxdy, v.getX().getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(dxdz, v.getX().getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(dydx, v.getY().getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(dydy, v.getY().getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(dydz, v.getY().getPartialDerivative(0, 0, 1), 1.0e-12); + Assert.assertEquals(dzdx, v.getZ().getPartialDerivative(1, 0, 0), 1.0e-12); + Assert.assertEquals(dzdy, v.getZ().getPartialDerivative(0, 1, 0), 1.0e-12); + Assert.assertEquals(dzdz, v.getZ().getPartialDerivative(0, 0, 1), 1.0e-12); + } + + private void checkVector(Vector3DDS v, double x, double y, double z, + double dxdx, double dxdy, double dxdz, double dxdt, + double dydx, double dydy, double dydz, double dydt, + double dzdx, double dzdy, double dzdz, double dzdt) { + Assert.assertEquals(x, v.getX().getValue(), 1.0e-12); + Assert.assertEquals(y, v.getY().getValue(), 1.0e-12); + Assert.assertEquals(z, v.getZ().getValue(), 1.0e-12); + Assert.assertEquals(dxdx, v.getX().getPartialDerivative(1, 0, 0, 0), 1.0e-12); + Assert.assertEquals(dxdy, v.getX().getPartialDerivative(0, 1, 0, 0), 1.0e-12); + Assert.assertEquals(dxdz, v.getX().getPartialDerivative(0, 0, 1, 0), 1.0e-12); + Assert.assertEquals(dxdt, v.getX().getPartialDerivative(0, 0, 0, 1), 1.0e-12); + Assert.assertEquals(dydx, v.getY().getPartialDerivative(1, 0, 0, 0), 1.0e-12); + Assert.assertEquals(dydy, v.getY().getPartialDerivative(0, 1, 0, 0), 1.0e-12); + Assert.assertEquals(dydz, v.getY().getPartialDerivative(0, 0, 1, 0), 1.0e-12); + Assert.assertEquals(dydt, v.getY().getPartialDerivative(0, 0, 0, 1), 1.0e-12); + Assert.assertEquals(dzdx, v.getZ().getPartialDerivative(1, 0, 0, 0), 1.0e-12); + Assert.assertEquals(dzdy, v.getZ().getPartialDerivative(0, 1, 0, 0), 1.0e-12); + Assert.assertEquals(dzdz, v.getZ().getPartialDerivative(0, 0, 1, 0), 1.0e-12); + Assert.assertEquals(dzdt, v.getZ().getPartialDerivative(0, 0, 0, 1), 1.0e-12); + } + +}