diff options
Diffstat (limited to 'src/main/java/org/apache/commons/math3/geometry/euclidean/threed')
20 files changed, 8147 insertions, 0 deletions
diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/CardanEulerSingularityException.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/CardanEulerSingularityException.java new file mode 100644 index 0000000..728074d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/CardanEulerSingularityException.java @@ -0,0 +1,44 @@ +/* + * 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.exception.MathIllegalStateException; +import org.apache.commons.math3.exception.util.LocalizedFormats; + +/** This class represents exceptions thrown while extractiong Cardan + * or Euler angles from a rotation. + + * @since 1.2 + */ +public class CardanEulerSingularityException + extends MathIllegalStateException { + + /** Serializable version identifier */ + private static final long serialVersionUID = -1360952845582206770L; + + /** + * Simple constructor. + * build an exception with a default message. + * @param isCardan if true, the rotation is related to Cardan angles, + * if false it is related to EulerAngles + */ + public CardanEulerSingularityException(boolean isCardan) { + super(isCardan ? LocalizedFormats.CARDAN_ANGLES_SINGULARITY : LocalizedFormats.EULER_ANGLES_SINGULARITY); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Euclidean3D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Euclidean3D.java new file mode 100644 index 0000000..dc06936 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Euclidean3D.java @@ -0,0 +1,74 @@ +/* + * 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.geometry.Space; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; + +/** + * This class implements a three-dimensional space. + * @since 3.0 + */ +public class Euclidean3D implements Serializable, Space { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 6249091865814886817L; + + /** Private constructor for the singleton. + */ + private Euclidean3D() { + } + + /** Get the unique instance. + * @return the unique instance + */ + public static Euclidean3D getInstance() { + return LazyHolder.INSTANCE; + } + + /** {@inheritDoc} */ + public int getDimension() { + return 3; + } + + /** {@inheritDoc} */ + public Euclidean2D getSubSpace() { + return Euclidean2D.getInstance(); + } + + // CHECKSTYLE: stop HideUtilityClassConstructor + /** Holder for the instance. + * <p>We use here the Initialization On Demand Holder Idiom.</p> + */ + private static class LazyHolder { + /** Cached field instance. */ + private static final Euclidean3D INSTANCE = new Euclidean3D(); + } + // CHECKSTYLE: resume HideUtilityClassConstructor + + /** Handle deserialization of the singleton. + * @return the singleton instance + */ + private Object readResolve() { + // return the singleton instance + return LazyHolder.INSTANCE; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java new file mode 100644 index 0000000..4e2278b --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java @@ -0,0 +1,1663 @@ +/* + * 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.RealFieldElement; +import org.apache.commons.math3.Field; +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 RealFieldElement}. + * <p>Instance of this class are guaranteed to be immutable.</p> + * + * @param <T> the type of the field elements + * @see FieldVector3D + * @see RotationOrder + * @since 3.2 + */ + +public class FieldRotation<T extends RealFieldElement<T>> implements Serializable { + + /** Serializable version identifier */ + private static final long serialVersionUID = 20130224l; + + /** Scalar coordinate of the quaternion. */ + private final T q0; + + /** First coordinate of the vectorial part of the quaternion. */ + private final T q1; + + /** Second coordinate of the vectorial part of the quaternion. */ + private final T q2; + + /** Third coordinate of the vectorial part of the quaternion. */ + private final T q3; + + /** Build a rotation from the quaternion coordinates. + * <p>A rotation can be built from a <em>normalized</em> quaternion, + * i.e. a quaternion for which q<sub>0</sub><sup>2</sup> + + * q<sub>1</sub><sup>2</sup> + q<sub>2</sub><sup>2</sup> + + * q<sub>3</sub><sup>2</sup> = 1. If the quaternion is not normalized, + * the constructor can normalize it in a preprocessing step.</p> + * <p>Note that some conventions put the scalar part of the quaternion + * as the 4<sup>th</sup> component and the vector part as the first three + * components. This is <em>not</em> our convention. We put the scalar part + * as the first component.</p> + * @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 FieldRotation(final T q0, final T q1, final T q2, final T q3, final boolean needsNormalization) { + + if (needsNormalization) { + // normalization preprocessing + final T inv = + q0.multiply(q0).add(q1.multiply(q1)).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().reciprocal(); + this.q0 = inv.multiply(q0); + this.q1 = inv.multiply(q1); + 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. + * <p>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(FieldVector3D) apply} the instance to +i, we will get + * +j.</p> + * <p>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!</p> + * <p>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.</p> + * @param axis axis around which to rotate + * @param angle rotation angle. + * @exception MathIllegalArgumentException if the axis norm is zero + * @deprecated as of 3.6, replaced with {@link + * #FieldRotation(FieldVector3D, RealFieldElement, RotationConvention)} + */ + @Deprecated + public FieldRotation(final FieldVector3D<T> axis, final T angle) + throws MathIllegalArgumentException { + this(axis, angle, RotationConvention.VECTOR_OPERATOR); + } + + /** Build a rotation from an axis and an angle. + * <p>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(FieldVector3D) apply} the instance to +i, we will get + * +j.</p> + * <p>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!</p> + * <p>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.</p> + * @param axis axis around which to rotate + * @param angle rotation angle. + * @param convention convention to use for the semantics of the angle + * @exception MathIllegalArgumentException if the axis norm is zero + * @since 3.6 + */ + public FieldRotation(final FieldVector3D<T> axis, final T angle, final RotationConvention convention) + throws MathIllegalArgumentException { + + final T norm = axis.getNorm(); + if (norm.getReal() == 0) { + throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); + } + + final T halfAngle = angle.multiply(convention == RotationConvention.VECTOR_OPERATOR ? -0.5 : 0.5); + final T 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. + + * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices + * (which are matrices for which m.m<sup>T</sup> = 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.</p> + + * <p>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.<p> + + * @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 FieldRotation(final T[][] 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 T[][] ort = orthogonalizeMatrix(m, threshold); + + // check the sign of the determinant + final T d0 = ort[1][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[1][2])); + final T d1 = ort[0][1].multiply(ort[2][2]).subtract(ort[2][1].multiply(ort[0][2])); + final T d2 = ort[0][1].multiply(ort[1][2]).subtract(ort[1][1].multiply(ort[0][2])); + final T det = + ort[0][0].multiply(d0).subtract(ort[1][0].multiply(d1)).add(ort[2][0].multiply(d2)); + if (det.getReal() < 0.0) { + throw new NotARotationMatrixException( + LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, + det); + } + + final T[] quat = mat2quat(ort); + q0 = quat[0]; + q1 = quat[1]; + q2 = quat[2]; + q3 = quat[3]; + + } + + /** Build the rotation that transforms a pair of vectors into another pair. + + * <p>Except for possible scale factors, if the instance were applied to + * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair + * (v<sub>1</sub>, v<sub>2</sub>).</p> + + * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is + * not the same as the angular separation between v<sub>1</sub> and + * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than + * v<sub>2</sub>, the corrected vector will be in the (±v<sub>1</sub>, + * +v<sub>2</sub>) half-plane.</p> + + * @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 collinear) + */ + public FieldRotation(FieldVector3D<T> u1, FieldVector3D<T> u2, FieldVector3D<T> v1, FieldVector3D<T> v2) + throws MathArithmeticException { + + // build orthonormalized base from u1, u2 + // this fails when vectors are null or collinear, which is forbidden to define a rotation + final FieldVector3D<T> u3 = FieldVector3D.crossProduct(u1, u2).normalize(); + u2 = FieldVector3D.crossProduct(u3, u1).normalize(); + u1 = u1.normalize(); + + // build an orthonormalized base from v1, v2 + // this fails when vectors are null or collinear, which is forbidden to define a rotation + final FieldVector3D<T> v3 = FieldVector3D.crossProduct(v1, v2).normalize(); + v2 = FieldVector3D.crossProduct(v3, v1).normalize(); + v1 = v1.normalize(); + + // buid a matrix transforming the first base into the second one + final T[][] array = MathArrays.buildArray(u1.getX().getField(), 3, 3); + array[0][0] = u1.getX().multiply(v1.getX()).add(u2.getX().multiply(v2.getX())).add(u3.getX().multiply(v3.getX())); + array[0][1] = u1.getY().multiply(v1.getX()).add(u2.getY().multiply(v2.getX())).add(u3.getY().multiply(v3.getX())); + array[0][2] = u1.getZ().multiply(v1.getX()).add(u2.getZ().multiply(v2.getX())).add(u3.getZ().multiply(v3.getX())); + array[1][0] = u1.getX().multiply(v1.getY()).add(u2.getX().multiply(v2.getY())).add(u3.getX().multiply(v3.getY())); + array[1][1] = u1.getY().multiply(v1.getY()).add(u2.getY().multiply(v2.getY())).add(u3.getY().multiply(v3.getY())); + array[1][2] = u1.getZ().multiply(v1.getY()).add(u2.getZ().multiply(v2.getY())).add(u3.getZ().multiply(v3.getY())); + array[2][0] = u1.getX().multiply(v1.getZ()).add(u2.getX().multiply(v2.getZ())).add(u3.getX().multiply(v3.getZ())); + array[2][1] = u1.getY().multiply(v1.getZ()).add(u2.getY().multiply(v2.getZ())).add(u3.getY().multiply(v3.getZ())); + array[2][2] = u1.getZ().multiply(v1.getZ()).add(u2.getZ().multiply(v2.getZ())).add(u3.getZ().multiply(v3.getZ())); + + T[] quat = mat2quat(array); + q0 = quat[0]; + q1 = quat[1]; + q2 = quat[2]; + q3 = quat[3]; + + } + + /** Build one of the rotations that transform one vector into another one. + + * <p>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 collinear, an + * arbitrary rotation axis is chosen.</p> + + * @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 FieldRotation(final FieldVector3D<T> u, final FieldVector3D<T> v) throws MathArithmeticException { + + final T normProduct = u.getNorm().multiply(v.getNorm()); + if (normProduct.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); + } + + final T dot = FieldVector3D.dotProduct(u, v); + + if (dot.getReal() < ((2.0e-15 - 1.0) * normProduct.getReal())) { + // special case u = -v: we select a PI angle rotation around + // an arbitrary vector orthogonal to u + final FieldVector3D<T> 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 T coeff = q0.multiply(normProduct).multiply(2.0).reciprocal(); + final FieldVector3D<T> q = FieldVector3D.crossProduct(v, 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. + + * <p>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.</p> + * <p>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).</p> + + * @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 + * @deprecated as of 3.6, replaced with {@link + * #FieldRotation(RotationOrder, RotationConvention, + * RealFieldElement, RealFieldElement, RealFieldElement)} + */ + @Deprecated + public FieldRotation(final RotationOrder order, final T alpha1, final T alpha2, final T alpha3) { + this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3); + } + + /** Build a rotation from three Cardan or Euler elementary rotations. + + * <p>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.</p> + * <p>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).</p> + + * @param order order of rotations to compose, from left to right + * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)}) + * @param convention convention to use for the semantics of the angle + * @param alpha1 angle of the first elementary rotation + * @param alpha2 angle of the second elementary rotation + * @param alpha3 angle of the third elementary rotation + * @since 3.6 + */ + public FieldRotation(final RotationOrder order, final RotationConvention convention, + final T alpha1, final T alpha2, final T alpha3) { + final T one = alpha1.getField().getOne(); + final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1, convention); + final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2, convention); + final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3, convention); + final FieldRotation<T> composed = r1.compose(r2.compose(r3, convention), convention); + 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 T[] mat2quat(final T[][] ort) { + + final T[] quat = MathArrays.buildArray(ort[0][0].getField(), 4); + + // There are different ways to compute the quaternions elements + // from the matrix. They all involve computing one element from + // 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) + T s = ort[0][0].add(ort[1][1]).add(ort[2][2]); + if (s.getReal() > -0.19) { + // compute q0 and deduce q1, q2 and q3 + quat[0] = s.add(1.0).sqrt().multiply(0.5); + T inv = quat[0].reciprocal().multiply(0.25); + quat[1] = inv.multiply(ort[1][2].subtract(ort[2][1])); + quat[2] = inv.multiply(ort[2][0].subtract(ort[0][2])); + quat[3] = inv.multiply(ort[0][1].subtract(ort[1][0])); + } else { + s = ort[0][0].subtract(ort[1][1]).subtract(ort[2][2]); + if (s.getReal() > -0.19) { + // compute q1 and deduce q0, q2 and q3 + quat[1] = s.add(1.0).sqrt().multiply(0.5); + T inv = quat[1].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[1][2].subtract(ort[2][1])); + quat[2] = inv.multiply(ort[0][1].add(ort[1][0])); + quat[3] = inv.multiply(ort[0][2].add(ort[2][0])); + } else { + s = ort[1][1].subtract(ort[0][0]).subtract(ort[2][2]); + if (s.getReal() > -0.19) { + // compute q2 and deduce q0, q1 and q3 + quat[2] = s.add(1.0).sqrt().multiply(0.5); + T inv = quat[2].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[2][0].subtract(ort[0][2])); + quat[1] = inv.multiply(ort[0][1].add(ort[1][0])); + quat[3] = inv.multiply(ort[2][1].add(ort[1][2])); + } 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); + T inv = quat[3].reciprocal().multiply(0.25); + quat[0] = inv.multiply(ort[0][1].subtract(ort[1][0])); + quat[1] = inv.multiply(ort[0][2].add(ort[2][0])); + quat[2] = inv.multiply(ort[2][1].add(ort[1][2])); + } + } + } + + 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 FieldRotation<T> revert() { + return new FieldRotation<T>(q0.negate(), q1, q2, q3, false); + } + + /** Get the scalar coordinate of the quaternion. + * @return scalar coordinate of the quaternion + */ + public T getQ0() { + return q0; + } + + /** Get the first coordinate of the vectorial part of the quaternion. + * @return first coordinate of the vectorial part of the quaternion + */ + public T getQ1() { + return q1; + } + + /** Get the second coordinate of the vectorial part of the quaternion. + * @return second coordinate of the vectorial part of the quaternion + */ + public T getQ2() { + return q2; + } + + /** Get the third coordinate of the vectorial part of the quaternion. + * @return third coordinate of the vectorial part of the quaternion + */ + public T getQ3() { + return q3; + } + + /** Get the normalized axis of the rotation. + * @return normalized axis of the rotation + * @see #FieldRotation(FieldVector3D, RealFieldElement) + * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)} + */ + @Deprecated + public FieldVector3D<T> getAxis() { + return getAxis(RotationConvention.VECTOR_OPERATOR); + } + + /** Get the normalized axis of the rotation. + * <p> + * Note that as {@link #getAngle()} always returns an angle + * between 0 and π, changing the convention changes the + * direction of the axis, not the sign of the angle. + * </p> + * @param convention convention to use for the semantics of the angle + * @return normalized axis of the rotation + * @see #FieldRotation(FieldVector3D, RealFieldElement) + * @since 3.6 + */ + public FieldVector3D<T> getAxis(final RotationConvention convention) { + final T squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)); + if (squaredSine.getReal() == 0) { + final Field<T> field = squaredSine.getField(); + return new FieldVector3D<T>(convention == RotationConvention.VECTOR_OPERATOR ? field.getOne(): field.getOne().negate(), + field.getZero(), + field.getZero()); + } else { + final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1; + if (q0.getReal() < 0) { + T inverse = squaredSine.sqrt().reciprocal().multiply(sgn); + return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + } + final T inverse = squaredSine.sqrt().reciprocal().negate().multiply(sgn); + return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + } + } + + /** Get the angle of the rotation. + * @return angle of the rotation (between 0 and π) + * @see #FieldRotation(FieldVector3D, RealFieldElement) + */ + public T getAngle() { + if ((q0.getReal() < -0.1) || (q0.getReal() > 0.1)) { + return q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)).sqrt().asin().multiply(2); + } else if (q0.getReal() < 0) { + return q0.negate().acos().multiply(2); + } + return q0.acos().multiply(2); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + * <p>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 + * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as + * the rotation defined by the angles π + a<sub>1</sub>, π + * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements + * the following arbitrary choices:</p> + * <ul> + * <li>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),</li> + * <li>for Euler angles, the chosen set is the one for which the + * second angle is between 0 and π (i.e its sine is positive).</li> + * </ul> + + * <p>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 + * <em>nothing</em> 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!</p> + + * @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 + * @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)} + */ + @Deprecated + public T[] getAngles(final RotationOrder order) + throws CardanEulerSingularityException { + return getAngles(order, RotationConvention.VECTOR_OPERATOR); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + * <p>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 + * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as + * the rotation defined by the angles π + a<sub>1</sub>, π + * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements + * the following arbitrary choices:</p> + * <ul> + * <li>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),</li> + * <li>for Euler angles, the chosen set is the one for which the + * second angle is between 0 and π (i.e its sine is positive).</li> + * </ul> + + * <p>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 + * <em>nothing</em> 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!</p> + + * @param order rotation order to use + * @param convention convention to use for the semantics of the angle + * @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 + * @since 3.6 + */ + public T[] getAngles(final RotationOrder order, RotationConvention convention) + throws CardanEulerSingularityException { + + if (convention == RotationConvention.VECTOR_OPERATOR) { + 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] + FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getY().negate().atan2(v1.getZ()), + v2.getZ().asin(), + v2.getY().negate().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getY().asin().negate(), + v2.getZ().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getZ().asin().negate(), + v2.getX().atan2(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 FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getZ().negate().atan2(v1.getX()), + v2.getX().asin(), + v2.getZ().negate().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getX().negate().atan2(v1.getY()), + v2.getY().asin(), + v2.getX().negate().atan2(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 FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getY().atan2(v1.getX()), + v2.getX().asin().negate(), + v2.getY().atan2(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 FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getY().atan2(v1.getZ().negate()), + v2.getX().acos(), + v2.getY().atan2(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 FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getX().acos(), + v2.getZ().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getY().acos(), + v2.getX().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getZ().atan2(v1.getX().negate()), + v2.getY().acos(), + v2.getZ().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getX().atan2(v1.getY().negate()), + v2.getZ().acos(), + v2.getX().atan2(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 FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getY().atan2(v1.getX()), + v2.getZ().acos(), + v2.getY().atan2(v2.getX().negate())); + + } + } else { + if (order == RotationOrder.XYZ) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta) + // (-r) (Vector3D.plusK) 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getY().negate().atan2(v2.getZ()), + v2.getX().asin(), + v1.getY().negate().atan2(v1.getX())); + + } else if (order == RotationOrder.XZY) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta) + // (-r) (Vector3D.plusJ) 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getZ().atan2(v2.getY()), + v2.getX().asin().negate(), + v1.getZ().atan2(v1.getX())); + + } else if (order == RotationOrder.YXZ) { + + // r (Vector3D.plusJ) coordinates are : + // cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi) + // (-r) (Vector3D.plusK) 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getX().atan2(v2.getZ()), + v2.getY().asin().negate(), + v1.getX().atan2(v1.getY())); + + } else if (order == RotationOrder.YZX) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi) + // (-r) (Vector3D.plusI) 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getZ().negate().atan2(v2.getX()), + v2.getY().asin(), + v1.getZ().negate().atan2(v1.getY())); + + } else if (order == RotationOrder.ZXY) { + + // r (Vector3D.plusK) coordinates are : + // -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta) + // (-r) (Vector3D.plusJ) 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getX().negate().atan2(v2.getY()), + v2.getZ().asin(), + v1.getX().negate().atan2(v1.getZ())); + + } else if (order == RotationOrder.ZYX) { + + // r (Vector3D.plusK) coordinates are : + // -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getY().atan2(v2.getX()), + v2.getZ().asin().negate(), + v1.getY().atan2(v1.getZ())); + + } else if (order == RotationOrder.XYX) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1) + // and we can choose to have theta in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getY().atan2(v2.getZ().negate()), + v2.getX().acos(), + v1.getY().atan2(v1.getZ())); + + } else if (order == RotationOrder.XZX) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1) + // and we can choose to have psi in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getZ().atan2(v2.getY()), + v2.getX().acos(), + v1.getZ().atan2(v1.getY().negate())); + + } else if (order == RotationOrder.YXY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // and we can choose to have phi in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getX().atan2(v2.getZ()), + v2.getY().acos(), + v1.getX().atan2(v1.getZ().negate())); + + } else if (order == RotationOrder.YZY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // and we can choose to have psi in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getZ().atan2(v2.getX().negate()), + v2.getY().acos(), + v1.getZ().atan2(v1.getX())); + + } else if (order == RotationOrder.ZXZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getX().atan2(v2.getY().negate()), + v2.getZ().acos(), + v1.getX().atan2(v1.getY())); + + } else { // last possibility is ZYZ + + // r (Vector3D.plusK) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // (-r) (Vector3D.plusK) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getY().atan2(v2.getX()), + v2.getZ().acos(), + v1.getY().atan2(v1.getX().negate())); + + } + } + + } + + /** Create a dimension 3 array. + * @param a0 first array element + * @param a1 second array element + * @param a2 third array element + * @return new array + */ + private T[] buildArray(final T a0, final T a1, final T a2) { + final T[] array = MathArrays.buildArray(a0.getField(), 3); + array[0] = a0; + array[1] = a1; + array[2] = a2; + return array; + } + + /** Create a constant vector. + * @param x abscissa + * @param y ordinate + * @param z height + * @return a constant vector + */ + private FieldVector3D<T> vector(final double x, final double y, final double z) { + final T zero = q0.getField().getZero(); + return new FieldVector3D<T>(zero.add(x), zero.add(y), zero.add(z)); + } + + /** Get the 3X3 matrix corresponding to the instance + * @return the matrix corresponding to the instance + */ + public T[][] getMatrix() { + + // products + final T q0q0 = q0.multiply(q0); + final T q0q1 = q0.multiply(q1); + final T q0q2 = q0.multiply(q2); + final T q0q3 = q0.multiply(q3); + final T q1q1 = q1.multiply(q1); + final T q1q2 = q1.multiply(q2); + final T q1q3 = q1.multiply(q3); + final T q2q2 = q2.multiply(q2); + final T q2q3 = q2.multiply(q3); + final T q3q3 = q3.multiply(q3); + + // create the matrix + final T[][] m = MathArrays.buildArray(q0.getField(), 3, 3); + + m [0][0] = q0q0.add(q1q1).multiply(2).subtract(1); + m [1][0] = q1q2.subtract(q0q3).multiply(2); + 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; + + } + + /** Convert to a constant vector without derivatives. + * @return a constant vector + */ + public Rotation toRotation() { + return new Rotation(q0.getReal(), q1.getReal(), q2.getReal(), q3.getReal(), false); + } + + /** Apply the rotation to a vector. + * @param u vector to apply the rotation to + * @return a new vector which is the image of u by the rotation + */ + public FieldVector3D<T> applyTo(final FieldVector3D<T> u) { + + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + return new FieldVector3D<T>(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 FieldVector3D<T> applyTo(final Vector3D u) { + + final double x = u.getX(); + final double y = u.getY(); + final double z = u.getZ(); + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + return new FieldVector3D<T>(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 T[] in, final T[] out) { + + final T x = in[0]; + final T y = in[1]; + final T z = in[2]; + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + out[0] = q0.multiply(x.multiply(q0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = q0.multiply(y.multiply(q0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + 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 T[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + + out[0] = q0.multiply(q0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = q0.multiply(q0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + 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 + * @param <T> the type of the field elements + * @return a new vector which is the image of u by the rotation + */ + public static <T extends RealFieldElement<T>> FieldVector3D<T> applyTo(final Rotation r, final FieldVector3D<T> u) { + + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); + + final T s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + + return new FieldVector3D<T>(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 FieldVector3D<T> applyInverseTo(final FieldVector3D<T> u) { + + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); + + return new FieldVector3D<T>(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 FieldVector3D<T> applyInverseTo(final Vector3D u) { + + final double x = u.getX(); + final double y = u.getY(); + final double z = u.getZ(); + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); + + return new FieldVector3D<T>(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 T[] in, final T[] out) { + + final T x = in[0]; + final T y = in[1]; + final T z = in[2]; + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); + + out[0] = m0.multiply(x.multiply(m0).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = m0.multiply(y.multiply(m0).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + 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 T[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final T s = q1.multiply(x).add(q2.multiply(y)).add(q3.multiply(z)); + final T m0 = q0.negate(); + + out[0] = m0.multiply(m0.multiply(x).subtract(q2.multiply(z).subtract(q3.multiply(y)))).add(s.multiply(q1)).multiply(2).subtract(x); + out[1] = m0.multiply(m0.multiply(y).subtract(q3.multiply(x).subtract(q1.multiply(z)))).add(s.multiply(q2)).multiply(2).subtract(y); + 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 + * @param <T> the type of the field elements + * @return a new vector which such that u is its image by the rotation + */ + public static <T extends RealFieldElement<T>> FieldVector3D<T> applyInverseTo(final Rotation r, final FieldVector3D<T> u) { + + final T x = u.getX(); + final T y = u.getY(); + final T z = u.getZ(); + + final T s = x.multiply(r.getQ1()).add(y.multiply(r.getQ2())).add(z.multiply(r.getQ3())); + final double m0 = -r.getQ0(); + + return new FieldVector3D<T>(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. + * <p> + * Calling this method is equivalent to call + * {@link #compose(FieldRotation, RotationConvention) + * compose(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public FieldRotation<T> applyTo(final FieldRotation<T> r) { + return compose(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the instance to a rotation is computing the composition + * in an order compliant with the following rule : let {@code u} be any + * vector and {@code v} its image by {@code r1} (i.e. + * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by + * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then + * {@code w = comp.applyTo(u)}, where + * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the instance + */ + public FieldRotation<T> compose(final FieldRotation<T> r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInternal(r) : r.composeInternal(this); + } + + /** Compose the instance with another rotation using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + * using vector operator convention + */ + private FieldRotation<T> composeInternal(final FieldRotation<T> r) { + return new FieldRotation<T>(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. + * <p> + * Calling this method is equivalent to call + * {@link #compose(Rotation, RotationConvention) + * compose(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public FieldRotation<T> applyTo(final Rotation r) { + return compose(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the instance to a rotation is computing the composition + * in an order compliant with the following rule : let {@code u} be any + * vector and {@code v} its image by {@code r1} (i.e. + * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by + * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then + * {@code w = comp.applyTo(u)}, where + * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the instance + */ + public FieldRotation<T> compose(final Rotation r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInternal(r) : applyTo(r, this); + } + + /** Compose the instance with another rotation using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + * using vector operator convention + */ + private FieldRotation<T> composeInternal(final Rotation r) { + return new FieldRotation<T>(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 + * @param <T> the type of the field elements + * @return a new rotation which is the composition of r by the instance + */ + public static <T extends RealFieldElement<T>> FieldRotation<T> applyTo(final Rotation r1, final FieldRotation<T> rInner) { + return new FieldRotation<T>(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. + * <p> + * Calling this method is equivalent to call + * {@link #composeInverse(FieldRotation, RotationConvention) + * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @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 FieldRotation<T> applyInverseTo(final FieldRotation<T> r) { + return composeInverse(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the inverse of the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the inverse of the instance to a rotation is computing + * the composition in an order compliant with the following rule : + * let {@code u} be any vector and {@code v} its image by {@code r1} + * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image + * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}). + * Then {@code w = comp.applyTo(u)}, where + * {@code comp = r2.composeInverse(r1)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed, which means it is the + * <em>innermost</em> rotation that will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public FieldRotation<T> composeInverse(final FieldRotation<T> r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInverseInternal(r) : r.composeInternal(revert()); + } + + /** Compose the inverse of the instance with another rotation + * using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance using vector operator convention + */ + private FieldRotation<T> composeInverseInternal(FieldRotation<T> r) { + return new FieldRotation<T>(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. + * <p> + * Calling this method is equivalent to call + * {@link #composeInverse(Rotation, RotationConvention) + * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @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 FieldRotation<T> applyInverseTo(final Rotation r) { + return composeInverse(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the inverse of the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the inverse of the instance to a rotation is computing + * the composition in an order compliant with the following rule : + * let {@code u} be any vector and {@code v} its image by {@code r1} + * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image + * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}). + * Then {@code w = comp.applyTo(u)}, where + * {@code comp = r2.composeInverse(r1)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed, which means it is the + * <em>innermost</em> rotation that will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public FieldRotation<T> composeInverse(final Rotation r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInverseInternal(r) : applyTo(r, revert()); + } + + /** Compose the inverse of the instance with another rotation + * using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance using vector operator convention + */ + private FieldRotation<T> composeInverseInternal(Rotation r) { + return new FieldRotation<T>(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 + * @param <T> the type of the field elements + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public static <T extends RealFieldElement<T>> FieldRotation<T> applyInverseTo(final Rotation rOuter, final FieldRotation<T> rInner) { + return new FieldRotation<T>(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 T[][] orthogonalizeMatrix(final T[][] m, final double threshold) + throws NotARotationMatrixException { + + T x00 = m[0][0]; + T x01 = m[0][1]; + T x02 = m[0][2]; + T x10 = m[1][0]; + T x11 = m[1][1]; + T x12 = m[1][2]; + T x20 = m[2][0]; + T x21 = m[2][1]; + T x22 = m[2][2]; + double fn = 0; + double fn1; + + final T[][] o = MathArrays.buildArray(m[0][0].getField(), 3, 3); + + // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) + int i = 0; + while (++i < 11) { + + // Mt.Xn + final T mx00 = m[0][0].multiply(x00).add(m[1][0].multiply(x10)).add(m[2][0].multiply(x20)); + final T mx10 = m[0][1].multiply(x00).add(m[1][1].multiply(x10)).add(m[2][1].multiply(x20)); + final T mx20 = m[0][2].multiply(x00).add(m[1][2].multiply(x10)).add(m[2][2].multiply(x20)); + final T mx01 = m[0][0].multiply(x01).add(m[1][0].multiply(x11)).add(m[2][0].multiply(x21)); + final T mx11 = m[0][1].multiply(x01).add(m[1][1].multiply(x11)).add(m[2][1].multiply(x21)); + final T mx21 = m[0][2].multiply(x01).add(m[1][2].multiply(x11)).add(m[2][2].multiply(x21)); + final T mx02 = m[0][0].multiply(x02).add(m[1][0].multiply(x12)).add(m[2][0].multiply(x22)); + final T mx12 = m[0][1].multiply(x02).add(m[1][1].multiply(x12)).add(m[2][1].multiply(x22)); + final T mx22 = m[0][2].multiply(x02).add(m[1][2].multiply(x12)).add(m[2][2].multiply(x22)); + + // Xn+1 + o[0][0] = x00.subtract(x00.multiply(mx00).add(x01.multiply(mx10)).add(x02.multiply(mx20)).subtract(m[0][0]).multiply(0.5)); + 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].getReal() - m[0][0].getReal(); + final double corr01 = o[0][1].getReal() - m[0][1].getReal(); + final double corr02 = o[0][2].getReal() - m[0][2].getReal(); + final double corr10 = o[1][0].getReal() - m[1][0].getReal(); + final double corr11 = o[1][1].getReal() - m[1][1].getReal(); + final double corr12 = o[1][2].getReal() - m[1][2].getReal(); + final double corr20 = o[2][0].getReal() - m[2][0].getReal(); + final double corr21 = o[2][1].getReal() - m[2][1].getReal(); + final double corr22 = o[2][2].getReal() - m[2][2].getReal(); + + // Frobenius norm of the correction + fn1 = corr00 * corr00 + corr01 * corr01 + corr02 * corr02 + + 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 <i>distance</i> between two rotations. + * <p>The <i>distance</i> 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:</p> + * <pre> + * r<sub>1</sub>(r) = r<sub>2</sub> + * </pre> + * <p>This distance is an angle between 0 and π. Its value is the smallest + * possible upper bound of the angle in radians between r<sub>1</sub>(v) + * and r<sub>2</sub>(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.</p> + * <p>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).</p> + * @param r1 first rotation + * @param r2 second rotation + * @param <T> the type of the field elements + * @return <i>distance</i> between r1 and r2 + */ + public static <T extends RealFieldElement<T>> T distance(final FieldRotation<T> r1, final FieldRotation<T> r2) { + return r1.composeInverseInternal(r2).getAngle(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java new file mode 100644 index 0000000..0bd04e5 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldVector3D.java @@ -0,0 +1,1185 @@ +/* + * 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.RealFieldElement; +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 RealFieldElement}. + * <p>Instance of this class are guaranteed to be immutable.</p> + * @param <T> the type of the field elements + * @since 3.2 + */ +public class FieldVector3D<T extends RealFieldElement<T>> implements Serializable { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 20130224L; + + /** Abscissa. */ + private final T x; + + /** Ordinate. */ + private final T y; + + /** Height. */ + private final T z; + + /** Simple constructor. + * Build a vector from its coordinates + * @param x abscissa + * @param y ordinate + * @param z height + * @see #getX() + * @see #getY() + * @see #getZ() + */ + public FieldVector3D(final T x, final T y, final T z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** Simple constructor. + * Build a vector from its coordinates + * @param v coordinates array + * @exception DimensionMismatchException if array does not have 3 elements + * @see #toArray() + */ + public FieldVector3D(final T[] v) throws DimensionMismatchException { + if (v.length != 3) { + throw new DimensionMismatchException(v.length, 3); + } + this.x = v[0]; + this.y = v[1]; + this.z = v[2]; + } + + /** Simple constructor. + * Build a vector from its azimuthal coordinates + * @param alpha azimuth (α) around Z + * (0 is +X, π/2 is +Y, π is -X and 3π/2 is -Y) + * @param delta elevation (δ) above (XY) plane, from -π/2 to +π/2 + * @see #getAlpha() + * @see #getDelta() + */ + public FieldVector3D(final T alpha, final T delta) { + T cosDelta = delta.cos(); + this.x = alpha.cos().multiply(cosDelta); + this.y = alpha.sin().multiply(cosDelta); + this.z = delta.sin(); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final T a, final FieldVector3D<T>u) { + this.x = a.multiply(u.x); + this.y = a.multiply(u.y); + this.z = a.multiply(u.z); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final T a, final Vector3D u) { + this.x = a.multiply(u.getX()); + this.y = a.multiply(u.getY()); + this.z = a.multiply(u.getZ()); + } + + /** Multiplicative constructor + * Build a vector from another one and a scale factor. + * The vector built will be a * u + * @param a scale factor + * @param u base (unscaled) vector + */ + public FieldVector3D(final double a, final FieldVector3D<T> u) { + this.x = u.x.multiply(a); + this.y = u.y.multiply(a); + this.z = u.z.multiply(a); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D<T> u1, + final T a2, final FieldVector3D<T> u2) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2); + } + + /** Linear constructor + * Build a vector from two other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D<T> u1, + final double a2, final FieldVector3D<T> u2) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ()); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D<T> u1, + final T a2, final FieldVector3D<T> u2, + final T a3, final FieldVector3D<T> u3) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2, + final T a3, final Vector3D u3) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3); + } + + /** Linear constructor + * Build a vector from three other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D<T> u1, + final double a2, final FieldVector3D<T> u2, + final double a3, final FieldVector3D<T> u3) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ()); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final T a1, final FieldVector3D<T> u1, + final T a2, final FieldVector3D<T> u2, + final T a3, final FieldVector3D<T> u3, + final T a4, final FieldVector3D<T> u4) { + final T prototype = a1; + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final T a1, final Vector3D u1, + final T a2, final Vector3D u2, + final T a3, final Vector3D u3, + final T a4, final Vector3D u4) { + final T prototype = a1; + this.x = prototype.linearCombination(u1.getX(), a1, u2.getX(), a2, u3.getX(), a3, u4.getX(), a4); + this.y = prototype.linearCombination(u1.getY(), a1, u2.getY(), a2, u3.getY(), a3, u4.getY(), a4); + this.z = prototype.linearCombination(u1.getZ(), a1, u2.getZ(), a2, u3.getZ(), a3, u4.getZ(), a4); + } + + /** Linear constructor + * Build a vector from four other ones and corresponding scale factors. + * The vector built will be a1 * u1 + a2 * u2 + a3 * u3 + a4 * u4 + * @param a1 first scale factor + * @param u1 first base (unscaled) vector + * @param a2 second scale factor + * @param u2 second base (unscaled) vector + * @param a3 third scale factor + * @param u3 third base (unscaled) vector + * @param a4 fourth scale factor + * @param u4 fourth base (unscaled) vector + */ + public FieldVector3D(final double a1, final FieldVector3D<T> u1, + final double a2, final FieldVector3D<T> u2, + final double a3, final FieldVector3D<T> u3, + final double a4, final FieldVector3D<T> u4) { + final T prototype = u1.getX(); + this.x = prototype.linearCombination(a1, u1.getX(), a2, u2.getX(), a3, u3.getX(), a4, u4.getX()); + this.y = prototype.linearCombination(a1, u1.getY(), a2, u2.getY(), a3, u3.getY(), a4, u4.getY()); + this.z = prototype.linearCombination(a1, u1.getZ(), a2, u2.getZ(), a3, u3.getZ(), a4, u4.getZ()); + } + + /** Get the abscissa of the vector. + * @return abscissa of the vector + * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) + */ + public T getX() { + return x; + } + + /** Get the ordinate of the vector. + * @return ordinate of the vector + * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) + */ + public T getY() { + return y; + } + + /** Get the height of the vector. + * @return height of the vector + * @see #FieldVector3D(RealFieldElement, RealFieldElement, RealFieldElement) + */ + public T getZ() { + return z; + } + + /** Get the vector coordinates as a dimension 3 array. + * @return vector coordinates + * @see #FieldVector3D(RealFieldElement[]) + */ + public T[] toArray() { + final T[] array = MathArrays.buildArray(x.getField(), 3); + array[0] = x; + array[1] = y; + array[2] = z; + return array; + } + + /** Convert to a constant vector without derivatives. + * @return a constant vector + */ + public Vector3D toVector3D() { + return new Vector3D(x.getReal(), y.getReal(), z.getReal()); + } + + /** Get the L<sub>1</sub> norm for the vector. + * @return L<sub>1</sub> norm for the vector + */ + public T getNorm1() { + return x.abs().add(y.abs()).add(z.abs()); + } + + /** Get the L<sub>2</sub> norm for the vector. + * @return Euclidean norm for the vector + */ + public T getNorm() { + // there are no cancellation problems here, so we use the straightforward formula + return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)).sqrt(); + } + + /** Get the square of the norm for the vector. + * @return square of the Euclidean norm for the vector + */ + public T getNormSq() { + // there are no cancellation problems here, so we use the straightforward formula + return x.multiply(x).add(y.multiply(y)).add(z.multiply(z)); + } + + /** Get the L<sub>∞</sub> norm for the vector. + * @return L<sub>∞</sub> norm for the vector + */ + public T getNormInf() { + final T xAbs = x.abs(); + final T yAbs = y.abs(); + final T zAbs = z.abs(); + if (xAbs.getReal() <= yAbs.getReal()) { + if (yAbs.getReal() <= zAbs.getReal()) { + return zAbs; + } else { + return yAbs; + } + } else { + if (xAbs.getReal() <= zAbs.getReal()) { + return zAbs; + } else { + return xAbs; + } + } + } + + /** Get the azimuth of the vector. + * @return azimuth (α) of the vector, between -π and +π + * @see #FieldVector3D(RealFieldElement, RealFieldElement) + */ + public T getAlpha() { + return y.atan2(x); + } + + /** Get the elevation of the vector. + * @return elevation (δ) of the vector, between -π/2 and +π/2 + * @see #FieldVector3D(RealFieldElement, RealFieldElement) + */ + public T getDelta() { + return z.divide(getNorm()).asin(); + } + + /** Add a vector to the instance. + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final FieldVector3D<T> v) { + return new FieldVector3D<T>(x.add(v.x), y.add(v.y), z.add(v.z)); + } + + /** Add a vector to the instance. + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final Vector3D v) { + return new FieldVector3D<T>(x.add(v.getX()), y.add(v.getY()), z.add(v.getZ())); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final T factor, final FieldVector3D<T> v) { + return new FieldVector3D<T>(x.getField().getOne(), this, factor, v); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final T factor, final Vector3D v) { + return new FieldVector3D<T>(x.add(factor.multiply(v.getX())), + y.add(factor.multiply(v.getY())), + z.add(factor.multiply(v.getZ()))); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final double factor, final FieldVector3D<T> v) { + return new FieldVector3D<T>(1.0, this, factor, v); + } + + /** Add a scaled vector to the instance. + * @param factor scale factor to apply to v before adding it + * @param v vector to add + * @return a new vector + */ + public FieldVector3D<T> add(final double factor, final Vector3D v) { + return new FieldVector3D<T>(x.add(factor * v.getX()), + y.add(factor * v.getY()), + z.add(factor * v.getZ())); + } + + /** Subtract a vector from the instance. + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final FieldVector3D<T> v) { + return new FieldVector3D<T>(x.subtract(v.x), y.subtract(v.y), z.subtract(v.z)); + } + + /** Subtract a vector from the instance. + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final Vector3D v) { + return new FieldVector3D<T>(x.subtract(v.getX()), y.subtract(v.getY()), z.subtract(v.getZ())); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final T factor, final FieldVector3D<T> v) { + return new FieldVector3D<T>(x.getField().getOne(), this, factor.negate(), v); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final T factor, final Vector3D v) { + return new FieldVector3D<T>(x.subtract(factor.multiply(v.getX())), + y.subtract(factor.multiply(v.getY())), + z.subtract(factor.multiply(v.getZ()))); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final double factor, final FieldVector3D<T> v) { + return new FieldVector3D<T>(1.0, this, -factor, v); + } + + /** Subtract a scaled vector from the instance. + * @param factor scale factor to apply to v before subtracting it + * @param v vector to subtract + * @return a new vector + */ + public FieldVector3D<T> subtract(final double factor, final Vector3D v) { + return new FieldVector3D<T>(x.subtract(factor * v.getX()), + y.subtract(factor * v.getY()), + z.subtract(factor * v.getZ())); + } + + /** Get a normalized vector aligned with the instance. + * @return a new normalized vector + * @exception MathArithmeticException if the norm is zero + */ + public FieldVector3D<T> normalize() throws MathArithmeticException { + final T s = getNorm(); + if (s.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); + } + return scalarMultiply(s.reciprocal()); + } + + /** Get a vector orthogonal to the instance. + * <p>There are an infinite number of normalized vectors orthogonal + * to the instance. This method picks up one of them almost + * arbitrarily. It is useful when one needs to compute a reference + * frame with one of the axes in a predefined direction. The + * following example shows how to build a frame having the k axis + * aligned with the known vector u : + * <pre><code> + * Vector3D k = u.normalize(); + * Vector3D i = k.orthogonal(); + * Vector3D j = Vector3D.crossProduct(k, i); + * </code></pre></p> + * @return a new normalized vector orthogonal to the instance + * @exception MathArithmeticException if the norm of the instance is null + */ + public FieldVector3D<T> orthogonal() throws MathArithmeticException { + + final double threshold = 0.6 * getNorm().getReal(); + if (threshold == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + if (FastMath.abs(x.getReal()) <= threshold) { + final T inverse = y.multiply(y).add(z.multiply(z)).sqrt().reciprocal(); + return new FieldVector3D<T>(inverse.getField().getZero(), inverse.multiply(z), inverse.multiply(y).negate()); + } else if (FastMath.abs(y.getReal()) <= threshold) { + final T inverse = x.multiply(x).add(z.multiply(z)).sqrt().reciprocal(); + return new FieldVector3D<T>(inverse.multiply(z).negate(), inverse.getField().getZero(), inverse.multiply(x)); + } else { + final T inverse = x.multiply(x).add(y.multiply(y)).sqrt().reciprocal(); + return new FieldVector3D<T>(inverse.multiply(y), inverse.multiply(x).negate(), inverse.getField().getZero()); + } + + } + + /** Compute the angular separation between two vectors. + * <p>This method computes the angular separation between two + * vectors using the dot product for well separated vectors and the + * cross product for almost aligned vectors. This allows to have a + * good accuracy in all cases, even for vectors very close to each + * other.</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return angular separation between v1 and v2 + * @exception MathArithmeticException if either vector has a null norm + */ + public static <T extends RealFieldElement<T>> T angle(final FieldVector3D<T> v1, final FieldVector3D<T> v2) + throws MathArithmeticException { + + final T normProduct = v1.getNorm().multiply(v2.getNorm()); + if (normProduct.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + final T dot = dotProduct(v1, v2); + final double threshold = normProduct.getReal() * 0.9999; + if ((dot.getReal() < -threshold) || (dot.getReal() > threshold)) { + // the vectors are almost aligned, compute using the sine + FieldVector3D<T> v3 = crossProduct(v1, v2); + if (dot.getReal() >= 0) { + return v3.getNorm().divide(normProduct).asin(); + } + return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); + } + + // the vectors are sufficiently separated to use the cosine + return dot.divide(normProduct).acos(); + + } + + /** Compute the angular separation between two vectors. + * <p>This method computes the angular separation between two + * vectors using the dot product for well separated vectors and the + * cross product for almost aligned vectors. This allows to have a + * good accuracy in all cases, even for vectors very close to each + * other.</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return angular separation between v1 and v2 + * @exception MathArithmeticException if either vector has a null norm + */ + public static <T extends RealFieldElement<T>> T angle(final FieldVector3D<T> v1, final Vector3D v2) + throws MathArithmeticException { + + final T normProduct = v1.getNorm().multiply(v2.getNorm()); + if (normProduct.getReal() == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + final T dot = dotProduct(v1, v2); + final double threshold = normProduct.getReal() * 0.9999; + if ((dot.getReal() < -threshold) || (dot.getReal() > threshold)) { + // the vectors are almost aligned, compute using the sine + FieldVector3D<T> v3 = crossProduct(v1, v2); + if (dot.getReal() >= 0) { + return v3.getNorm().divide(normProduct).asin(); + } + return v3.getNorm().divide(normProduct).asin().subtract(FastMath.PI).negate(); + } + + // the vectors are sufficiently separated to use the cosine + return dot.divide(normProduct).acos(); + + } + + /** Compute the angular separation between two vectors. + * <p>This method computes the angular separation between two + * vectors using the dot product for well separated vectors and the + * cross product for almost aligned vectors. This allows to have a + * good accuracy in all cases, even for vectors very close to each + * other.</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return angular separation between v1 and v2 + * @exception MathArithmeticException if either vector has a null norm + */ + public static <T extends RealFieldElement<T>> T angle(final Vector3D v1, final FieldVector3D<T> v2) + throws MathArithmeticException { + return angle(v2, v1); + } + + /** Get the opposite of the instance. + * @return a new vector which is opposite to the instance + */ + public FieldVector3D<T> negate() { + return new FieldVector3D<T>(x.negate(), y.negate(), z.negate()); + } + + /** Multiply the instance by a scalar. + * @param a scalar + * @return a new vector + */ + public FieldVector3D<T> scalarMultiply(final T a) { + return new FieldVector3D<T>(x.multiply(a), y.multiply(a), z.multiply(a)); + } + + /** Multiply the instance by a scalar. + * @param a scalar + * @return a new vector + */ + public FieldVector3D<T> scalarMultiply(final double a) { + return new FieldVector3D<T>(x.multiply(a), y.multiply(a), z.multiply(a)); + } + + /** + * Returns true if any coordinate of this vector is NaN; false otherwise + * @return true if any coordinate of this vector is NaN; false otherwise + */ + public boolean isNaN() { + return Double.isNaN(x.getReal()) || Double.isNaN(y.getReal()) || Double.isNaN(z.getReal()); + } + + /** + * Returns true if any coordinate of this vector is infinite and none are NaN; + * false otherwise + * @return true if any coordinate of this vector is infinite and none are NaN; + * false otherwise + */ + public boolean isInfinite() { + return !isNaN() && (Double.isInfinite(x.getReal()) || Double.isInfinite(y.getReal()) || Double.isInfinite(z.getReal())); + } + + /** + * Test for the equality of two 3D vectors. + * <p> + * If all coordinates of two 3D vectors are exactly the same, and none of their + * {@link RealFieldElement#getReal() real part} are <code>NaN</code>, the + * two 3D vectors are considered to be equal. + * </p> + * <p> + * <code>NaN</code> coordinates are considered to affect globally the vector + * and be equals to each other - i.e, if either (or all) real part of the + * coordinates of the 3D vector are <code>NaN</code>, the 3D vector is <code>NaN</code>. + * </p> + * + * @param other Object to test for equality to this + * @return true if two 3D vector objects are equal, false if + * object is null, not an instance of Vector3D, or + * not equal to this Vector3D instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof FieldVector3D) { + @SuppressWarnings("unchecked") + final FieldVector3D<T> rhs = (FieldVector3D<T>) other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return x.equals(rhs.x) && y.equals(rhs.y) && z.equals(rhs.z); + + } + return false; + } + + /** + * Get a hashCode for the 3D vector. + * <p> + * All NaN values have the same hash code.</p> + * + * @return a hash code value for this object + */ + @Override + public int hashCode() { + if (isNaN()) { + return 409; + } + return 311 * (107 * x.hashCode() + 83 * y.hashCode() + z.hashCode()); + } + + /** Compute the dot-product of the instance and another vector. + * <p> + * The implementation uses specific multiplication and addition + * algorithms to preserve accuracy and reduce cancellation effects. + * It should be very accurate even for nearly orthogonal vectors. + * </p> + * @see MathArrays#linearCombination(double, double, double, double, double, double) + * @param v second vector + * @return the dot product this.v + */ + public T dotProduct(final FieldVector3D<T> v) { + return x.linearCombination(x, v.x, y, v.y, z, v.z); + } + + /** Compute the dot-product of the instance and another vector. + * <p> + * The implementation uses specific multiplication and addition + * algorithms to preserve accuracy and reduce cancellation effects. + * It should be very accurate even for nearly orthogonal vectors. + * </p> + * @see MathArrays#linearCombination(double, double, double, double, double, double) + * @param v second vector + * @return the dot product this.v + */ + public T dotProduct(final Vector3D v) { + return x.linearCombination(v.getX(), x, v.getY(), y, v.getZ(), z); + } + + /** Compute the cross-product of the instance with another vector. + * @param v other vector + * @return the cross product this ^ v as a new Vector3D + */ + public FieldVector3D<T> crossProduct(final FieldVector3D<T> v) { + return new FieldVector3D<T>(x.linearCombination(y, v.z, z.negate(), v.y), + y.linearCombination(z, v.x, x.negate(), v.z), + z.linearCombination(x, v.y, y.negate(), v.x)); + } + + /** Compute the cross-product of the instance with another vector. + * @param v other vector + * @return the cross product this ^ v as a new Vector3D + */ + public FieldVector3D<T> crossProduct(final Vector3D v) { + return new FieldVector3D<T>(x.linearCombination(v.getZ(), y, -v.getY(), z), + y.linearCombination(v.getX(), z, -v.getZ(), x), + z.linearCombination(v.getY(), x, -v.getX(), y)); + } + + /** Compute the distance between the instance and another vector according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>1</sub> norm + */ + public T distance1(final FieldVector3D<T> v) { + final T dx = v.x.subtract(x).abs(); + final T dy = v.y.subtract(y).abs(); + final T dz = v.z.subtract(z).abs(); + return dx.add(dy).add(dz); + } + + /** Compute the distance between the instance and another vector according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>1</sub> norm + */ + public T distance1(final Vector3D v) { + final T dx = x.subtract(v.getX()).abs(); + final T dy = y.subtract(v.getY()).abs(); + final T dz = z.subtract(v.getZ()).abs(); + return dx.add(dy).add(dz); + } + + /** Compute the distance between the instance and another vector according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>2</sub> norm + */ + public T distance(final FieldVector3D<T> v) { + final T dx = v.x.subtract(x); + final T dy = v.y.subtract(y); + final T dz = v.z.subtract(z); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); + } + + /** Compute the distance between the instance and another vector according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>2</sub> norm + */ + public T distance(final Vector3D v) { + final T dx = x.subtract(v.getX()); + final T dy = y.subtract(v.getY()); + final T dz = z.subtract(v.getZ()); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)).sqrt(); + } + + /** Compute the distance between the instance and another vector according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>∞</sub> norm + */ + public T distanceInf(final FieldVector3D<T> v) { + final T dx = v.x.subtract(x).abs(); + final T dy = v.y.subtract(y).abs(); + final T dz = v.z.subtract(z).abs(); + if (dx.getReal() <= dy.getReal()) { + if (dy.getReal() <= dz.getReal()) { + return dz; + } else { + return dy; + } + } else { + if (dx.getReal() <= dz.getReal()) { + return dz; + } else { + return dx; + } + } + } + + /** Compute the distance between the instance and another vector according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the distance between the instance and p according to the L<sub>∞</sub> norm + */ + public T distanceInf(final Vector3D v) { + final T dx = x.subtract(v.getX()).abs(); + final T dy = y.subtract(v.getY()).abs(); + final T dz = z.subtract(v.getZ()).abs(); + if (dx.getReal() <= dy.getReal()) { + if (dy.getReal() <= dz.getReal()) { + return dz; + } else { + return dy; + } + } else { + if (dx.getReal() <= dz.getReal()) { + return dz; + } else { + return dx; + } + } + } + + /** Compute the square of the distance between the instance and another vector. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the square of the distance between the instance and p + */ + public T distanceSq(final FieldVector3D<T> v) { + final T dx = v.x.subtract(x); + final T dy = v.y.subtract(y); + final T dz = v.z.subtract(z); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); + } + + /** Compute the square of the distance between the instance and another vector. + * <p>Calling this method is equivalent to calling: + * <code>q.subtract(p).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v second vector + * @return the square of the distance between the instance and p + */ + public T distanceSq(final Vector3D v) { + final T dx = x.subtract(v.getX()); + final T dy = y.subtract(v.getY()); + final T dz = z.subtract(v.getZ()); + return dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); + } + + /** Compute the dot-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the dot product v1.v2 + */ + public static <T extends RealFieldElement<T>> T dotProduct(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.dotProduct(v2); + } + + /** Compute the dot-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the dot product v1.v2 + */ + public static <T extends RealFieldElement<T>> T dotProduct(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.dotProduct(v2); + } + + /** Compute the dot-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the dot product v1.v2 + */ + public static <T extends RealFieldElement<T>> T dotProduct(final Vector3D v1, + final FieldVector3D<T> v2) { + return v2.dotProduct(v1); + } + + /** Compute the cross-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the cross product v1 ^ v2 as a new Vector + */ + public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.crossProduct(v2); + } + + /** Compute the cross-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the cross product v1 ^ v2 as a new Vector + */ + public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.crossProduct(v2); + } + + /** Compute the cross-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the cross product v1 ^ v2 as a new Vector + */ + public static <T extends RealFieldElement<T>> FieldVector3D<T> crossProduct(final Vector3D v1, + final FieldVector3D<T> v2) { + return new FieldVector3D<T>(v2.x.linearCombination(v1.getY(), v2.z, -v1.getZ(), v2.y), + v2.y.linearCombination(v1.getZ(), v2.x, -v1.getX(), v2.z), + v2.z.linearCombination(v1.getX(), v2.y, -v1.getY(), v2.x)); + } + + /** Compute the distance between two vectors according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>1</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance1(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.distance1(v2); + } + + /** Compute the distance between two vectors according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>1</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance1(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.distance1(v2); + } + + /** Compute the distance between two vectors according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>1</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance1(final Vector3D v1, + final FieldVector3D<T> v2) { + return v2.distance1(v1); + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>2</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.distance(v2); + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>2</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.distance(v2); + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>2</sub> norm + */ + public static <T extends RealFieldElement<T>> T distance(final Vector3D v1, + final FieldVector3D<T> v2) { + return v2.distance(v1); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm + */ + public static <T extends RealFieldElement<T>> T distanceInf(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.distanceInf(v2); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm + */ + public static <T extends RealFieldElement<T>> T distanceInf(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.distanceInf(v2); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm + */ + public static <T extends RealFieldElement<T>> T distanceInf(final Vector3D v1, + final FieldVector3D<T> v2) { + return v2.distanceInf(v1); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the square of the distance between v1 and v2 + */ + public static <T extends RealFieldElement<T>> T distanceSq(final FieldVector3D<T> v1, + final FieldVector3D<T> v2) { + return v1.distanceSq(v2); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the square of the distance between v1 and v2 + */ + public static <T extends RealFieldElement<T>> T distanceSq(final FieldVector3D<T> v1, + final Vector3D v2) { + return v1.distanceSq(v2); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @param <T> the type of the field elements + * @return the square of the distance between v1 and v2 + */ + public static <T extends RealFieldElement<T>> T distanceSq(final Vector3D v1, + final FieldVector3D<T> 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()); + } + + /** Get a string representation of this vector. + * @param format the custom format for components + * @return a string representation of this vector + */ + public String toString(final NumberFormat format) { + return new Vector3DFormat(format).format(toVector3D()); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Line.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Line.java new file mode 100644 index 0000000..e234495 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Line.java @@ -0,0 +1,275 @@ +/* + * 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.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Vector; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; +import org.apache.commons.math3.geometry.euclidean.oned.IntervalsSet; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +import org.apache.commons.math3.geometry.partitioning.Embedding; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.Precision; + +/** The class represent lines in a three dimensional space. + + * <p>Each oriented line is intrinsically associated with an abscissa + * which is a coordinate on the line. The point at abscissa 0 is the + * orthogonal projection of the origin on the line, another equivalent + * way to express this is to say that it is the point of the line + * which is closest to the origin. Abscissa increases in the line + * direction.</p> + + * @since 3.0 + */ +public class Line implements Embedding<Euclidean3D, Euclidean1D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Line direction. */ + private Vector3D direction; + + /** Line point closest to the origin. */ + private Vector3D zero; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** Build a line from two points. + * @param p1 first point belonging to the line (this can be any point) + * @param p2 second point belonging to the line (this can be any point, different from p1) + * @param tolerance tolerance below which points are considered identical + * @exception MathIllegalArgumentException if the points are equal + * @since 3.3 + */ + public Line(final Vector3D p1, final Vector3D p2, final double tolerance) + throws MathIllegalArgumentException { + reset(p1, p2); + this.tolerance = tolerance; + } + + /** Copy constructor. + * <p>The created instance is completely independent from the + * original instance, it is a deep copy.</p> + * @param line line to copy + */ + public Line(final Line line) { + this.direction = line.direction; + this.zero = line.zero; + this.tolerance = line.tolerance; + } + + /** Build a line from two points. + * @param p1 first point belonging to the line (this can be any point) + * @param p2 second point belonging to the line (this can be any point, different from p1) + * @exception MathIllegalArgumentException if the points are equal + * @deprecated as of 3.3, replaced with {@link #Line(Vector3D, Vector3D, double)} + */ + @Deprecated + public Line(final Vector3D p1, final Vector3D p2) throws MathIllegalArgumentException { + this(p1, p2, DEFAULT_TOLERANCE); + } + + /** Reset the instance as if built from two points. + * @param p1 first point belonging to the line (this can be any point) + * @param p2 second point belonging to the line (this can be any point, different from p1) + * @exception MathIllegalArgumentException if the points are equal + */ + public void reset(final Vector3D p1, final Vector3D p2) throws MathIllegalArgumentException { + final Vector3D delta = p2.subtract(p1); + final double norm2 = delta.getNormSq(); + if (norm2 == 0.0) { + throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM); + } + this.direction = new Vector3D(1.0 / FastMath.sqrt(norm2), delta); + zero = new Vector3D(1.0, p1, -p1.dotProduct(delta) / norm2, delta); + } + + /** Get the tolerance below which points are considered identical. + * @return tolerance below which points are considered identical + * @since 3.3 + */ + public double getTolerance() { + return tolerance; + } + + /** Get a line with reversed direction. + * @return a new instance, with reversed direction + */ + public Line revert() { + final Line reverted = new Line(this); + reverted.direction = reverted.direction.negate(); + return reverted; + } + + /** Get the normalized direction vector. + * @return normalized direction vector + */ + public Vector3D getDirection() { + return direction; + } + + /** Get the line point closest to the origin. + * @return line point closest to the origin + */ + public Vector3D getOrigin() { + return zero; + } + + /** Get the abscissa of a point with respect to the line. + * <p>The abscissa is 0 if the projection of the point and the + * projection of the frame origin on the line are the same + * point.</p> + * @param point point to check + * @return abscissa of the point + */ + public double getAbscissa(final Vector3D point) { + return point.subtract(zero).dotProduct(direction); + } + + /** Get one point from the line. + * @param abscissa desired abscissa for the point + * @return one point belonging to the line, at specified abscissa + */ + public Vector3D pointAt(final double abscissa) { + return new Vector3D(1.0, zero, abscissa, direction); + } + + /** Transform a space point into a sub-space point. + * @param vector n-dimension point of the space + * @return (n-1)-dimension point of the sub-space corresponding to + * the specified space point + */ + public Vector1D toSubSpace(Vector<Euclidean3D> vector) { + return toSubSpace((Point<Euclidean3D>) vector); + } + + /** Transform a sub-space point into a space point. + * @param vector (n-1)-dimension point of the sub-space + * @return n-dimension point of the space corresponding to the + * specified sub-space point + */ + public Vector3D toSpace(Vector<Euclidean1D> vector) { + return toSpace((Point<Euclidean1D>) vector); + } + + /** {@inheritDoc} + * @see #getAbscissa(Vector3D) + */ + public Vector1D toSubSpace(final Point<Euclidean3D> point) { + return new Vector1D(getAbscissa((Vector3D) point)); + } + + /** {@inheritDoc} + * @see #pointAt(double) + */ + public Vector3D toSpace(final Point<Euclidean1D> point) { + return pointAt(((Vector1D) point).getX()); + } + + /** Check if the instance is similar to another line. + * <p>Lines are considered similar if they contain the same + * points. This does not mean they are equal since they can have + * opposite directions.</p> + * @param line line to which instance should be compared + * @return true if the lines are similar + */ + public boolean isSimilarTo(final Line line) { + final double angle = Vector3D.angle(direction, line.direction); + return ((angle < tolerance) || (angle > (FastMath.PI - tolerance))) && contains(line.zero); + } + + /** Check if the instance contains a point. + * @param p point to check + * @return true if p belongs to the line + */ + public boolean contains(final Vector3D p) { + return distance(p) < tolerance; + } + + /** Compute the distance between the instance and a point. + * @param p to check + * @return distance between the instance and the point + */ + public double distance(final Vector3D p) { + final Vector3D d = p.subtract(zero); + final Vector3D n = new Vector3D(1.0, d, -d.dotProduct(direction), direction); + return n.getNorm(); + } + + /** Compute the shortest distance between the instance and another line. + * @param line line to check against the instance + * @return shortest distance between the instance and the line + */ + public double distance(final Line line) { + + final Vector3D normal = Vector3D.crossProduct(direction, line.direction); + final double n = normal.getNorm(); + if (n < Precision.SAFE_MIN) { + // lines are parallel + return distance(line.zero); + } + + // signed separation of the two parallel planes that contains the lines + final double offset = line.zero.subtract(zero).dotProduct(normal) / n; + + return FastMath.abs(offset); + + } + + /** Compute the point of the instance closest to another line. + * @param line line to check against the instance + * @return point of the instance closest to another line + */ + public Vector3D closestPoint(final Line line) { + + final double cos = direction.dotProduct(line.direction); + final double n = 1 - cos * cos; + if (n < Precision.EPSILON) { + // the lines are parallel + return zero; + } + + final Vector3D delta0 = line.zero.subtract(zero); + final double a = delta0.dotProduct(direction); + final double b = delta0.dotProduct(line.direction); + + return new Vector3D(1, zero, (a - b * cos) / n, direction); + + } + + /** Get the intersection point of the instance and another line. + * @param line other line + * @return intersection point of the instance and the other line + * or null if there are no intersection points + */ + public Vector3D intersection(final Line line) { + final Vector3D closest = closestPoint(line); + return line.contains(closest) ? closest : null; + } + + /** Build a sub-line covering the whole line. + * @return a sub-line covering the whole line + */ + public SubLine wholeLine() { + return new SubLine(this, new IntervalsSet(tolerance)); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/NotARotationMatrixException.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/NotARotationMatrixException.java new file mode 100644 index 0000000..3f1f3d3 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/NotARotationMatrixException.java @@ -0,0 +1,47 @@ +/* + * 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.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.util.Localizable; + +/** + * This class represents exceptions thrown while building rotations + * from matrices. + * + * @since 1.2 + */ + +public class NotARotationMatrixException + extends MathIllegalArgumentException { + + /** Serializable version identifier */ + private static final long serialVersionUID = 5647178478658937642L; + + /** + * Simple constructor. + * Build an exception by translating and formating a message + * @param specifier format specifier (to be translated) + * @param parts to insert in the format (no translation) + * @since 2.2 + */ + public NotARotationMatrixException(Localizable specifier, Object ... parts) { + super(specifier, parts); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/OutlineExtractor.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/OutlineExtractor.java new file mode 100644 index 0000000..0f8af88 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/OutlineExtractor.java @@ -0,0 +1,263 @@ +/* + * 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.util.ArrayList; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.partitioning.AbstractSubHyperplane; +import org.apache.commons.math3.geometry.partitioning.BSPTree; +import org.apache.commons.math3.geometry.partitioning.BSPTreeVisitor; +import org.apache.commons.math3.geometry.partitioning.BoundaryAttribute; +import org.apache.commons.math3.geometry.partitioning.RegionFactory; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.util.FastMath; + +/** Extractor for {@link PolygonsSet polyhedrons sets} outlines. + * <p>This class extracts the 2D outlines from {{@link PolygonsSet + * polyhedrons sets} in a specified projection plane.</p> + * @since 3.0 + */ +public class OutlineExtractor { + + /** Abscissa axis of the projection plane. */ + private Vector3D u; + + /** Ordinate axis of the projection plane. */ + private Vector3D v; + + /** Normal of the projection plane (viewing direction). */ + private Vector3D w; + + /** Build an extractor for a specific projection plane. + * @param u abscissa axis of the projection point + * @param v ordinate axis of the projection point + */ + public OutlineExtractor(final Vector3D u, final Vector3D v) { + this.u = u; + this.v = v; + w = Vector3D.crossProduct(u, v); + } + + /** Extract the outline of a polyhedrons set. + * @param polyhedronsSet polyhedrons set whose outline must be extracted + * @return an outline, as an array of loops. + */ + public Vector2D[][] getOutline(final PolyhedronsSet polyhedronsSet) { + + // project all boundary facets into one polygons set + final BoundaryProjector projector = new BoundaryProjector(polyhedronsSet.getTolerance()); + polyhedronsSet.getTree(true).visit(projector); + final PolygonsSet projected = projector.getProjected(); + + // Remove the spurious intermediate vertices from the outline + final Vector2D[][] outline = projected.getVertices(); + for (int i = 0; i < outline.length; ++i) { + final Vector2D[] rawLoop = outline[i]; + int end = rawLoop.length; + int j = 0; + while (j < end) { + if (pointIsBetween(rawLoop, end, j)) { + // the point should be removed + for (int k = j; k < (end - 1); ++k) { + rawLoop[k] = rawLoop[k + 1]; + } + --end; + } else { + // the point remains in the loop + ++j; + } + } + if (end != rawLoop.length) { + // resize the array + outline[i] = new Vector2D[end]; + System.arraycopy(rawLoop, 0, outline[i], 0, end); + } + } + + return outline; + + } + + /** Check if a point is geometrically between its neighbor in an array. + * <p>The neighbors are computed considering the array is a loop + * (i.e. point at index (n-1) is before point at index 0)</p> + * @param loop points array + * @param n number of points to consider in the array + * @param i index of the point to check (must be between 0 and n-1) + * @return true if the point is exactly between its neighbors + */ + private boolean pointIsBetween(final Vector2D[] loop, final int n, final int i) { + final Vector2D previous = loop[(i + n - 1) % n]; + final Vector2D current = loop[i]; + final Vector2D next = loop[(i + 1) % n]; + final double dx1 = current.getX() - previous.getX(); + final double dy1 = current.getY() - previous.getY(); + final double dx2 = next.getX() - current.getX(); + final double dy2 = next.getY() - current.getY(); + final double cross = dx1 * dy2 - dx2 * dy1; + final double dot = dx1 * dx2 + dy1 * dy2; + final double d1d2 = FastMath.sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)); + return (FastMath.abs(cross) <= (1.0e-6 * d1d2)) && (dot >= 0.0); + } + + /** Visitor projecting the boundary facets on a plane. */ + private class BoundaryProjector implements BSPTreeVisitor<Euclidean3D> { + + /** Projection of the polyhedrons set on the plane. */ + private PolygonsSet projected; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** Simple constructor. + * @param tolerance tolerance below which points are considered identical + */ + BoundaryProjector(final double tolerance) { + this.projected = new PolygonsSet(new BSPTree<Euclidean2D>(Boolean.FALSE), tolerance); + this.tolerance = tolerance; + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<Euclidean3D> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<Euclidean3D> node) { + @SuppressWarnings("unchecked") + final BoundaryAttribute<Euclidean3D> attribute = + (BoundaryAttribute<Euclidean3D>) node.getAttribute(); + if (attribute.getPlusOutside() != null) { + addContribution(attribute.getPlusOutside(), false); + } + if (attribute.getPlusInside() != null) { + addContribution(attribute.getPlusInside(), true); + } + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<Euclidean3D> node) { + } + + /** Add he contribution of a boundary facet. + * @param facet boundary facet + * @param reversed if true, the facet has the inside on its plus side + */ + private void addContribution(final SubHyperplane<Euclidean3D> facet, final boolean reversed) { + + // extract the vertices of the facet + @SuppressWarnings("unchecked") + final AbstractSubHyperplane<Euclidean3D, Euclidean2D> absFacet = + (AbstractSubHyperplane<Euclidean3D, Euclidean2D>) facet; + final Plane plane = (Plane) facet.getHyperplane(); + + final double scal = plane.getNormal().dotProduct(w); + if (FastMath.abs(scal) > 1.0e-3) { + Vector2D[][] vertices = + ((PolygonsSet) absFacet.getRemainingRegion()).getVertices(); + + if ((scal < 0) ^ reversed) { + // the facet is seen from the inside, + // we need to invert its boundary orientation + final Vector2D[][] newVertices = new Vector2D[vertices.length][]; + for (int i = 0; i < vertices.length; ++i) { + final Vector2D[] loop = vertices[i]; + final Vector2D[] newLoop = new Vector2D[loop.length]; + if (loop[0] == null) { + newLoop[0] = null; + for (int j = 1; j < loop.length; ++j) { + newLoop[j] = loop[loop.length - j]; + } + } else { + for (int j = 0; j < loop.length; ++j) { + newLoop[j] = loop[loop.length - (j + 1)]; + } + } + newVertices[i] = newLoop; + } + + // use the reverted vertices + vertices = newVertices; + + } + + // compute the projection of the facet in the outline plane + final ArrayList<SubHyperplane<Euclidean2D>> edges = new ArrayList<SubHyperplane<Euclidean2D>>(); + for (Vector2D[] loop : vertices) { + final boolean closed = loop[0] != null; + int previous = closed ? (loop.length - 1) : 1; + Vector3D previous3D = plane.toSpace((Point<Euclidean2D>) loop[previous]); + int current = (previous + 1) % loop.length; + Vector2D pPoint = new Vector2D(previous3D.dotProduct(u), + previous3D.dotProduct(v)); + while (current < loop.length) { + + final Vector3D current3D = plane.toSpace((Point<Euclidean2D>) loop[current]); + final Vector2D cPoint = new Vector2D(current3D.dotProduct(u), + current3D.dotProduct(v)); + final org.apache.commons.math3.geometry.euclidean.twod.Line line = + new org.apache.commons.math3.geometry.euclidean.twod.Line(pPoint, cPoint, tolerance); + SubHyperplane<Euclidean2D> edge = line.wholeHyperplane(); + + if (closed || (previous != 1)) { + // the previous point is a real vertex + // it defines one bounding point of the edge + final double angle = line.getAngle() + 0.5 * FastMath.PI; + final org.apache.commons.math3.geometry.euclidean.twod.Line l = + new org.apache.commons.math3.geometry.euclidean.twod.Line(pPoint, angle, tolerance); + edge = edge.split(l).getPlus(); + } + + if (closed || (current != (loop.length - 1))) { + // the current point is a real vertex + // it defines one bounding point of the edge + final double angle = line.getAngle() + 0.5 * FastMath.PI; + final org.apache.commons.math3.geometry.euclidean.twod.Line l = + new org.apache.commons.math3.geometry.euclidean.twod.Line(cPoint, angle, tolerance); + edge = edge.split(l).getMinus(); + } + + edges.add(edge); + + previous = current++; + previous3D = current3D; + pPoint = cPoint; + + } + } + final PolygonsSet projectedFacet = new PolygonsSet(edges, tolerance); + + // add the contribution of the facet to the global outline + projected = (PolygonsSet) new RegionFactory<Euclidean2D>().union(projected, projectedFacet); + + } + } + + /** Get the projection of the polyhedrons set on the plane. + * @return projection of the polyhedrons set on the plane + */ + public PolygonsSet getProjected() { + return projected; + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Plane.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Plane.java new file mode 100644 index 0000000..158818d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Plane.java @@ -0,0 +1,527 @@ +/* + * 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.exception.MathArithmeticException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Vector; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.partitioning.Embedding; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.util.FastMath; + +/** The class represent planes in a three dimensional space. + * @since 3.0 + */ +public class Plane implements Hyperplane<Euclidean3D>, Embedding<Euclidean3D, Euclidean2D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Offset of the origin with respect to the plane. */ + private double originOffset; + + /** Origin of the plane frame. */ + private Vector3D origin; + + /** First vector of the plane frame (in plane). */ + private Vector3D u; + + /** Second vector of the plane frame (in plane). */ + private Vector3D v; + + /** Third vector of the plane frame (plane normal). */ + private Vector3D w; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** Build a plane normal to a given direction and containing the origin. + * @param normal normal direction to the plane + * @param tolerance tolerance below which points are considered identical + * @exception MathArithmeticException if the normal norm is too small + * @since 3.3 + */ + public Plane(final Vector3D normal, final double tolerance) + throws MathArithmeticException { + setNormal(normal); + this.tolerance = tolerance; + originOffset = 0; + setFrame(); + } + + /** Build a plane from a point and a normal. + * @param p point belonging to the plane + * @param normal normal direction to the plane + * @param tolerance tolerance below which points are considered identical + * @exception MathArithmeticException if the normal norm is too small + * @since 3.3 + */ + public Plane(final Vector3D p, final Vector3D normal, final double tolerance) + throws MathArithmeticException { + setNormal(normal); + this.tolerance = tolerance; + originOffset = -p.dotProduct(w); + setFrame(); + } + + /** Build a plane from three points. + * <p>The plane is oriented in the direction of + * {@code (p2-p1) ^ (p3-p1)}</p> + * @param p1 first point belonging to the plane + * @param p2 second point belonging to the plane + * @param p3 third point belonging to the plane + * @param tolerance tolerance below which points are considered identical + * @exception MathArithmeticException if the points do not constitute a plane + * @since 3.3 + */ + public Plane(final Vector3D p1, final Vector3D p2, final Vector3D p3, final double tolerance) + throws MathArithmeticException { + this(p1, p2.subtract(p1).crossProduct(p3.subtract(p1)), tolerance); + } + + /** Build a plane normal to a given direction and containing the origin. + * @param normal normal direction to the plane + * @exception MathArithmeticException if the normal norm is too small + * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, double)} + */ + @Deprecated + public Plane(final Vector3D normal) throws MathArithmeticException { + this(normal, DEFAULT_TOLERANCE); + } + + /** Build a plane from a point and a normal. + * @param p point belonging to the plane + * @param normal normal direction to the plane + * @exception MathArithmeticException if the normal norm is too small + * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, Vector3D, double)} + */ + @Deprecated + public Plane(final Vector3D p, final Vector3D normal) throws MathArithmeticException { + this(p, normal, DEFAULT_TOLERANCE); + } + + /** Build a plane from three points. + * <p>The plane is oriented in the direction of + * {@code (p2-p1) ^ (p3-p1)}</p> + * @param p1 first point belonging to the plane + * @param p2 second point belonging to the plane + * @param p3 third point belonging to the plane + * @exception MathArithmeticException if the points do not constitute a plane + * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, Vector3D, Vector3D, double)} + */ + @Deprecated + public Plane(final Vector3D p1, final Vector3D p2, final Vector3D p3) + throws MathArithmeticException { + this(p1, p2, p3, DEFAULT_TOLERANCE); + } + + /** Copy constructor. + * <p>The instance created is completely independant of the original + * one. A deep copy is used, none of the underlying object are + * shared.</p> + * @param plane plane to copy + */ + public Plane(final Plane plane) { + originOffset = plane.originOffset; + origin = plane.origin; + u = plane.u; + v = plane.v; + w = plane.w; + tolerance = plane.tolerance; + } + + /** Copy the instance. + * <p>The instance created is completely independant of the original + * one. A deep copy is used, none of the underlying objects are + * shared (except for immutable objects).</p> + * @return a new hyperplane, copy of the instance + */ + public Plane copySelf() { + return new Plane(this); + } + + /** Reset the instance as if built from a point and a normal. + * @param p point belonging to the plane + * @param normal normal direction to the plane + * @exception MathArithmeticException if the normal norm is too small + */ + public void reset(final Vector3D p, final Vector3D normal) throws MathArithmeticException { + setNormal(normal); + originOffset = -p.dotProduct(w); + setFrame(); + } + + /** Reset the instance from another one. + * <p>The updated instance is completely independant of the original + * one. A deep reset is used none of the underlying object is + * shared.</p> + * @param original plane to reset from + */ + public void reset(final Plane original) { + originOffset = original.originOffset; + origin = original.origin; + u = original.u; + v = original.v; + w = original.w; + } + + /** Set the normal vactor. + * @param normal normal direction to the plane (will be copied) + * @exception MathArithmeticException if the normal norm is too small + */ + private void setNormal(final Vector3D normal) throws MathArithmeticException { + final double norm = normal.getNorm(); + if (norm < 1.0e-10) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + w = new Vector3D(1.0 / norm, normal); + } + + /** Reset the plane frame. + */ + private void setFrame() { + origin = new Vector3D(-originOffset, w); + u = w.orthogonal(); + v = Vector3D.crossProduct(w, u); + } + + /** Get the origin point of the plane frame. + * <p>The point returned is the orthogonal projection of the + * 3D-space origin in the plane.</p> + * @return the origin point of the plane frame (point closest to the + * 3D-space origin) + */ + public Vector3D getOrigin() { + return origin; + } + + /** Get the normalized normal vector. + * <p>The frame defined by ({@link #getU getU}, {@link #getV getV}, + * {@link #getNormal getNormal}) is a rigth-handed orthonormalized + * frame).</p> + * @return normalized normal vector + * @see #getU + * @see #getV + */ + public Vector3D getNormal() { + return w; + } + + /** Get the plane first canonical vector. + * <p>The frame defined by ({@link #getU getU}, {@link #getV getV}, + * {@link #getNormal getNormal}) is a rigth-handed orthonormalized + * frame).</p> + * @return normalized first canonical vector + * @see #getV + * @see #getNormal + */ + public Vector3D getU() { + return u; + } + + /** Get the plane second canonical vector. + * <p>The frame defined by ({@link #getU getU}, {@link #getV getV}, + * {@link #getNormal getNormal}) is a rigth-handed orthonormalized + * frame).</p> + * @return normalized second canonical vector + * @see #getU + * @see #getNormal + */ + public Vector3D getV() { + return v; + } + + /** {@inheritDoc} + * @since 3.3 + */ + public Point<Euclidean3D> project(Point<Euclidean3D> point) { + return toSpace(toSubSpace(point)); + } + + /** {@inheritDoc} + * @since 3.3 + */ + public double getTolerance() { + return tolerance; + } + + /** Revert the plane. + * <p>Replace the instance by a similar plane with opposite orientation.</p> + * <p>The new plane frame is chosen in such a way that a 3D point that had + * {@code (x, y)} in-plane coordinates and {@code z} offset with + * respect to the plane and is unaffected by the change will have + * {@code (y, x)} in-plane coordinates and {@code -z} offset with + * respect to the new plane. This means that the {@code u} and {@code v} + * vectors returned by the {@link #getU} and {@link #getV} methods are exchanged, + * and the {@code w} vector returned by the {@link #getNormal} method is + * reversed.</p> + */ + public void revertSelf() { + final Vector3D tmp = u; + u = v; + v = tmp; + w = w.negate(); + originOffset = -originOffset; + } + + /** Transform a space point into a sub-space point. + * @param vector n-dimension point of the space + * @return (n-1)-dimension point of the sub-space corresponding to + * the specified space point + */ + public Vector2D toSubSpace(Vector<Euclidean3D> vector) { + return toSubSpace((Point<Euclidean3D>) vector); + } + + /** Transform a sub-space point into a space point. + * @param vector (n-1)-dimension point of the sub-space + * @return n-dimension point of the space corresponding to the + * specified sub-space point + */ + public Vector3D toSpace(Vector<Euclidean2D> vector) { + return toSpace((Point<Euclidean2D>) vector); + } + + /** Transform a 3D space point into an in-plane point. + * @param point point of the space (must be a {@link Vector3D + * Vector3D} instance) + * @return in-plane point (really a {@link + * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance) + * @see #toSpace + */ + public Vector2D toSubSpace(final Point<Euclidean3D> point) { + final Vector3D p3D = (Vector3D) point; + return new Vector2D(p3D.dotProduct(u), p3D.dotProduct(v)); + } + + /** Transform an in-plane point into a 3D space point. + * @param point in-plane point (must be a {@link + * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance) + * @return 3D space point (really a {@link Vector3D Vector3D} instance) + * @see #toSubSpace + */ + public Vector3D toSpace(final Point<Euclidean2D> point) { + final Vector2D p2D = (Vector2D) point; + return new Vector3D(p2D.getX(), u, p2D.getY(), v, -originOffset, w); + } + + /** Get one point from the 3D-space. + * @param inPlane desired in-plane coordinates for the point in the + * plane + * @param offset desired offset for the point + * @return one point in the 3D-space, with given coordinates and offset + * relative to the plane + */ + public Vector3D getPointAt(final Vector2D inPlane, final double offset) { + return new Vector3D(inPlane.getX(), u, inPlane.getY(), v, offset - originOffset, w); + } + + /** Check if the instance is similar to another plane. + * <p>Planes are considered similar if they contain the same + * points. This does not mean they are equal since they can have + * opposite normals.</p> + * @param plane plane to which the instance is compared + * @return true if the planes are similar + */ + public boolean isSimilarTo(final Plane plane) { + final double angle = Vector3D.angle(w, plane.w); + return ((angle < 1.0e-10) && (FastMath.abs(originOffset - plane.originOffset) < tolerance)) || + ((angle > (FastMath.PI - 1.0e-10)) && (FastMath.abs(originOffset + plane.originOffset) < tolerance)); + } + + /** Rotate the plane around the specified point. + * <p>The instance is not modified, a new instance is created.</p> + * @param center rotation center + * @param rotation vectorial rotation operator + * @return a new plane + */ + public Plane rotate(final Vector3D center, final Rotation rotation) { + + final Vector3D delta = origin.subtract(center); + final Plane plane = new Plane(center.add(rotation.applyTo(delta)), + rotation.applyTo(w), tolerance); + + // make sure the frame is transformed as desired + plane.u = rotation.applyTo(u); + plane.v = rotation.applyTo(v); + + return plane; + + } + + /** Translate the plane by the specified amount. + * <p>The instance is not modified, a new instance is created.</p> + * @param translation translation to apply + * @return a new plane + */ + public Plane translate(final Vector3D translation) { + + final Plane plane = new Plane(origin.add(translation), w, tolerance); + + // make sure the frame is transformed as desired + plane.u = u; + plane.v = v; + + return plane; + + } + + /** Get the intersection of a line with the instance. + * @param line line intersecting the instance + * @return intersection point between between the line and the + * instance (null if the line is parallel to the instance) + */ + public Vector3D intersection(final Line line) { + final Vector3D direction = line.getDirection(); + final double dot = w.dotProduct(direction); + if (FastMath.abs(dot) < 1.0e-10) { + return null; + } + final Vector3D point = line.toSpace((Point<Euclidean1D>) Vector1D.ZERO); + final double k = -(originOffset + w.dotProduct(point)) / dot; + return new Vector3D(1.0, point, k, direction); + } + + /** Build the line shared by the instance and another plane. + * @param other other plane + * @return line at the intersection of the instance and the + * other plane (really a {@link Line Line} instance) + */ + public Line intersection(final Plane other) { + final Vector3D direction = Vector3D.crossProduct(w, other.w); + if (direction.getNorm() < tolerance) { + return null; + } + final Vector3D point = intersection(this, other, new Plane(direction, tolerance)); + return new Line(point, point.add(direction), tolerance); + } + + /** Get the intersection point of three planes. + * @param plane1 first plane1 + * @param plane2 second plane2 + * @param plane3 third plane2 + * @return intersection point of three planes, null if some planes are parallel + */ + public static Vector3D intersection(final Plane plane1, final Plane plane2, final Plane plane3) { + + // coefficients of the three planes linear equations + final double a1 = plane1.w.getX(); + final double b1 = plane1.w.getY(); + final double c1 = plane1.w.getZ(); + final double d1 = plane1.originOffset; + + final double a2 = plane2.w.getX(); + final double b2 = plane2.w.getY(); + final double c2 = plane2.w.getZ(); + final double d2 = plane2.originOffset; + + final double a3 = plane3.w.getX(); + final double b3 = plane3.w.getY(); + final double c3 = plane3.w.getZ(); + final double d3 = plane3.originOffset; + + // direct Cramer resolution of the linear system + // (this is still feasible for a 3x3 system) + final double a23 = b2 * c3 - b3 * c2; + final double b23 = c2 * a3 - c3 * a2; + final double c23 = a2 * b3 - a3 * b2; + final double determinant = a1 * a23 + b1 * b23 + c1 * c23; + if (FastMath.abs(determinant) < 1.0e-10) { + return null; + } + + final double r = 1.0 / determinant; + return new Vector3D( + (-a23 * d1 - (c1 * b3 - c3 * b1) * d2 - (c2 * b1 - c1 * b2) * d3) * r, + (-b23 * d1 - (c3 * a1 - c1 * a3) * d2 - (c1 * a2 - c2 * a1) * d3) * r, + (-c23 * d1 - (b1 * a3 - b3 * a1) * d2 - (b2 * a1 - b1 * a2) * d3) * r); + + } + + /** Build a region covering the whole hyperplane. + * @return a region covering the whole hyperplane + */ + public SubPlane wholeHyperplane() { + return new SubPlane(this, new PolygonsSet(tolerance)); + } + + /** Build a region covering the whole space. + * @return a region containing the instance (really a {@link + * PolyhedronsSet PolyhedronsSet} instance) + */ + public PolyhedronsSet wholeSpace() { + return new PolyhedronsSet(tolerance); + } + + /** Check if the instance contains a point. + * @param p point to check + * @return true if p belongs to the plane + */ + public boolean contains(final Vector3D p) { + return FastMath.abs(getOffset(p)) < tolerance; + } + + /** Get the offset (oriented distance) of a parallel plane. + * <p>This method should be called only for parallel planes otherwise + * the result is not meaningful.</p> + * <p>The offset is 0 if both planes are the same, it is + * positive if the plane is on the plus side of the instance and + * negative if it is on the minus side, according to its natural + * orientation.</p> + * @param plane plane to check + * @return offset of the plane + */ + public double getOffset(final Plane plane) { + return originOffset + (sameOrientationAs(plane) ? -plane.originOffset : plane.originOffset); + } + + /** Get the offset (oriented distance) of a vector. + * @param vector vector to check + * @return offset of the vector + */ + public double getOffset(Vector<Euclidean3D> vector) { + return getOffset((Point<Euclidean3D>) vector); + } + + /** Get the offset (oriented distance) of a point. + * <p>The offset is 0 if the point is on the underlying hyperplane, + * it is positive if the point is on one particular side of the + * hyperplane, and it is negative if the point is on the other side, + * according to the hyperplane natural orientation.</p> + * @param point point to check + * @return offset of the point + */ + public double getOffset(final Point<Euclidean3D> point) { + return ((Vector3D) point).dotProduct(w) + originOffset; + } + + /** Check if the instance has the same orientation as another hyperplane. + * @param other other hyperplane to check against the instance + * @return true if the instance and the other hyperplane have + * the same orientation + */ + public boolean sameOrientationAs(final Hyperplane<Euclidean3D> other) { + return (((Plane) other).w).dotProduct(w) > 0.0; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/PolyhedronsSet.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/PolyhedronsSet.java new file mode 100644 index 0000000..f190e22 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/PolyhedronsSet.java @@ -0,0 +1,739 @@ +/* + * 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.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.List; + +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.NumberIsTooSmallException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet; +import org.apache.commons.math3.geometry.euclidean.twod.SubLine; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.partitioning.AbstractRegion; +import org.apache.commons.math3.geometry.partitioning.BSPTree; +import org.apache.commons.math3.geometry.partitioning.BSPTreeVisitor; +import org.apache.commons.math3.geometry.partitioning.BoundaryAttribute; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.Region; +import org.apache.commons.math3.geometry.partitioning.RegionFactory; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.geometry.partitioning.Transform; +import org.apache.commons.math3.util.FastMath; + +/** This class represents a 3D region: a set of polyhedrons. + * @since 3.0 + */ +public class PolyhedronsSet extends AbstractRegion<Euclidean3D, Euclidean2D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Build a polyhedrons set representing the whole real line. + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolyhedronsSet(final double tolerance) { + super(tolerance); + } + + /** Build a polyhedrons set from a BSP tree. + * <p>The leaf nodes of the BSP tree <em>must</em> have a + * {@code Boolean} attribute representing the inside status of + * the corresponding cell (true for inside cells, false for outside + * cells). In order to avoid building too many small objects, it is + * recommended to use the predefined constants + * {@code Boolean.TRUE} and {@code Boolean.FALSE}</p> + * <p> + * This constructor is aimed at expert use, as building the tree may + * be a difficult task. It is not intended for general use and for + * performances reasons does not check thoroughly its input, as this would + * require walking the full tree each time. Failing to provide a tree with + * the proper attributes, <em>will</em> therefore generate problems like + * {@link NullPointerException} or {@link ClassCastException} only later on. + * This limitation is known and explains why this constructor is for expert + * use only. The caller does have the responsibility to provided correct arguments. + * </p> + * @param tree inside/outside BSP tree representing the region + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolyhedronsSet(final BSPTree<Euclidean3D> tree, final double tolerance) { + super(tree, tolerance); + } + + /** Build a polyhedrons set from a Boundary REPresentation (B-rep) specified by sub-hyperplanes. + * <p>The boundary is provided as a collection of {@link + * SubHyperplane sub-hyperplanes}. Each sub-hyperplane has the + * interior part of the region on its minus side and the exterior on + * its plus side.</p> + * <p>The boundary elements can be in any order, and can form + * several non-connected sets (like for example polyhedrons with holes + * or a set of disjoint polyhedrons considered as a whole). In + * fact, the elements do not even need to be connected together + * (their topological connections are not used here). However, if the + * boundary does not really separate an inside open from an outside + * open (open having here its topological meaning), then subsequent + * calls to the {@link Region#checkPoint(Point) checkPoint} method will + * not be meaningful anymore.</p> + * <p>If the boundary is empty, the region will represent the whole + * space.</p> + * @param boundary collection of boundary elements, as a + * collection of {@link SubHyperplane SubHyperplane} objects + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolyhedronsSet(final Collection<SubHyperplane<Euclidean3D>> boundary, + final double tolerance) { + super(boundary, tolerance); + } + + /** Build a polyhedrons set from a Boundary REPresentation (B-rep) specified by connected vertices. + * <p> + * The boundary is provided as a list of vertices and a list of facets. + * Each facet is specified as an integer array containing the arrays vertices + * indices in the vertices list. Each facet normal is oriented by right hand + * rule to the facet vertices list. + * </p> + * <p> + * Some basic sanity checks are performed but not everything is thoroughly + * assessed, so it remains under caller responsibility to ensure the vertices + * and facets are consistent and properly define a polyhedrons set. + * </p> + * @param vertices list of polyhedrons set vertices + * @param facets list of facets, as vertices indices in the vertices list + * @param tolerance tolerance below which points are considered identical + * @exception MathIllegalArgumentException if some basic sanity checks fail + * @since 3.5 + */ + public PolyhedronsSet(final List<Vector3D> vertices, final List<int[]> facets, + final double tolerance) { + super(buildBoundary(vertices, facets, tolerance), tolerance); + } + + /** Build a parallellepipedic box. + * @param xMin low bound along the x direction + * @param xMax high bound along the x direction + * @param yMin low bound along the y direction + * @param yMax high bound along the y direction + * @param zMin low bound along the z direction + * @param zMax high bound along the z direction + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolyhedronsSet(final double xMin, final double xMax, + final double yMin, final double yMax, + final double zMin, final double zMax, + final double tolerance) { + super(buildBoundary(xMin, xMax, yMin, yMax, zMin, zMax, tolerance), tolerance); + } + + /** Build a polyhedrons set representing the whole real line. + * @deprecated as of 3.3, replaced with {@link #PolyhedronsSet(double)} + */ + @Deprecated + public PolyhedronsSet() { + this(DEFAULT_TOLERANCE); + } + + /** Build a polyhedrons set from a BSP tree. + * <p>The leaf nodes of the BSP tree <em>must</em> have a + * {@code Boolean} attribute representing the inside status of + * the corresponding cell (true for inside cells, false for outside + * cells). In order to avoid building too many small objects, it is + * recommended to use the predefined constants + * {@code Boolean.TRUE} and {@code Boolean.FALSE}</p> + * @param tree inside/outside BSP tree representing the region + * @deprecated as of 3.3, replaced with {@link #PolyhedronsSet(BSPTree, double)} + */ + @Deprecated + public PolyhedronsSet(final BSPTree<Euclidean3D> tree) { + this(tree, DEFAULT_TOLERANCE); + } + + /** Build a polyhedrons set from a Boundary REPresentation (B-rep). + * <p>The boundary is provided as a collection of {@link + * SubHyperplane sub-hyperplanes}. Each sub-hyperplane has the + * interior part of the region on its minus side and the exterior on + * its plus side.</p> + * <p>The boundary elements can be in any order, and can form + * several non-connected sets (like for example polyhedrons with holes + * or a set of disjoint polyhedrons considered as a whole). In + * fact, the elements do not even need to be connected together + * (their topological connections are not used here). However, if the + * boundary does not really separate an inside open from an outside + * open (open having here its topological meaning), then subsequent + * calls to the {@link Region#checkPoint(Point) checkPoint} method will + * not be meaningful anymore.</p> + * <p>If the boundary is empty, the region will represent the whole + * space.</p> + * @param boundary collection of boundary elements, as a + * collection of {@link SubHyperplane SubHyperplane} objects + * @deprecated as of 3.3, replaced with {@link #PolyhedronsSet(Collection, double)} + */ + @Deprecated + public PolyhedronsSet(final Collection<SubHyperplane<Euclidean3D>> boundary) { + this(boundary, DEFAULT_TOLERANCE); + } + + /** Build a parallellepipedic box. + * @param xMin low bound along the x direction + * @param xMax high bound along the x direction + * @param yMin low bound along the y direction + * @param yMax high bound along the y direction + * @param zMin low bound along the z direction + * @param zMax high bound along the z direction + * @deprecated as of 3.3, replaced with {@link #PolyhedronsSet(double, double, + * double, double, double, double, double)} + */ + @Deprecated + public PolyhedronsSet(final double xMin, final double xMax, + final double yMin, final double yMax, + final double zMin, final double zMax) { + this(xMin, xMax, yMin, yMax, zMin, zMax, DEFAULT_TOLERANCE); + } + + /** Build a parallellepipedic box boundary. + * @param xMin low bound along the x direction + * @param xMax high bound along the x direction + * @param yMin low bound along the y direction + * @param yMax high bound along the y direction + * @param zMin low bound along the z direction + * @param zMax high bound along the z direction + * @param tolerance tolerance below which points are considered identical + * @return boundary tree + * @since 3.3 + */ + private static BSPTree<Euclidean3D> buildBoundary(final double xMin, final double xMax, + final double yMin, final double yMax, + final double zMin, final double zMax, + final double tolerance) { + if ((xMin >= xMax - tolerance) || (yMin >= yMax - tolerance) || (zMin >= zMax - tolerance)) { + // too thin box, build an empty polygons set + return new BSPTree<Euclidean3D>(Boolean.FALSE); + } + final Plane pxMin = new Plane(new Vector3D(xMin, 0, 0), Vector3D.MINUS_I, tolerance); + final Plane pxMax = new Plane(new Vector3D(xMax, 0, 0), Vector3D.PLUS_I, tolerance); + final Plane pyMin = new Plane(new Vector3D(0, yMin, 0), Vector3D.MINUS_J, tolerance); + final Plane pyMax = new Plane(new Vector3D(0, yMax, 0), Vector3D.PLUS_J, tolerance); + final Plane pzMin = new Plane(new Vector3D(0, 0, zMin), Vector3D.MINUS_K, tolerance); + final Plane pzMax = new Plane(new Vector3D(0, 0, zMax), Vector3D.PLUS_K, tolerance); + @SuppressWarnings("unchecked") + final Region<Euclidean3D> boundary = + new RegionFactory<Euclidean3D>().buildConvex(pxMin, pxMax, pyMin, pyMax, pzMin, pzMax); + return boundary.getTree(false); + } + + /** Build boundary from vertices and facets. + * @param vertices list of polyhedrons set vertices + * @param facets list of facets, as vertices indices in the vertices list + * @param tolerance tolerance below which points are considered identical + * @return boundary as a list of sub-hyperplanes + * @exception MathIllegalArgumentException if some basic sanity checks fail + * @since 3.5 + */ + private static List<SubHyperplane<Euclidean3D>> buildBoundary(final List<Vector3D> vertices, + final List<int[]> facets, + final double tolerance) { + + // check vertices distances + for (int i = 0; i < vertices.size() - 1; ++i) { + final Vector3D vi = vertices.get(i); + for (int j = i + 1; j < vertices.size(); ++j) { + if (Vector3D.distance(vi, vertices.get(j)) <= tolerance) { + throw new MathIllegalArgumentException(LocalizedFormats.CLOSE_VERTICES, + vi.getX(), vi.getY(), vi.getZ()); + } + } + } + + // find how vertices are referenced by facets + final int[][] references = findReferences(vertices, facets); + + // find how vertices are linked together by edges along the facets they belong to + final int[][] successors = successors(vertices, facets, references); + + // check edges orientations + for (int vA = 0; vA < vertices.size(); ++vA) { + for (final int vB : successors[vA]) { + + if (vB >= 0) { + // when facets are properly oriented, if vB is the successor of vA on facet f1, + // then there must be an adjacent facet f2 where vA is the successor of vB + boolean found = false; + for (final int v : successors[vB]) { + found = found || (v == vA); + } + if (!found) { + final Vector3D start = vertices.get(vA); + final Vector3D end = vertices.get(vB); + throw new MathIllegalArgumentException(LocalizedFormats.EDGE_CONNECTED_TO_ONE_FACET, + start.getX(), start.getY(), start.getZ(), + end.getX(), end.getY(), end.getZ()); + } + } + } + } + + final List<SubHyperplane<Euclidean3D>> boundary = new ArrayList<SubHyperplane<Euclidean3D>>(); + + for (final int[] facet : facets) { + + // define facet plane from the first 3 points + Plane plane = new Plane(vertices.get(facet[0]), vertices.get(facet[1]), vertices.get(facet[2]), + tolerance); + + // check all points are in the plane + final Vector2D[] two2Points = new Vector2D[facet.length]; + for (int i = 0 ; i < facet.length; ++i) { + final Vector3D v = vertices.get(facet[i]); + if (!plane.contains(v)) { + throw new MathIllegalArgumentException(LocalizedFormats.OUT_OF_PLANE, + v.getX(), v.getY(), v.getZ()); + } + two2Points[i] = plane.toSubSpace(v); + } + + // create the polygonal facet + boundary.add(new SubPlane(plane, new PolygonsSet(tolerance, two2Points))); + + } + + return boundary; + + } + + /** Find the facets that reference each edges. + * @param vertices list of polyhedrons set vertices + * @param facets list of facets, as vertices indices in the vertices list + * @return references array such that r[v][k] = f for some k if facet f contains vertex v + * @exception MathIllegalArgumentException if some facets have fewer than 3 vertices + * @since 3.5 + */ + private static int[][] findReferences(final List<Vector3D> vertices, final List<int[]> facets) { + + // find the maximum number of facets a vertex belongs to + final int[] nbFacets = new int[vertices.size()]; + int maxFacets = 0; + for (final int[] facet : facets) { + if (facet.length < 3) { + throw new NumberIsTooSmallException(LocalizedFormats.WRONG_NUMBER_OF_POINTS, + 3, facet.length, true); + } + for (final int index : facet) { + maxFacets = FastMath.max(maxFacets, ++nbFacets[index]); + } + } + + // set up the references array + final int[][] references = new int[vertices.size()][maxFacets]; + for (int[] r : references) { + Arrays.fill(r, -1); + } + for (int f = 0; f < facets.size(); ++f) { + for (final int v : facets.get(f)) { + // vertex v is referenced by facet f + int k = 0; + while (k < maxFacets && references[v][k] >= 0) { + ++k; + } + references[v][k] = f; + } + } + + return references; + + } + + /** Find the successors of all vertices among all facets they belong to. + * @param vertices list of polyhedrons set vertices + * @param facets list of facets, as vertices indices in the vertices list + * @param references facets references array + * @return indices of vertices that follow vertex v in some facet (the array + * may contain extra entries at the end, set to negative indices) + * @exception MathIllegalArgumentException if the same vertex appears more than + * once in the successors list (which means one facet orientation is wrong) + * @since 3.5 + */ + private static int[][] successors(final List<Vector3D> vertices, final List<int[]> facets, + final int[][] references) { + + // create an array large enough + final int[][] successors = new int[vertices.size()][references[0].length]; + for (final int[] s : successors) { + Arrays.fill(s, -1); + } + + for (int v = 0; v < vertices.size(); ++v) { + for (int k = 0; k < successors[v].length && references[v][k] >= 0; ++k) { + + // look for vertex v + final int[] facet = facets.get(references[v][k]); + int i = 0; + while (i < facet.length && facet[i] != v) { + ++i; + } + + // we have found vertex v, we deduce its successor on current facet + successors[v][k] = facet[(i + 1) % facet.length]; + for (int l = 0; l < k; ++l) { + if (successors[v][l] == successors[v][k]) { + final Vector3D start = vertices.get(v); + final Vector3D end = vertices.get(successors[v][k]); + throw new MathIllegalArgumentException(LocalizedFormats.FACET_ORIENTATION_MISMATCH, + start.getX(), start.getY(), start.getZ(), + end.getX(), end.getY(), end.getZ()); + } + } + + } + } + + return successors; + + } + + /** {@inheritDoc} */ + @Override + public PolyhedronsSet buildNew(final BSPTree<Euclidean3D> tree) { + return new PolyhedronsSet(tree, getTolerance()); + } + + /** {@inheritDoc} */ + @Override + protected void computeGeometricalProperties() { + + // compute the contribution of all boundary facets + getTree(true).visit(new FacetsContributionVisitor()); + + if (getSize() < 0) { + // the polyhedrons set as a finite outside + // surrounded by an infinite inside + setSize(Double.POSITIVE_INFINITY); + setBarycenter((Point<Euclidean3D>) Vector3D.NaN); + } else { + // the polyhedrons set is finite, apply the remaining scaling factors + setSize(getSize() / 3.0); + setBarycenter((Point<Euclidean3D>) new Vector3D(1.0 / (4 * getSize()), (Vector3D) getBarycenter())); + } + + } + + /** Visitor computing geometrical properties. */ + private class FacetsContributionVisitor implements BSPTreeVisitor<Euclidean3D> { + + /** Simple constructor. */ + FacetsContributionVisitor() { + setSize(0); + setBarycenter((Point<Euclidean3D>) new Vector3D(0, 0, 0)); + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<Euclidean3D> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<Euclidean3D> node) { + @SuppressWarnings("unchecked") + final BoundaryAttribute<Euclidean3D> attribute = + (BoundaryAttribute<Euclidean3D>) node.getAttribute(); + if (attribute.getPlusOutside() != null) { + addContribution(attribute.getPlusOutside(), false); + } + if (attribute.getPlusInside() != null) { + addContribution(attribute.getPlusInside(), true); + } + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<Euclidean3D> node) { + } + + /** Add he contribution of a boundary facet. + * @param facet boundary facet + * @param reversed if true, the facet has the inside on its plus side + */ + private void addContribution(final SubHyperplane<Euclidean3D> facet, final boolean reversed) { + + final Region<Euclidean2D> polygon = ((SubPlane) facet).getRemainingRegion(); + final double area = polygon.getSize(); + + if (Double.isInfinite(area)) { + setSize(Double.POSITIVE_INFINITY); + setBarycenter((Point<Euclidean3D>) Vector3D.NaN); + } else { + + final Plane plane = (Plane) facet.getHyperplane(); + final Vector3D facetB = plane.toSpace(polygon.getBarycenter()); + double scaled = area * facetB.dotProduct(plane.getNormal()); + if (reversed) { + scaled = -scaled; + } + + setSize(getSize() + scaled); + setBarycenter((Point<Euclidean3D>) new Vector3D(1.0, (Vector3D) getBarycenter(), scaled, facetB)); + + } + + } + + } + + /** Get the first sub-hyperplane crossed by a semi-infinite line. + * @param point start point of the part of the line considered + * @param line line to consider (contains point) + * @return the first sub-hyperplane crossed by the line after the + * given point, or null if the line does not intersect any + * sub-hyperplane + */ + public SubHyperplane<Euclidean3D> firstIntersection(final Vector3D point, final Line line) { + return recurseFirstIntersection(getTree(true), point, line); + } + + /** Get the first sub-hyperplane crossed by a semi-infinite line. + * @param node current node + * @param point start point of the part of the line considered + * @param line line to consider (contains point) + * @return the first sub-hyperplane crossed by the line after the + * given point, or null if the line does not intersect any + * sub-hyperplane + */ + private SubHyperplane<Euclidean3D> recurseFirstIntersection(final BSPTree<Euclidean3D> node, + final Vector3D point, + final Line line) { + + final SubHyperplane<Euclidean3D> cut = node.getCut(); + if (cut == null) { + return null; + } + final BSPTree<Euclidean3D> minus = node.getMinus(); + final BSPTree<Euclidean3D> plus = node.getPlus(); + final Plane plane = (Plane) cut.getHyperplane(); + + // establish search order + final double offset = plane.getOffset((Point<Euclidean3D>) point); + final boolean in = FastMath.abs(offset) < getTolerance(); + final BSPTree<Euclidean3D> near; + final BSPTree<Euclidean3D> far; + if (offset < 0) { + near = minus; + far = plus; + } else { + near = plus; + far = minus; + } + + if (in) { + // search in the cut hyperplane + final SubHyperplane<Euclidean3D> facet = boundaryFacet(point, node); + if (facet != null) { + return facet; + } + } + + // search in the near branch + final SubHyperplane<Euclidean3D> crossed = recurseFirstIntersection(near, point, line); + if (crossed != null) { + return crossed; + } + + if (!in) { + // search in the cut hyperplane + final Vector3D hit3D = plane.intersection(line); + if (hit3D != null && line.getAbscissa(hit3D) > line.getAbscissa(point)) { + final SubHyperplane<Euclidean3D> facet = boundaryFacet(hit3D, node); + if (facet != null) { + return facet; + } + } + } + + // search in the far branch + return recurseFirstIntersection(far, point, line); + + } + + /** Check if a point belongs to the boundary part of a node. + * @param point point to check + * @param node node containing the boundary facet to check + * @return the boundary facet this points belongs to (or null if it + * does not belong to any boundary facet) + */ + private SubHyperplane<Euclidean3D> boundaryFacet(final Vector3D point, + final BSPTree<Euclidean3D> node) { + final Vector2D point2D = ((Plane) node.getCut().getHyperplane()).toSubSpace((Point<Euclidean3D>) point); + @SuppressWarnings("unchecked") + final BoundaryAttribute<Euclidean3D> attribute = + (BoundaryAttribute<Euclidean3D>) node.getAttribute(); + if ((attribute.getPlusOutside() != null) && + (((SubPlane) attribute.getPlusOutside()).getRemainingRegion().checkPoint(point2D) == Location.INSIDE)) { + return attribute.getPlusOutside(); + } + if ((attribute.getPlusInside() != null) && + (((SubPlane) attribute.getPlusInside()).getRemainingRegion().checkPoint(point2D) == Location.INSIDE)) { + return attribute.getPlusInside(); + } + return null; + } + + /** Rotate the region around the specified point. + * <p>The instance is not modified, a new instance is created.</p> + * @param center rotation center + * @param rotation vectorial rotation operator + * @return a new instance representing the rotated region + */ + public PolyhedronsSet rotate(final Vector3D center, final Rotation rotation) { + return (PolyhedronsSet) applyTransform(new RotationTransform(center, rotation)); + } + + /** 3D rotation as a Transform. */ + private static class RotationTransform implements Transform<Euclidean3D, Euclidean2D> { + + /** Center point of the rotation. */ + private Vector3D center; + + /** Vectorial rotation. */ + private Rotation rotation; + + /** Cached original hyperplane. */ + private Plane cachedOriginal; + + /** Cached 2D transform valid inside the cached original hyperplane. */ + private Transform<Euclidean2D, Euclidean1D> cachedTransform; + + /** Build a rotation transform. + * @param center center point of the rotation + * @param rotation vectorial rotation + */ + RotationTransform(final Vector3D center, final Rotation rotation) { + this.center = center; + this.rotation = rotation; + } + + /** {@inheritDoc} */ + public Vector3D apply(final Point<Euclidean3D> point) { + final Vector3D delta = ((Vector3D) point).subtract(center); + return new Vector3D(1.0, center, 1.0, rotation.applyTo(delta)); + } + + /** {@inheritDoc} */ + public Plane apply(final Hyperplane<Euclidean3D> hyperplane) { + return ((Plane) hyperplane).rotate(center, rotation); + } + + /** {@inheritDoc} */ + public SubHyperplane<Euclidean2D> apply(final SubHyperplane<Euclidean2D> sub, + final Hyperplane<Euclidean3D> original, + final Hyperplane<Euclidean3D> transformed) { + if (original != cachedOriginal) { + // we have changed hyperplane, reset the in-hyperplane transform + + final Plane oPlane = (Plane) original; + final Plane tPlane = (Plane) transformed; + final Vector3D p00 = oPlane.getOrigin(); + final Vector3D p10 = oPlane.toSpace((Point<Euclidean2D>) new Vector2D(1.0, 0.0)); + final Vector3D p01 = oPlane.toSpace((Point<Euclidean2D>) new Vector2D(0.0, 1.0)); + final Vector2D tP00 = tPlane.toSubSpace((Point<Euclidean3D>) apply(p00)); + final Vector2D tP10 = tPlane.toSubSpace((Point<Euclidean3D>) apply(p10)); + final Vector2D tP01 = tPlane.toSubSpace((Point<Euclidean3D>) apply(p01)); + + cachedOriginal = (Plane) original; + cachedTransform = + org.apache.commons.math3.geometry.euclidean.twod.Line.getTransform(tP10.getX() - tP00.getX(), + tP10.getY() - tP00.getY(), + tP01.getX() - tP00.getX(), + tP01.getY() - tP00.getY(), + tP00.getX(), + tP00.getY()); + + } + return ((SubLine) sub).applyTransform(cachedTransform); + } + + } + + /** Translate the region by the specified amount. + * <p>The instance is not modified, a new instance is created.</p> + * @param translation translation to apply + * @return a new instance representing the translated region + */ + public PolyhedronsSet translate(final Vector3D translation) { + return (PolyhedronsSet) applyTransform(new TranslationTransform(translation)); + } + + /** 3D translation as a transform. */ + private static class TranslationTransform implements Transform<Euclidean3D, Euclidean2D> { + + /** Translation vector. */ + private Vector3D translation; + + /** Cached original hyperplane. */ + private Plane cachedOriginal; + + /** Cached 2D transform valid inside the cached original hyperplane. */ + private Transform<Euclidean2D, Euclidean1D> cachedTransform; + + /** Build a translation transform. + * @param translation translation vector + */ + TranslationTransform(final Vector3D translation) { + this.translation = translation; + } + + /** {@inheritDoc} */ + public Vector3D apply(final Point<Euclidean3D> point) { + return new Vector3D(1.0, (Vector3D) point, 1.0, translation); + } + + /** {@inheritDoc} */ + public Plane apply(final Hyperplane<Euclidean3D> hyperplane) { + return ((Plane) hyperplane).translate(translation); + } + + /** {@inheritDoc} */ + public SubHyperplane<Euclidean2D> apply(final SubHyperplane<Euclidean2D> sub, + final Hyperplane<Euclidean3D> original, + final Hyperplane<Euclidean3D> transformed) { + if (original != cachedOriginal) { + // we have changed hyperplane, reset the in-hyperplane transform + + final Plane oPlane = (Plane) original; + final Plane tPlane = (Plane) transformed; + final Vector2D shift = tPlane.toSubSpace((Point<Euclidean3D>) apply(oPlane.getOrigin())); + + cachedOriginal = (Plane) original; + cachedTransform = + org.apache.commons.math3.geometry.euclidean.twod.Line.getTransform(1, 0, 0, 1, + shift.getX(), + shift.getY()); + + } + + return ((SubLine) sub).applyTransform(cachedTransform); + + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java new file mode 100644 index 0000000..f4df3b5 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java @@ -0,0 +1,1424 @@ +/* + * 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.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 implements rotations in a three-dimensional space. + * + * <p>Rotations can be represented by several different mathematical + * entities (matrices, axe and angle, Cardan or Euler angles, + * quaternions). This class presents an higher level abstraction, more + * user-oriented and hiding this implementation details. Well, for the + * curious, we use quaternions for the internal representation. The + * user can build a rotation from any of these representations, and + * any of these representations can be retrieved from a + * <code>Rotation</code> instance (see the various constructors and + * getters). In addition, a rotation can also be built implicitly + * from a set of vectors and their image.</p> + * <p>This implies that this class can be used to convert from one + * representation to another one. For example, converting a rotation + * matrix into a set of Cardan angles from can be done using the + * following single line of code:</p> + * <pre> + * double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ); + * </pre> + * <p>Focus is oriented on what a rotation <em>do</em> rather than on its + * underlying representation. Once it has been built, and regardless of its + * internal representation, a rotation is an <em>operator</em> which basically + * transforms three dimensional {@link Vector3D vectors} into other three + * dimensional {@link Vector3D vectors}. Depending on the application, the + * meaning of these vectors may vary and the semantics of the rotation also.</p> + * <p>For example in an spacecraft attitude simulation tool, users will often + * consider the vectors are fixed (say the Earth direction for example) and the + * frames change. The rotation transforms the coordinates of the vector in inertial + * frame into the coordinates of the same vector in satellite frame. In this + * case, the rotation implicitly defines the relation between the two frames.</p> + * <p>Another example could be a telescope control application, where the rotation + * would transform the sighting direction at rest into the desired observing + * direction when the telescope is pointed towards an object of interest. In this + * case the rotation transforms the direction at rest in a topocentric frame + * into the sighting direction in the same topocentric frame. This implies in this + * case the frame is fixed and the vector moves.</p> + * <p>In many case, both approaches will be combined. In our telescope example, + * we will probably also need to transform the observing direction in the topocentric + * frame into the observing direction in inertial frame taking into account the observatory + * location and the Earth rotation, which would essentially be an application of the + * first approach.</p> + * + * <p>These examples show that a rotation is what the user wants it to be. This + * class does not push the user towards one specific definition and hence does not + * provide methods like <code>projectVectorIntoDestinationFrame</code> or + * <code>computeTransformedDirection</code>. It provides simpler and more generic + * methods: {@link #applyTo(Vector3D) applyTo(Vector3D)} and {@link + * #applyInverseTo(Vector3D) applyInverseTo(Vector3D)}.</p> + * + * <p>Since a rotation is basically a vectorial operator, several rotations can be + * composed together and the composite operation <code>r = r<sub>1</sub> o + * r<sub>2</sub></code> (which means that for each vector <code>u</code>, + * <code>r(u) = r<sub>1</sub>(r<sub>2</sub>(u))</code>) is also a rotation. Hence + * we can consider that in addition to vectors, a rotation can be applied to other + * rotations as well (or to itself). With our previous notations, we would say we + * can apply <code>r<sub>1</sub></code> to <code>r<sub>2</sub></code> and the result + * we get is <code>r = r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the + * class provides the methods: {@link #applyTo(Rotation) applyTo(Rotation)} and + * {@link #applyInverseTo(Rotation) applyInverseTo(Rotation)}.</p> + * + * <p>Rotations are guaranteed to be immutable objects.</p> + * + * @see Vector3D + * @see RotationOrder + * @since 1.2 + */ + +public class Rotation implements Serializable { + + /** Identity rotation. */ + public static final Rotation IDENTITY = new Rotation(1.0, 0.0, 0.0, 0.0, false); + + /** Serializable version identifier */ + private static final long serialVersionUID = -2153622329907944313L; + + /** Scalar coordinate of the quaternion. */ + private final double q0; + + /** First coordinate of the vectorial part of the quaternion. */ + private final double q1; + + /** Second coordinate of the vectorial part of the quaternion. */ + private final double q2; + + /** Third coordinate of the vectorial part of the quaternion. */ + private final double q3; + + /** Build a rotation from the quaternion coordinates. + * <p>A rotation can be built from a <em>normalized</em> quaternion, + * i.e. a quaternion for which q<sub>0</sub><sup>2</sup> + + * q<sub>1</sub><sup>2</sup> + q<sub>2</sub><sup>2</sup> + + * q<sub>3</sub><sup>2</sup> = 1. If the quaternion is not normalized, + * the constructor can normalize it in a preprocessing step.</p> + * <p>Note that some conventions put the scalar part of the quaternion + * as the 4<sup>th</sup> component and the vector part as the first three + * components. This is <em>not</em> our convention. We put the scalar part + * as the first component.</p> + * @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 Rotation(double q0, double q1, double q2, double q3, + boolean needsNormalization) { + + if (needsNormalization) { + // normalization preprocessing + double inv = 1.0 / FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= inv; + q1 *= inv; + q2 *= inv; + q3 *= inv; + } + + this.q0 = q0; + this.q1 = q1; + this.q2 = q2; + this.q3 = q3; + + } + + /** Build a rotation from an axis and an angle. + * <p> + * Calling this constructor is equivalent to call + * {@link #Rotation(Vector3D, double, RotationConvention) + * new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)} + * </p> + * @param axis axis around which to rotate + * @param angle rotation angle. + * @exception MathIllegalArgumentException if the axis norm is zero + * @deprecated as of 3.6, replaced with {@link #Rotation(Vector3D, double, RotationConvention)} + */ + @Deprecated + public Rotation(Vector3D axis, double angle) throws MathIllegalArgumentException { + this(axis, angle, RotationConvention.VECTOR_OPERATOR); + } + + /** Build a rotation from an axis and an angle. + * @param axis axis around which to rotate + * @param angle rotation angle + * @param convention convention to use for the semantics of the angle + * @exception MathIllegalArgumentException if the axis norm is zero + * @since 3.6 + */ + public Rotation(final Vector3D axis, final double angle, final RotationConvention convention) + throws MathIllegalArgumentException { + + double norm = axis.getNorm(); + if (norm == 0) { + throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); + } + + double halfAngle = convention == RotationConvention.VECTOR_OPERATOR ? -0.5 * angle : +0.5 * angle; + double coeff = FastMath.sin(halfAngle) / norm; + + q0 = FastMath.cos (halfAngle); + q1 = coeff * axis.getX(); + q2 = coeff * axis.getY(); + q3 = coeff * axis.getZ(); + + } + + /** Build a rotation from a 3X3 matrix. + + * <p>Rotation matrices are orthogonal matrices, i.e. unit matrices + * (which are matrices for which m.m<sup>T</sup> = 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.</p> + + * <p>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.<p> + + * @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 Rotation(double[][] m, 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 + double[][] ort = orthogonalizeMatrix(m, threshold); + + // check the sign of the determinant + double det = ort[0][0] * (ort[1][1] * ort[2][2] - ort[2][1] * ort[1][2]) - + ort[1][0] * (ort[0][1] * ort[2][2] - ort[2][1] * ort[0][2]) + + ort[2][0] * (ort[0][1] * ort[1][2] - ort[1][1] * ort[0][2]); + if (det < 0.0) { + throw new NotARotationMatrixException( + LocalizedFormats.CLOSEST_ORTHOGONAL_MATRIX_HAS_NEGATIVE_DETERMINANT, + det); + } + + double[] quat = mat2quat(ort); + q0 = quat[0]; + q1 = quat[1]; + q2 = quat[2]; + q3 = quat[3]; + + } + + /** Build the rotation that transforms a pair of vectors into another pair. + + * <p>Except for possible scale factors, if the instance were applied to + * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair + * (v<sub>1</sub>, v<sub>2</sub>).</p> + + * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is + * not the same as the angular separation between v<sub>1</sub> and + * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than + * v<sub>2</sub>, the corrected vector will be in the (±v<sub>1</sub>, + * +v<sub>2</sub>) half-plane.</p> + + * @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 collinear) + */ + public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) + throws MathArithmeticException { + + // build orthonormalized base from u1, u2 + // this fails when vectors are null or collinear, which is forbidden to define a rotation + final Vector3D 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 collinear, which is forbidden to define a rotation + final Vector3D 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 double[][] m = new double[][] { + { + 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()) + } + }; + + double[] 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. + + * <p>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 collinear, an + * arbitrary rotation axis is chosen.</p> + + * @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 Rotation(Vector3D u, Vector3D v) throws MathArithmeticException { + + double normProduct = u.getNorm() * v.getNorm(); + if (normProduct == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_DEFINING_VECTOR); + } + + double dot = u.dotProduct(v); + + if (dot < ((2.0e-15 - 1.0) * normProduct)) { + // special case u = -v: we select a PI angle rotation around + // an arbitrary vector orthogonal to u + Vector3D w = u.orthogonal(); + q0 = 0.0; + q1 = -w.getX(); + q2 = -w.getY(); + q3 = -w.getZ(); + } else { + // general case: (u, v) defines a plane, we select + // the shortest possible rotation: axis orthogonal to this plane + q0 = FastMath.sqrt(0.5 * (1.0 + dot / normProduct)); + double coeff = 1.0 / (2.0 * q0 * normProduct); + Vector3D q = v.crossProduct(u); + q1 = coeff * q.getX(); + q2 = coeff * q.getY(); + q3 = coeff * q.getZ(); + } + + } + + /** Build a rotation from three Cardan or Euler elementary rotations. + + * <p> + * Calling this constructor is equivalent to call + * {@link #Rotation(RotationOrder, RotationConvention, double, double, double) + * new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)} + * </p> + + * @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 + * @deprecated as of 3.6, replaced with {@link + * #Rotation(RotationOrder, RotationConvention, double, double, double)} + */ + @Deprecated + public Rotation(RotationOrder order, + double alpha1, double alpha2, double alpha3) { + this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3); + } + + /** Build a rotation from three Cardan or Euler elementary rotations. + + * <p>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.</p> + * <p>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).</p> + + * @param order order of rotations to compose, from left to right + * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)}) + * @param convention convention to use for the semantics of the angle + * @param alpha1 angle of the first elementary rotation + * @param alpha2 angle of the second elementary rotation + * @param alpha3 angle of the third elementary rotation + * @since 3.6 + */ + public Rotation(RotationOrder order, RotationConvention convention, + double alpha1, double alpha2, double alpha3) { + Rotation r1 = new Rotation(order.getA1(), alpha1, convention); + Rotation r2 = new Rotation(order.getA2(), alpha2, convention); + Rotation r3 = new Rotation(order.getA3(), alpha3, convention); + Rotation composed = r1.compose(r2.compose(r3, convention), convention); + 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 double[] mat2quat(final double[][] ort) { + + final double[] quat = new double[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) + double s = ort[0][0] + ort[1][1] + ort[2][2]; + if (s > -0.19) { + // compute q0 and deduce q1, q2 and q3 + quat[0] = 0.5 * FastMath.sqrt(s + 1.0); + double inv = 0.25 / quat[0]; + quat[1] = inv * (ort[1][2] - ort[2][1]); + quat[2] = inv * (ort[2][0] - ort[0][2]); + quat[3] = inv * (ort[0][1] - ort[1][0]); + } else { + s = ort[0][0] - ort[1][1] - ort[2][2]; + if (s > -0.19) { + // compute q1 and deduce q0, q2 and q3 + quat[1] = 0.5 * FastMath.sqrt(s + 1.0); + double inv = 0.25 / quat[1]; + quat[0] = inv * (ort[1][2] - ort[2][1]); + quat[2] = inv * (ort[0][1] + ort[1][0]); + quat[3] = inv * (ort[0][2] + ort[2][0]); + } else { + s = ort[1][1] - ort[0][0] - ort[2][2]; + if (s > -0.19) { + // compute q2 and deduce q0, q1 and q3 + quat[2] = 0.5 * FastMath.sqrt(s + 1.0); + double inv = 0.25 / quat[2]; + quat[0] = inv * (ort[2][0] - ort[0][2]); + quat[1] = inv * (ort[0][1] + ort[1][0]); + quat[3] = inv * (ort[2][1] + ort[1][2]); + } else { + // compute q3 and deduce q0, q1 and q2 + s = ort[2][2] - ort[0][0] - ort[1][1]; + quat[3] = 0.5 * FastMath.sqrt(s + 1.0); + double inv = 0.25 / quat[3]; + quat[0] = inv * (ort[0][1] - ort[1][0]); + quat[1] = inv * (ort[0][2] + ort[2][0]); + quat[2] = inv * (ort[2][1] + 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 Rotation revert() { + return new Rotation(-q0, q1, q2, q3, false); + } + + /** Get the scalar coordinate of the quaternion. + * @return scalar coordinate of the quaternion + */ + public double 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 double 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 double 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 double getQ3() { + return q3; + } + + /** Get the normalized axis of the rotation. + * <p> + * Calling this method is equivalent to call + * {@link #getAxis(RotationConvention) getAxis(RotationConvention.VECTOR_OPERATOR)} + * </p> + * @return normalized axis of the rotation + * @see #Rotation(Vector3D, double, RotationConvention) + * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)} + */ + @Deprecated + public Vector3D getAxis() { + return getAxis(RotationConvention.VECTOR_OPERATOR); + } + + /** Get the normalized axis of the rotation. + * <p> + * Note that as {@link #getAngle()} always returns an angle + * between 0 and π, changing the convention changes the + * direction of the axis, not the sign of the angle. + * </p> + * @param convention convention to use for the semantics of the angle + * @return normalized axis of the rotation + * @see #Rotation(Vector3D, double, RotationConvention) + * @since 3.6 + */ + public Vector3D getAxis(final RotationConvention convention) { + final double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; + if (squaredSine == 0) { + return convention == RotationConvention.VECTOR_OPERATOR ? Vector3D.PLUS_I : Vector3D.MINUS_I; + } else { + final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1; + if (q0 < 0) { + final double inverse = sgn / FastMath.sqrt(squaredSine); + return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); + } + final double inverse = -sgn / FastMath.sqrt(squaredSine); + return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); + } + } + + /** Get the angle of the rotation. + * @return angle of the rotation (between 0 and π) + * @see #Rotation(Vector3D, double) + */ + public double getAngle() { + if ((q0 < -0.1) || (q0 > 0.1)) { + return 2 * FastMath.asin(FastMath.sqrt(q1 * q1 + q2 * q2 + q3 * q3)); + } else if (q0 < 0) { + return 2 * FastMath.acos(-q0); + } + return 2 * FastMath.acos(q0); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + * <p> + * Calling this method is equivalent to call + * {@link #getAngles(RotationOrder, RotationConvention) + * getAngles(order, RotationConvention.VECTOR_OPERATOR)} + * </p> + + * @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 + * @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)} + */ + @Deprecated + public double[] getAngles(RotationOrder order) + throws CardanEulerSingularityException { + return getAngles(order, RotationConvention.VECTOR_OPERATOR); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + * <p>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 + * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as + * the rotation defined by the angles π + a<sub>1</sub>, π + * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements + * the following arbitrary choices:</p> + * <ul> + * <li>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),</li> + * <li>for Euler angles, the chosen set is the one for which the + * second angle is between 0 and π (i.e its sine is positive).</li> + * </ul> + + * <p>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 + * <em>nothing</em> 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!</p> + + * @param order rotation order to use + * @param convention convention to use for the semantics of the angle + * @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 + * @since 3.6 + */ + public double[] getAngles(RotationOrder order, RotationConvention convention) + throws CardanEulerSingularityException { + + if (convention == RotationConvention.VECTOR_OPERATOR) { + if (order == RotationOrder.XYZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-(v1.getY()), v1.getZ()), + FastMath.asin(v2.getZ()), + FastMath.atan2(-(v2.getY()), v2.getX()) + }; + + } else if (order == RotationOrder.XZY) { + + // r (Vector3D.plusJ) coordinates are : + // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) + // (-r) (Vector3D.plusI) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v1.getZ(), v1.getY()), + -FastMath.asin(v2.getY()), + FastMath.atan2(v2.getZ(), v2.getX()) + }; + + } else if (order == RotationOrder.YXZ) { + + // r (Vector3D.plusK) coordinates are : + // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) + // (-r) (Vector3D.plusJ) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v1.getX(), v1.getZ()), + -FastMath.asin(v2.getZ()), + FastMath.atan2(v2.getX(), v2.getY()) + }; + + } else if (order == RotationOrder.YZX) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) + // (-r) (Vector3D.plusJ) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-(v1.getZ()), v1.getX()), + FastMath.asin(v2.getX()), + FastMath.atan2(-(v2.getZ()), v2.getY()) + }; + + } else if (order == RotationOrder.ZXY) { + + // r (Vector3D.plusJ) coordinates are : + // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) + // (-r) (Vector3D.plusK) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-(v1.getX()), v1.getY()), + FastMath.asin(v2.getY()), + FastMath.atan2(-(v2.getX()), v2.getZ()) + }; + + } else if (order == RotationOrder.ZYX) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) + // (-r) (Vector3D.plusK) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v1.getY(), v1.getX()), + -FastMath.asin(v2.getX()), + FastMath.atan2(v2.getY(), v2.getZ()) + }; + + } else if (order == RotationOrder.XYX) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) + // and we can choose to have theta in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getY(), -v1.getZ()), + FastMath.acos(v2.getX()), + FastMath.atan2(v2.getY(), v2.getZ()) + }; + + } else if (order == RotationOrder.XZX) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) + // and we can choose to have psi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getZ(), v1.getY()), + FastMath.acos(v2.getX()), + FastMath.atan2(v2.getZ(), -v2.getY()) + }; + + } else if (order == RotationOrder.YXY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // and we can choose to have phi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getX(), v1.getZ()), + FastMath.acos(v2.getY()), + FastMath.atan2(v2.getX(), -v2.getZ()) + }; + + } else if (order == RotationOrder.YZY) { + + // r (Vector3D.plusJ) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // and we can choose to have psi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getZ(), -v1.getX()), + FastMath.acos(v2.getY()), + FastMath.atan2(v2.getZ(), v2.getX()) + }; + + } else if (order == RotationOrder.ZXZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getX(), -v1.getY()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v2.getX(), v2.getY()) + }; + + } else { // last possibility is ZYZ + + // r (Vector3D.plusK) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // (-r) (Vector3D.plusK) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v1.getY(), v1.getX()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v2.getY(), -v2.getX()) + }; + + } + } else { + if (order == RotationOrder.XYZ) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta) + // (-r) (Vector3D.plusK) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-v2.getY(), v2.getZ()), + FastMath.asin(v2.getX()), + FastMath.atan2(-v1.getY(), v1.getX()) + }; + + } else if (order == RotationOrder.XZY) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta) + // (-r) (Vector3D.plusJ) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v2.getZ(), v2.getY()), + -FastMath.asin(v2.getX()), + FastMath.atan2(v1.getZ(), v1.getX()) + }; + + } else if (order == RotationOrder.YXZ) { + + // r (Vector3D.plusJ) coordinates are : + // cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi) + // (-r) (Vector3D.plusK) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v2.getX(), v2.getZ()), + -FastMath.asin(v2.getY()), + FastMath.atan2(v1.getX(), v1.getY()) + }; + + } else if (order == RotationOrder.YZX) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi) + // (-r) (Vector3D.plusI) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-v2.getZ(), v2.getX()), + FastMath.asin(v2.getY()), + FastMath.atan2(-v1.getZ(), v1.getY()) + }; + + } else if (order == RotationOrder.ZXY) { + + // r (Vector3D.plusK) coordinates are : + // -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta) + // (-r) (Vector3D.plusJ) 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] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(-v2.getX(), v2.getY()), + FastMath.asin(v2.getZ()), + FastMath.atan2(-v1.getX(), v1.getZ()) + }; + + } else if (order == RotationOrder.ZYX) { + + // r (Vector3D.plusK) coordinates are : + // -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return new double[] { + FastMath.atan2(v2.getY(), v2.getX()), + -FastMath.asin(v2.getZ()), + FastMath.atan2(v1.getY(), v1.getZ()) + }; + + } else if (order == RotationOrder.XYX) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1) + // and we can choose to have theta in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getY(), -v2.getZ()), + FastMath.acos(v2.getX()), + FastMath.atan2(v1.getY(), v1.getZ()) + }; + + } else if (order == RotationOrder.XZX) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1) + // and we can choose to have psi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_I); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getZ(), v2.getY()), + FastMath.acos(v2.getX()), + FastMath.atan2(v1.getZ(), -v1.getY()) + }; + + } else if (order == RotationOrder.YXY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // and we can choose to have phi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getX(), v2.getZ()), + FastMath.acos(v2.getY()), + FastMath.atan2(v1.getX(), -v1.getZ()) + }; + + } else if (order == RotationOrder.YZY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // and we can choose to have psi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_J); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getZ(), -v2.getX()), + FastMath.acos(v2.getY()), + FastMath.atan2(v1.getZ(), v1.getX()) + }; + + } else if (order == RotationOrder.ZXZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getX(), -v2.getY()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v1.getX(), v1.getY()) + }; + + } else { // last possibility is ZYZ + + // r (Vector3D.plusK) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // (-r) (Vector3D.plusK) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + Vector3D v1 = applyTo(Vector3D.PLUS_K); + Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return new double[] { + FastMath.atan2(v2.getY(), v2.getX()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v1.getY(), -v1.getX()) + }; + + } + } + + } + + /** Get the 3X3 matrix corresponding to the instance + * @return the matrix corresponding to the instance + */ + public double[][] getMatrix() { + + // products + double q0q0 = q0 * q0; + double q0q1 = q0 * q1; + double q0q2 = q0 * q2; + double q0q3 = q0 * q3; + double q1q1 = q1 * q1; + double q1q2 = q1 * q2; + double q1q3 = q1 * q3; + double q2q2 = q2 * q2; + double q2q3 = q2 * q3; + double q3q3 = q3 * q3; + + // create the matrix + double[][] m = new double[3][]; + m[0] = new double[3]; + m[1] = new double[3]; + m[2] = new double[3]; + + m [0][0] = 2.0 * (q0q0 + q1q1) - 1.0; + m [1][0] = 2.0 * (q1q2 - q0q3); + m [2][0] = 2.0 * (q1q3 + q0q2); + + m [0][1] = 2.0 * (q1q2 + q0q3); + m [1][1] = 2.0 * (q0q0 + q2q2) - 1.0; + m [2][1] = 2.0 * (q2q3 - q0q1); + + m [0][2] = 2.0 * (q1q3 - q0q2); + m [1][2] = 2.0 * (q2q3 + q0q1); + m [2][2] = 2.0 * (q0q0 + q3q3) - 1.0; + + 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 Vector3D applyTo(Vector3D u) { + + double x = u.getX(); + double y = u.getY(); + double z = u.getZ(); + + double s = q1 * x + q2 * y + q3 * z; + + return new Vector3D(2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x, + 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y, + 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - 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 double[] in, final double[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final double s = q1 * x + q2 * y + q3 * z; + + out[0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x; + out[1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y; + out[2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - 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 Vector3D applyInverseTo(Vector3D u) { + + double x = u.getX(); + double y = u.getY(); + double z = u.getZ(); + + double s = q1 * x + q2 * y + q3 * z; + double m0 = -q0; + + return new Vector3D(2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x, + 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y, + 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - 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 double[] in, final double[] out) { + + final double x = in[0]; + final double y = in[1]; + final double z = in[2]; + + final double s = q1 * x + q2 * y + q3 * z; + final double m0 = -q0; + + out[0] = 2 * (m0 * (x * m0 - (q2 * z - q3 * y)) + s * q1) - x; + out[1] = 2 * (m0 * (y * m0 - (q3 * x - q1 * z)) + s * q2) - y; + out[2] = 2 * (m0 * (z * m0 - (q1 * y - q2 * x)) + s * q3) - z; + + } + + /** Apply the instance to another rotation. + * <p> + * Calling this method is equivalent to call + * {@link #compose(Rotation, RotationConvention) + * compose(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + */ + public Rotation applyTo(Rotation r) { + return compose(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the instance to a rotation is computing the composition + * in an order compliant with the following rule : let {@code u} be any + * vector and {@code v} its image by {@code r1} (i.e. + * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by + * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then + * {@code w = comp.applyTo(u)}, where + * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the instance + */ + public Rotation compose(final Rotation r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInternal(r) : r.composeInternal(this); + } + + /** Compose the instance with another rotation using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the instance + * using vector operator convention + */ + private Rotation composeInternal(final Rotation r) { + return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), + r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), + r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), + r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), + false); + } + + /** Apply the inverse of the instance to another rotation. + * <p> + * Calling this method is equivalent to call + * {@link #composeInverse(Rotation, RotationConvention) + * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}. + * </p> + * @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 Rotation applyInverseTo(Rotation r) { + return composeInverse(r, RotationConvention.VECTOR_OPERATOR); + } + + /** Compose the inverse of the instance with another rotation. + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention, + * applying the inverse of the instance to a rotation is computing + * the composition in an order compliant with the following rule : + * let {@code u} be any vector and {@code v} its image by {@code r1} + * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image + * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}). + * Then {@code w = comp.applyTo(u)}, where + * {@code comp = r2.composeInverse(r1)}. + * </p> + * <p> + * If the semantics of the rotations composition corresponds to a + * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention, + * the application order will be reversed, which means it is the + * <em>innermost</em> rotation that will be reversed. So keeping the exact same + * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w} + * and {@code comp} as above, {@code comp} could also be computed as + * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}. + * </p> + * @param r rotation to apply the rotation to + * @param convention convention to use for the semantics of the angle + * @return a new rotation which is the composition of r by the inverse + * of the instance + */ + public Rotation composeInverse(final Rotation r, final RotationConvention convention) { + return convention == RotationConvention.VECTOR_OPERATOR ? + composeInverseInternal(r) : r.composeInternal(revert()); + } + + /** Compose the inverse of the instance with another rotation + * using vector operator convention. + * @param r rotation to apply the rotation to + * @return a new rotation which is the composition of r by the inverse + * of the instance using vector operator convention + */ + private Rotation composeInverseInternal(Rotation r) { + return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3), + -r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2), + -r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), + -r.q3 * q0 + r.q0 * q3 + (r.q1 * q2 - r.q2 * q1), + 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 double[][] orthogonalizeMatrix(double[][] m, double threshold) + throws NotARotationMatrixException { + double[] m0 = m[0]; + double[] m1 = m[1]; + double[] m2 = m[2]; + double x00 = m0[0]; + double x01 = m0[1]; + double x02 = m0[2]; + double x10 = m1[0]; + double x11 = m1[1]; + double x12 = m1[2]; + double x20 = m2[0]; + double x21 = m2[1]; + double x22 = m2[2]; + double fn = 0; + double fn1; + + double[][] o = new double[3][3]; + double[] o0 = o[0]; + double[] o1 = o[1]; + double[] o2 = o[2]; + + // iterative correction: Xn+1 = Xn - 0.5 * (Xn.Mt.Xn - M) + int i = 0; + while (++i < 11) { + + // Mt.Xn + double mx00 = m0[0] * x00 + m1[0] * x10 + m2[0] * x20; + double mx10 = m0[1] * x00 + m1[1] * x10 + m2[1] * x20; + double mx20 = m0[2] * x00 + m1[2] * x10 + m2[2] * x20; + double mx01 = m0[0] * x01 + m1[0] * x11 + m2[0] * x21; + double mx11 = m0[1] * x01 + m1[1] * x11 + m2[1] * x21; + double mx21 = m0[2] * x01 + m1[2] * x11 + m2[2] * x21; + double mx02 = m0[0] * x02 + m1[0] * x12 + m2[0] * x22; + double mx12 = m0[1] * x02 + m1[1] * x12 + m2[1] * x22; + double mx22 = m0[2] * x02 + m1[2] * x12 + m2[2] * x22; + + // Xn+1 + o0[0] = x00 - 0.5 * (x00 * mx00 + x01 * mx10 + x02 * mx20 - m0[0]); + o0[1] = x01 - 0.5 * (x00 * mx01 + x01 * mx11 + x02 * mx21 - m0[1]); + o0[2] = x02 - 0.5 * (x00 * mx02 + x01 * mx12 + x02 * mx22 - m0[2]); + o1[0] = x10 - 0.5 * (x10 * mx00 + x11 * mx10 + x12 * mx20 - m1[0]); + o1[1] = x11 - 0.5 * (x10 * mx01 + x11 * mx11 + x12 * mx21 - m1[1]); + o1[2] = x12 - 0.5 * (x10 * mx02 + x11 * mx12 + x12 * mx22 - m1[2]); + o2[0] = x20 - 0.5 * (x20 * mx00 + x21 * mx10 + x22 * mx20 - m2[0]); + o2[1] = x21 - 0.5 * (x20 * mx01 + x21 * mx11 + x22 * mx21 - m2[1]); + o2[2] = x22 - 0.5 * (x20 * mx02 + x21 * mx12 + x22 * mx22 - m2[2]); + + // correction on each elements + double corr00 = o0[0] - m0[0]; + double corr01 = o0[1] - m0[1]; + double corr02 = o0[2] - m0[2]; + double corr10 = o1[0] - m1[0]; + double corr11 = o1[1] - m1[1]; + double corr12 = o1[2] - m1[2]; + double corr20 = o2[0] - m2[0]; + double corr21 = o2[1] - m2[1]; + double corr22 = o2[2] - m2[2]; + + // 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 = o0[0]; + x01 = o0[1]; + x02 = o0[2]; + x10 = o1[0]; + x11 = o1[1]; + x12 = o1[2]; + x20 = o2[0]; + x21 = o2[1]; + x22 = o2[2]; + fn = fn1; + + } + + // the algorithm did not converge after 10 iterations + throw new NotARotationMatrixException( + LocalizedFormats.UNABLE_TO_ORTHOGONOLIZE_MATRIX, + i - 1); + } + + /** Compute the <i>distance</i> between two rotations. + * <p>The <i>distance</i> 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:</p> + * <pre> + * r<sub>1</sub>(r) = r<sub>2</sub> + * </pre> + * <p>This distance is an angle between 0 and π. Its value is the smallest + * possible upper bound of the angle in radians between r<sub>1</sub>(v) + * and r<sub>2</sub>(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.</p> + * <p>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).</p> + * @param r1 first rotation + * @param r2 second rotation + * @return <i>distance</i> between r1 and r2 + */ + public static double distance(Rotation r1, Rotation r2) { + return r1.composeInverseInternal(r2).getAngle(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java new file mode 100644 index 0000000..6111ac3 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java @@ -0,0 +1,79 @@ +/* + * 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; + +/** + * This enumerates is used to differentiate the semantics of a rotation. + * @see Rotation + * @since 3.6 + */ +public enum RotationConvention { + + /** Constant for rotation that have the semantics of a vector operator. + * <p> + * According to this convention, the rotation moves vectors with respect + * to a fixed reference frame. + * </p> + * <p> + * This means that if we define rotation r is a 90 degrees rotation around + * the Z axis, the image of vector {@link Vector3D#PLUS_I} would be + * {@link Vector3D#PLUS_J}, the image of vector {@link Vector3D#PLUS_J} + * would be {@link Vector3D#MINUS_I}, the image of vector {@link Vector3D#PLUS_K} + * would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3) + * would be vector (-2, 1, 3). This means that the vector rotates counterclockwise. + * </p> + * <p> + * This convention was the only one supported by Apache Commons Math up to version 3.5. + * </p> + * <p> + * The difference with {@link #FRAME_TRANSFORM} is only the semantics of the sign + * of the angle. It is always possible to create or use a rotation using either + * convention to really represent a rotation that would have been best created or + * used with the other convention, by changing accordingly the sign of the + * rotation angle. This is how things were done up to version 3.5. + * </p> + */ + VECTOR_OPERATOR, + + /** Constant for rotation that have the semantics of a frame conversion. + * <p> + * According to this convention, the rotation considered vectors to be fixed, + * but their coordinates change as they are converted from an initial frame to + * a destination frame rotated with respect to the initial frame. + * </p> + * <p> + * This means that if we define rotation r is a 90 degrees rotation around + * the Z axis, the image of vector {@link Vector3D#PLUS_I} would be + * {@link Vector3D#MINUS_J}, the image of vector {@link Vector3D#PLUS_J} + * would be {@link Vector3D#PLUS_I}, the image of vector {@link Vector3D#PLUS_K} + * would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3) + * would be vector (2, -1, 3). This means that the coordinates of the vector rotates + * clockwise, because they are expressed with respect to a destination frame that is rotated + * counterclockwise. + * </p> + * <p> + * The difference with {@link #VECTOR_OPERATOR} is only the semantics of the sign + * of the angle. It is always possible to create or use a rotation using either + * convention to really represent a rotation that would have been best created or + * used with the other convention, by changing accordingly the sign of the + * rotation angle. This is how things were done up to version 3.5. + * </p> + */ + FRAME_TRANSFORM; + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationOrder.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationOrder.java new file mode 100644 index 0000000..03bc1c2 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationOrder.java @@ -0,0 +1,174 @@ +/* + * 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; + +/** + * This class is a utility representing a rotation order specification + * for Cardan or Euler angles specification. + * + * This class cannot be instanciated by the user. He can only use one + * of the twelve predefined supported orders as an argument to either + * the {@link Rotation#Rotation(RotationOrder,double,double,double)} + * constructor or the {@link Rotation#getAngles} method. + * + * @since 1.2 + */ +public final class RotationOrder { + + /** Set of Cardan angles. + * this ordered set of rotations is around X, then around Y, then + * around Z + */ + public static final RotationOrder XYZ = + new RotationOrder("XYZ", Vector3D.PLUS_I, Vector3D.PLUS_J, Vector3D.PLUS_K); + + /** Set of Cardan angles. + * this ordered set of rotations is around X, then around Z, then + * around Y + */ + public static final RotationOrder XZY = + new RotationOrder("XZY", Vector3D.PLUS_I, Vector3D.PLUS_K, Vector3D.PLUS_J); + + /** Set of Cardan angles. + * this ordered set of rotations is around Y, then around X, then + * around Z + */ + public static final RotationOrder YXZ = + new RotationOrder("YXZ", Vector3D.PLUS_J, Vector3D.PLUS_I, Vector3D.PLUS_K); + + /** Set of Cardan angles. + * this ordered set of rotations is around Y, then around Z, then + * around X + */ + public static final RotationOrder YZX = + new RotationOrder("YZX", Vector3D.PLUS_J, Vector3D.PLUS_K, Vector3D.PLUS_I); + + /** Set of Cardan angles. + * this ordered set of rotations is around Z, then around X, then + * around Y + */ + public static final RotationOrder ZXY = + new RotationOrder("ZXY", Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_J); + + /** Set of Cardan angles. + * this ordered set of rotations is around Z, then around Y, then + * around X + */ + public static final RotationOrder ZYX = + new RotationOrder("ZYX", Vector3D.PLUS_K, Vector3D.PLUS_J, Vector3D.PLUS_I); + + /** Set of Euler angles. + * this ordered set of rotations is around X, then around Y, then + * around X + */ + public static final RotationOrder XYX = + new RotationOrder("XYX", Vector3D.PLUS_I, Vector3D.PLUS_J, Vector3D.PLUS_I); + + /** Set of Euler angles. + * this ordered set of rotations is around X, then around Z, then + * around X + */ + public static final RotationOrder XZX = + new RotationOrder("XZX", Vector3D.PLUS_I, Vector3D.PLUS_K, Vector3D.PLUS_I); + + /** Set of Euler angles. + * this ordered set of rotations is around Y, then around X, then + * around Y + */ + public static final RotationOrder YXY = + new RotationOrder("YXY", Vector3D.PLUS_J, Vector3D.PLUS_I, Vector3D.PLUS_J); + + /** Set of Euler angles. + * this ordered set of rotations is around Y, then around Z, then + * around Y + */ + public static final RotationOrder YZY = + new RotationOrder("YZY", Vector3D.PLUS_J, Vector3D.PLUS_K, Vector3D.PLUS_J); + + /** Set of Euler angles. + * this ordered set of rotations is around Z, then around X, then + * around Z + */ + public static final RotationOrder ZXZ = + new RotationOrder("ZXZ", Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_K); + + /** Set of Euler angles. + * this ordered set of rotations is around Z, then around Y, then + * around Z + */ + public static final RotationOrder ZYZ = + new RotationOrder("ZYZ", Vector3D.PLUS_K, Vector3D.PLUS_J, Vector3D.PLUS_K); + + /** Name of the rotations order. */ + private final String name; + + /** Axis of the first rotation. */ + private final Vector3D a1; + + /** Axis of the second rotation. */ + private final Vector3D a2; + + /** Axis of the third rotation. */ + private final Vector3D a3; + + /** Private constructor. + * This is a utility class that cannot be instantiated by the user, + * so its only constructor is private. + * @param name name of the rotation order + * @param a1 axis of the first rotation + * @param a2 axis of the second rotation + * @param a3 axis of the third rotation + */ + private RotationOrder(final String name, + final Vector3D a1, final Vector3D a2, final Vector3D a3) { + this.name = name; + this.a1 = a1; + this.a2 = a2; + this.a3 = a3; + } + + /** Get a string representation of the instance. + * @return a string representation of the instance (in fact, its name) + */ + @Override + public String toString() { + return name; + } + + /** Get the axis of the first rotation. + * @return axis of the first rotation + */ + public Vector3D getA1() { + return a1; + } + + /** Get the axis of the second rotation. + * @return axis of the second rotation + */ + public Vector3D getA2() { + return a2; + } + + /** Get the axis of the second rotation. + * @return axis of the second rotation + */ + public Vector3D getA3() { + return a3; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Segment.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Segment.java new file mode 100644 index 0000000..200b462 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Segment.java @@ -0,0 +1,66 @@ +/* + * 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; + + +/** Simple container for a two-points segment. + * @since 3.0 + */ +public class Segment { + + /** Start point of the segment. */ + private final Vector3D start; + + /** End point of the segments. */ + private final Vector3D end; + + /** Line containing the segment. */ + private final Line line; + + /** Build a segment. + * @param start start point of the segment + * @param end end point of the segment + * @param line line containing the segment + */ + public Segment(final Vector3D start, final Vector3D end, final Line line) { + this.start = start; + this.end = end; + this.line = line; + } + + /** Get the start point of the segment. + * @return start point of the segment + */ + public Vector3D getStart() { + return start; + } + + /** Get the end point of the segment. + * @return end point of the segment + */ + public Vector3D getEnd() { + return end; + } + + /** Get the line containing the segment. + * @return line containing the segment + */ + public Line getLine() { + return line; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphereGenerator.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphereGenerator.java new file mode 100644 index 0000000..b553510 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphereGenerator.java @@ -0,0 +1,152 @@ +/* + * 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.util.Arrays; +import java.util.List; + +import org.apache.commons.math3.fraction.BigFraction; +import org.apache.commons.math3.geometry.enclosing.EnclosingBall; +import org.apache.commons.math3.geometry.enclosing.SupportBallGenerator; +import org.apache.commons.math3.geometry.euclidean.twod.DiskGenerator; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.util.FastMath; + +/** Class generating an enclosing ball from its support points. + * @since 3.3 + */ +public class SphereGenerator implements SupportBallGenerator<Euclidean3D, Vector3D> { + + /** {@inheritDoc} */ + public EnclosingBall<Euclidean3D, Vector3D> ballOnSupport(final List<Vector3D> support) { + + if (support.size() < 1) { + return new EnclosingBall<Euclidean3D, Vector3D>(Vector3D.ZERO, Double.NEGATIVE_INFINITY); + } else { + final Vector3D vA = support.get(0); + if (support.size() < 2) { + return new EnclosingBall<Euclidean3D, Vector3D>(vA, 0, vA); + } else { + final Vector3D vB = support.get(1); + if (support.size() < 3) { + return new EnclosingBall<Euclidean3D, Vector3D>(new Vector3D(0.5, vA, 0.5, vB), + 0.5 * vA.distance(vB), + vA, vB); + } else { + final Vector3D vC = support.get(2); + if (support.size() < 4) { + + // delegate to 2D disk generator + final Plane p = new Plane(vA, vB, vC, + 1.0e-10 * (vA.getNorm1() + vB.getNorm1() + vC.getNorm1())); + final EnclosingBall<Euclidean2D, Vector2D> disk = + new DiskGenerator().ballOnSupport(Arrays.asList(p.toSubSpace(vA), + p.toSubSpace(vB), + p.toSubSpace(vC))); + + // convert back to 3D + return new EnclosingBall<Euclidean3D, Vector3D>(p.toSpace(disk.getCenter()), + disk.getRadius(), vA, vB, vC); + + } else { + final Vector3D vD = support.get(3); + // a sphere is 3D can be defined as: + // (1) (x - x_0)^2 + (y - y_0)^2 + (z - z_0)^2 = r^2 + // which can be written: + // (2) (x^2 + y^2 + z^2) - 2 x_0 x - 2 y_0 y - 2 z_0 z + (x_0^2 + y_0^2 + z_0^2 - r^2) = 0 + // or simply: + // (3) (x^2 + y^2 + z^2) + a x + b y + c z + d = 0 + // with sphere center coordinates -a/2, -b/2, -c/2 + // If the sphere exists, a b, c and d are a non zero solution to + // [ (x^2 + y^2 + z^2) x y z 1 ] [ 1 ] [ 0 ] + // [ (xA^2 + yA^2 + zA^2) xA yA zA 1 ] [ a ] [ 0 ] + // [ (xB^2 + yB^2 + zB^2) xB yB zB 1 ] * [ b ] = [ 0 ] + // [ (xC^2 + yC^2 + zC^2) xC yC zC 1 ] [ c ] [ 0 ] + // [ (xD^2 + yD^2 + zD^2) xD yD zD 1 ] [ d ] [ 0 ] + // So the determinant of the matrix is zero. Computing this determinant + // by expanding it using the minors m_ij of first row leads to + // (4) m_11 (x^2 + y^2 + z^2) - m_12 x + m_13 y - m_14 z + m_15 = 0 + // So by identifying equations (2) and (4) we get the coordinates + // of center as: + // x_0 = +m_12 / (2 m_11) + // y_0 = -m_13 / (2 m_11) + // z_0 = +m_14 / (2 m_11) + // Note that the minors m_11, m_12, m_13 and m_14 all have the last column + // filled with 1.0, hence simplifying the computation + final BigFraction[] c2 = new BigFraction[] { + new BigFraction(vA.getX()), new BigFraction(vB.getX()), + new BigFraction(vC.getX()), new BigFraction(vD.getX()) + }; + final BigFraction[] c3 = new BigFraction[] { + new BigFraction(vA.getY()), new BigFraction(vB.getY()), + new BigFraction(vC.getY()), new BigFraction(vD.getY()) + }; + final BigFraction[] c4 = new BigFraction[] { + new BigFraction(vA.getZ()), new BigFraction(vB.getZ()), + new BigFraction(vC.getZ()), new BigFraction(vD.getZ()) + }; + final BigFraction[] c1 = new BigFraction[] { + c2[0].multiply(c2[0]).add(c3[0].multiply(c3[0])).add(c4[0].multiply(c4[0])), + c2[1].multiply(c2[1]).add(c3[1].multiply(c3[1])).add(c4[1].multiply(c4[1])), + c2[2].multiply(c2[2]).add(c3[2].multiply(c3[2])).add(c4[2].multiply(c4[2])), + c2[3].multiply(c2[3]).add(c3[3].multiply(c3[3])).add(c4[3].multiply(c4[3])) + }; + final BigFraction twoM11 = minor(c2, c3, c4).multiply(2); + final BigFraction m12 = minor(c1, c3, c4); + final BigFraction m13 = minor(c1, c2, c4); + final BigFraction m14 = minor(c1, c2, c3); + final BigFraction centerX = m12.divide(twoM11); + final BigFraction centerY = m13.divide(twoM11).negate(); + final BigFraction centerZ = m14.divide(twoM11); + final BigFraction dx = c2[0].subtract(centerX); + final BigFraction dy = c3[0].subtract(centerY); + final BigFraction dz = c4[0].subtract(centerZ); + final BigFraction r2 = dx.multiply(dx).add(dy.multiply(dy)).add(dz.multiply(dz)); + return new EnclosingBall<Euclidean3D, Vector3D>(new Vector3D(centerX.doubleValue(), + centerY.doubleValue(), + centerZ.doubleValue()), + FastMath.sqrt(r2.doubleValue()), + vA, vB, vC, vD); + } + } + } + } + } + + /** Compute a dimension 4 minor, when 4<sup>th</sup> column is known to be filled with 1.0. + * @param c1 first column + * @param c2 second column + * @param c3 third column + * @return value of the minor computed has an exact fraction + */ + private BigFraction minor(final BigFraction[] c1, final BigFraction[] c2, final BigFraction[] c3) { + return c2[0].multiply(c3[1]).multiply(c1[2].subtract(c1[3])). + add(c2[0].multiply(c3[2]).multiply(c1[3].subtract(c1[1]))). + add(c2[0].multiply(c3[3]).multiply(c1[1].subtract(c1[2]))). + add(c2[1].multiply(c3[0]).multiply(c1[3].subtract(c1[2]))). + add(c2[1].multiply(c3[2]).multiply(c1[0].subtract(c1[3]))). + add(c2[1].multiply(c3[3]).multiply(c1[2].subtract(c1[0]))). + add(c2[2].multiply(c3[0]).multiply(c1[1].subtract(c1[3]))). + add(c2[2].multiply(c3[1]).multiply(c1[3].subtract(c1[0]))). + add(c2[2].multiply(c3[3]).multiply(c1[0].subtract(c1[1]))). + add(c2[3].multiply(c3[0]).multiply(c1[2].subtract(c1[1]))). + add(c2[3].multiply(c3[1]).multiply(c1[0].subtract(c1[2]))). + add(c2[3].multiply(c3[2]).multiply(c1[1].subtract(c1[0]))); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphericalCoordinates.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphericalCoordinates.java new file mode 100644 index 0000000..016e0a0 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SphericalCoordinates.java @@ -0,0 +1,395 @@ +/* + * 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.util.FastMath; + +/** This class provides conversions related to <a + * href="http://mathworld.wolfram.com/SphericalCoordinates.html">spherical coordinates</a>. + * <p> + * The conventions used here are the mathematical ones, i.e. spherical coordinates are + * related to Cartesian coordinates as follows: + * </p> + * <ul> + * <li>x = r cos(θ) sin(Φ)</li> + * <li>y = r sin(θ) sin(Φ)</li> + * <li>z = r cos(Φ)</li> + * </ul> + * <ul> + * <li>r = √(x<sup>2</sup>+y<sup>2</sup>+z<sup>2</sup>)</li> + * <li>θ = atan2(y, x)</li> + * <li>Φ = acos(z/r)</li> + * </ul> + * <p> + * r is the radius, θ is the azimuthal angle in the x-y plane and Φ is the polar + * (co-latitude) angle. These conventions are <em>different</em> from the conventions used + * in physics (and in particular in spherical harmonics) where the meanings of θ and + * Φ are reversed. + * </p> + * <p> + * This class provides conversion of coordinates and also of gradient and Hessian + * between spherical and Cartesian coordinates. + * </p> + * @since 3.2 + */ +public class SphericalCoordinates implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20130206L; + + /** Cartesian coordinates. */ + private final Vector3D v; + + /** Radius. */ + private final double r; + + /** Azimuthal angle in the x-y plane θ. */ + private final double theta; + + /** Polar angle (co-latitude) Φ. */ + private final double phi; + + /** Jacobian of (r, θ &Phi). */ + private double[][] jacobian; + + /** Hessian of radius. */ + private double[][] rHessian; + + /** Hessian of azimuthal angle in the x-y plane θ. */ + private double[][] thetaHessian; + + /** Hessian of polar (co-latitude) angle Φ. */ + private double[][] phiHessian; + + /** Build a spherical coordinates transformer from Cartesian coordinates. + * @param v Cartesian coordinates + */ + public SphericalCoordinates(final Vector3D v) { + + // Cartesian coordinates + this.v = v; + + // remaining spherical coordinates + this.r = v.getNorm(); + this.theta = v.getAlpha(); + this.phi = FastMath.acos(v.getZ() / r); + + } + + /** Build a spherical coordinates transformer from spherical coordinates. + * @param r radius + * @param theta azimuthal angle in x-y plane + * @param phi polar (co-latitude) angle + */ + public SphericalCoordinates(final double r, final double theta, final double phi) { + + final double cosTheta = FastMath.cos(theta); + final double sinTheta = FastMath.sin(theta); + final double cosPhi = FastMath.cos(phi); + final double sinPhi = FastMath.sin(phi); + + // spherical coordinates + this.r = r; + this.theta = theta; + this.phi = phi; + + // Cartesian coordinates + this.v = new Vector3D(r * cosTheta * sinPhi, + r * sinTheta * sinPhi, + r * cosPhi); + + } + + /** Get the Cartesian coordinates. + * @return Cartesian coordinates + */ + public Vector3D getCartesian() { + return v; + } + + /** Get the radius. + * @return radius r + * @see #getTheta() + * @see #getPhi() + */ + public double getR() { + return r; + } + + /** Get the azimuthal angle in x-y plane. + * @return azimuthal angle in x-y plane θ + * @see #getR() + * @see #getPhi() + */ + public double getTheta() { + return theta; + } + + /** Get the polar (co-latitude) angle. + * @return polar (co-latitude) angle Φ + * @see #getR() + * @see #getTheta() + */ + public double getPhi() { + return phi; + } + + /** Convert a gradient with respect to spherical coordinates into a gradient + * with respect to Cartesian coordinates. + * @param sGradient gradient with respect to spherical coordinates + * {df/dr, df/dθ, df/dΦ} + * @return gradient with respect to Cartesian coordinates + * {df/dx, df/dy, df/dz} + */ + public double[] toCartesianGradient(final double[] sGradient) { + + // lazy evaluation of Jacobian + computeJacobian(); + + // compose derivatives as gradient^T . J + // the expressions have been simplified since we know jacobian[1][2] = dTheta/dZ = 0 + return new double[] { + sGradient[0] * jacobian[0][0] + sGradient[1] * jacobian[1][0] + sGradient[2] * jacobian[2][0], + sGradient[0] * jacobian[0][1] + sGradient[1] * jacobian[1][1] + sGradient[2] * jacobian[2][1], + sGradient[0] * jacobian[0][2] + sGradient[2] * jacobian[2][2] + }; + + } + + /** Convert a Hessian with respect to spherical coordinates into a Hessian + * with respect to Cartesian coordinates. + * <p> + * As Hessian are always symmetric, we use only the lower left part of the provided + * spherical Hessian, so the upper part may not be initialized. However, we still + * do fill up the complete array we create, with guaranteed symmetry. + * </p> + * @param sHessian Hessian with respect to spherical coordinates + * {{d<sup>2</sup>f/dr<sup>2</sup>, d<sup>2</sup>f/drdθ, d<sup>2</sup>f/drdΦ}, + * {d<sup>2</sup>f/drdθ, d<sup>2</sup>f/dθ<sup>2</sup>, d<sup>2</sup>f/dθdΦ}, + * {d<sup>2</sup>f/drdΦ, d<sup>2</sup>f/dθdΦ, d<sup>2</sup>f/dΦ<sup>2</sup>} + * @param sGradient gradient with respect to spherical coordinates + * {df/dr, df/dθ, df/dΦ} + * @return Hessian with respect to Cartesian coordinates + * {{d<sup>2</sup>f/dx<sup>2</sup>, d<sup>2</sup>f/dxdy, d<sup>2</sup>f/dxdz}, + * {d<sup>2</sup>f/dxdy, d<sup>2</sup>f/dy<sup>2</sup>, d<sup>2</sup>f/dydz}, + * {d<sup>2</sup>f/dxdz, d<sup>2</sup>f/dydz, d<sup>2</sup>f/dz<sup>2</sup>}} + */ + public double[][] toCartesianHessian(final double[][] sHessian, final double[] sGradient) { + + computeJacobian(); + computeHessians(); + + // compose derivative as J^T . H_f . J + df/dr H_r + df/dtheta H_theta + df/dphi H_phi + // the expressions have been simplified since we know jacobian[1][2] = dTheta/dZ = 0 + // and H_theta is only a 2x2 matrix as it does not depend on z + final double[][] hj = new double[3][3]; + final double[][] cHessian = new double[3][3]; + + // compute H_f . J + // beware we use ONLY the lower-left part of sHessian + hj[0][0] = sHessian[0][0] * jacobian[0][0] + sHessian[1][0] * jacobian[1][0] + sHessian[2][0] * jacobian[2][0]; + hj[0][1] = sHessian[0][0] * jacobian[0][1] + sHessian[1][0] * jacobian[1][1] + sHessian[2][0] * jacobian[2][1]; + hj[0][2] = sHessian[0][0] * jacobian[0][2] + sHessian[2][0] * jacobian[2][2]; + hj[1][0] = sHessian[1][0] * jacobian[0][0] + sHessian[1][1] * jacobian[1][0] + sHessian[2][1] * jacobian[2][0]; + hj[1][1] = sHessian[1][0] * jacobian[0][1] + sHessian[1][1] * jacobian[1][1] + sHessian[2][1] * jacobian[2][1]; + // don't compute hj[1][2] as it is not used below + hj[2][0] = sHessian[2][0] * jacobian[0][0] + sHessian[2][1] * jacobian[1][0] + sHessian[2][2] * jacobian[2][0]; + hj[2][1] = sHessian[2][0] * jacobian[0][1] + sHessian[2][1] * jacobian[1][1] + sHessian[2][2] * jacobian[2][1]; + hj[2][2] = sHessian[2][0] * jacobian[0][2] + sHessian[2][2] * jacobian[2][2]; + + // compute lower-left part of J^T . H_f . J + cHessian[0][0] = jacobian[0][0] * hj[0][0] + jacobian[1][0] * hj[1][0] + jacobian[2][0] * hj[2][0]; + cHessian[1][0] = jacobian[0][1] * hj[0][0] + jacobian[1][1] * hj[1][0] + jacobian[2][1] * hj[2][0]; + cHessian[2][0] = jacobian[0][2] * hj[0][0] + jacobian[2][2] * hj[2][0]; + cHessian[1][1] = jacobian[0][1] * hj[0][1] + jacobian[1][1] * hj[1][1] + jacobian[2][1] * hj[2][1]; + cHessian[2][1] = jacobian[0][2] * hj[0][1] + jacobian[2][2] * hj[2][1]; + cHessian[2][2] = jacobian[0][2] * hj[0][2] + jacobian[2][2] * hj[2][2]; + + // add gradient contribution + cHessian[0][0] += sGradient[0] * rHessian[0][0] + sGradient[1] * thetaHessian[0][0] + sGradient[2] * phiHessian[0][0]; + cHessian[1][0] += sGradient[0] * rHessian[1][0] + sGradient[1] * thetaHessian[1][0] + sGradient[2] * phiHessian[1][0]; + cHessian[2][0] += sGradient[0] * rHessian[2][0] + sGradient[2] * phiHessian[2][0]; + cHessian[1][1] += sGradient[0] * rHessian[1][1] + sGradient[1] * thetaHessian[1][1] + sGradient[2] * phiHessian[1][1]; + cHessian[2][1] += sGradient[0] * rHessian[2][1] + sGradient[2] * phiHessian[2][1]; + cHessian[2][2] += sGradient[0] * rHessian[2][2] + sGradient[2] * phiHessian[2][2]; + + // ensure symmetry + cHessian[0][1] = cHessian[1][0]; + cHessian[0][2] = cHessian[2][0]; + cHessian[1][2] = cHessian[2][1]; + + return cHessian; + + } + + /** Lazy evaluation of (r, θ, φ) Jacobian. + */ + private void computeJacobian() { + if (jacobian == null) { + + // intermediate variables + final double x = v.getX(); + final double y = v.getY(); + final double z = v.getZ(); + final double rho2 = x * x + y * y; + final double rho = FastMath.sqrt(rho2); + final double r2 = rho2 + z * z; + + jacobian = new double[3][3]; + + // row representing the gradient of r + jacobian[0][0] = x / r; + jacobian[0][1] = y / r; + jacobian[0][2] = z / r; + + // row representing the gradient of theta + jacobian[1][0] = -y / rho2; + jacobian[1][1] = x / rho2; + // jacobian[1][2] is already set to 0 at allocation time + + // row representing the gradient of phi + jacobian[2][0] = x * z / (rho * r2); + jacobian[2][1] = y * z / (rho * r2); + jacobian[2][2] = -rho / r2; + + } + } + + /** Lazy evaluation of Hessians. + */ + private void computeHessians() { + + if (rHessian == null) { + + // intermediate variables + final double x = v.getX(); + final double y = v.getY(); + final double z = v.getZ(); + final double x2 = x * x; + final double y2 = y * y; + final double z2 = z * z; + final double rho2 = x2 + y2; + final double rho = FastMath.sqrt(rho2); + final double r2 = rho2 + z2; + final double xOr = x / r; + final double yOr = y / r; + final double zOr = z / r; + final double xOrho2 = x / rho2; + final double yOrho2 = y / rho2; + final double xOr3 = xOr / r2; + final double yOr3 = yOr / r2; + final double zOr3 = zOr / r2; + + // lower-left part of Hessian of r + rHessian = new double[3][3]; + rHessian[0][0] = y * yOr3 + z * zOr3; + rHessian[1][0] = -x * yOr3; + rHessian[2][0] = -z * xOr3; + rHessian[1][1] = x * xOr3 + z * zOr3; + rHessian[2][1] = -y * zOr3; + rHessian[2][2] = x * xOr3 + y * yOr3; + + // upper-right part is symmetric + rHessian[0][1] = rHessian[1][0]; + rHessian[0][2] = rHessian[2][0]; + rHessian[1][2] = rHessian[2][1]; + + // lower-left part of Hessian of azimuthal angle theta + thetaHessian = new double[2][2]; + thetaHessian[0][0] = 2 * xOrho2 * yOrho2; + thetaHessian[1][0] = yOrho2 * yOrho2 - xOrho2 * xOrho2; + thetaHessian[1][1] = -2 * xOrho2 * yOrho2; + + // upper-right part is symmetric + thetaHessian[0][1] = thetaHessian[1][0]; + + // lower-left part of Hessian of polar (co-latitude) angle phi + final double rhor2 = rho * r2; + final double rho2r2 = rho * rhor2; + final double rhor4 = rhor2 * r2; + final double rho3r4 = rhor4 * rho2; + final double r2P2rho2 = 3 * rho2 + z2; + phiHessian = new double[3][3]; + phiHessian[0][0] = z * (rho2r2 - x2 * r2P2rho2) / rho3r4; + phiHessian[1][0] = -x * y * z * r2P2rho2 / rho3r4; + phiHessian[2][0] = x * (rho2 - z2) / rhor4; + phiHessian[1][1] = z * (rho2r2 - y2 * r2P2rho2) / rho3r4; + phiHessian[2][1] = y * (rho2 - z2) / rhor4; + phiHessian[2][2] = 2 * rho * zOr3 / r; + + // upper-right part is symmetric + phiHessian[0][1] = phiHessian[1][0]; + phiHessian[0][2] = phiHessian[2][0]; + phiHessian[1][2] = phiHessian[2][1]; + + } + + } + + /** + * Replace the instance with a data transfer object for serialization. + * @return data transfer object that will be serialized + */ + private Object writeReplace() { + return new DataTransferObject(v.getX(), v.getY(), v.getZ()); + } + + /** Internal class used only for serialization. */ + private static class DataTransferObject implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20130206L; + + /** Abscissa. + * @serial + */ + private final double x; + + /** Ordinate. + * @serial + */ + private final double y; + + /** Height. + * @serial + */ + private final double z; + + /** Simple constructor. + * @param x abscissa + * @param y ordinate + * @param z height + */ + DataTransferObject(final double x, final double y, final double z) { + this.x = x; + this.y = y; + this.z = z; + } + + /** Replace the deserialized data transfer object with a {@link SphericalCoordinates}. + * @return replacement {@link SphericalCoordinates} + */ + private Object readResolve() { + return new SphericalCoordinates(new Vector3D(x, y, z)); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubLine.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubLine.java new file mode 100644 index 0000000..2ac917f --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubLine.java @@ -0,0 +1,165 @@ +/* + * 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.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; +import org.apache.commons.math3.geometry.euclidean.oned.Interval; +import org.apache.commons.math3.geometry.euclidean.oned.IntervalsSet; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +import org.apache.commons.math3.geometry.partitioning.Region.Location; + +/** This class represents a subset of a {@link Line}. + * @since 3.0 + */ +public class SubLine { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Underlying line. */ + private final Line line; + + /** Remaining region of the hyperplane. */ + private final IntervalsSet remainingRegion; + + /** Simple constructor. + * @param line underlying line + * @param remainingRegion remaining region of the line + */ + public SubLine(final Line line, final IntervalsSet remainingRegion) { + this.line = line; + this.remainingRegion = remainingRegion; + } + + /** Create a sub-line from two endpoints. + * @param start start point + * @param end end point + * @param tolerance tolerance below which points are considered identical + * @exception MathIllegalArgumentException if the points are equal + * @since 3.3 + */ + public SubLine(final Vector3D start, final Vector3D end, final double tolerance) + throws MathIllegalArgumentException { + this(new Line(start, end, tolerance), buildIntervalSet(start, end, tolerance)); + } + + /** Create a sub-line from two endpoints. + * @param start start point + * @param end end point + * @exception MathIllegalArgumentException if the points are equal + * @deprecated as of 3.3, replaced with {@link #SubLine(Vector3D, Vector3D, double)} + */ + public SubLine(final Vector3D start, final Vector3D end) + throws MathIllegalArgumentException { + this(start, end, DEFAULT_TOLERANCE); + } + + /** Create a sub-line from a segment. + * @param segment single segment forming the sub-line + * @exception MathIllegalArgumentException if the segment endpoints are equal + */ + public SubLine(final Segment segment) throws MathIllegalArgumentException { + this(segment.getLine(), + buildIntervalSet(segment.getStart(), segment.getEnd(), segment.getLine().getTolerance())); + } + + /** Get the endpoints of the sub-line. + * <p> + * A subline may be any arbitrary number of disjoints segments, so the endpoints + * are provided as a list of endpoint pairs. Each element of the list represents + * one segment, and each segment contains a start point at index 0 and an end point + * at index 1. If the sub-line is unbounded in the negative infinity direction, + * the start point of the first segment will have infinite coordinates. If the + * sub-line is unbounded in the positive infinity direction, the end point of the + * last segment will have infinite coordinates. So a sub-line covering the whole + * line will contain just one row and both elements of this row will have infinite + * coordinates. If the sub-line is empty, the returned list will contain 0 segments. + * </p> + * @return list of segments endpoints + */ + public List<Segment> getSegments() { + + final List<Interval> list = remainingRegion.asList(); + final List<Segment> segments = new ArrayList<Segment>(list.size()); + + for (final Interval interval : list) { + final Vector3D start = line.toSpace((Point<Euclidean1D>) new Vector1D(interval.getInf())); + final Vector3D end = line.toSpace((Point<Euclidean1D>) new Vector1D(interval.getSup())); + segments.add(new Segment(start, end, line)); + } + + return segments; + + } + + /** Get the intersection of the instance and another sub-line. + * <p> + * This method is related to the {@link Line#intersection(Line) + * intersection} method in the {@link Line Line} class, but in addition + * to compute the point along infinite lines, it also checks the point + * lies on both sub-line ranges. + * </p> + * @param subLine other sub-line which may intersect instance + * @param includeEndPoints if true, endpoints are considered to belong to + * instance (i.e. they are closed sets) and may be returned, otherwise endpoints + * are considered to not belong to instance (i.e. they are open sets) and intersection + * occurring on endpoints lead to null being returned + * @return the intersection point if there is one, null if the sub-lines don't intersect + */ + public Vector3D intersection(final SubLine subLine, final boolean includeEndPoints) { + + // compute the intersection on infinite line + Vector3D v1D = line.intersection(subLine.line); + if (v1D == null) { + return null; + } + + // check location of point with respect to first sub-line + Location loc1 = remainingRegion.checkPoint((Point<Euclidean1D>) line.toSubSpace((Point<Euclidean3D>) v1D)); + + // check location of point with respect to second sub-line + Location loc2 = subLine.remainingRegion.checkPoint((Point<Euclidean1D>) subLine.line.toSubSpace((Point<Euclidean3D>) v1D)); + + if (includeEndPoints) { + return ((loc1 != Location.OUTSIDE) && (loc2 != Location.OUTSIDE)) ? v1D : null; + } else { + return ((loc1 == Location.INSIDE) && (loc2 == Location.INSIDE)) ? v1D : null; + } + + } + + /** Build an interval set from two points. + * @param start start point + * @param end end point + * @return an interval set + * @param tolerance tolerance below which points are considered identical + * @exception MathIllegalArgumentException if the points are equal + */ + private static IntervalsSet buildIntervalSet(final Vector3D start, final Vector3D end, final double tolerance) + throws MathIllegalArgumentException { + final Line line = new Line(start, end, tolerance); + return new IntervalsSet(line.toSubSpace((Point<Euclidean3D>) start).getX(), + line.toSubSpace((Point<Euclidean3D>) end).getX(), + tolerance); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubPlane.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubPlane.java new file mode 100644 index 0000000..ce02a38 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/SubPlane.java @@ -0,0 +1,108 @@ +/* + * 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.geometry.Point; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.partitioning.AbstractSubHyperplane; +import org.apache.commons.math3.geometry.partitioning.BSPTree; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.Region; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; + +/** This class represents a sub-hyperplane for {@link Plane}. + * @since 3.0 + */ +public class SubPlane extends AbstractSubHyperplane<Euclidean3D, Euclidean2D> { + + /** Simple constructor. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + public SubPlane(final Hyperplane<Euclidean3D> hyperplane, + final Region<Euclidean2D> remainingRegion) { + super(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + protected AbstractSubHyperplane<Euclidean3D, Euclidean2D> buildNew(final Hyperplane<Euclidean3D> hyperplane, + final Region<Euclidean2D> remainingRegion) { + return new SubPlane(hyperplane, remainingRegion); + } + + /** Split the instance in two parts by an hyperplane. + * @param hyperplane splitting hyperplane + * @return an object containing both the part of the instance + * on the plus side of the instance and the part of the + * instance on the minus side of the instance + */ + @Override + public SplitSubHyperplane<Euclidean3D> split(Hyperplane<Euclidean3D> hyperplane) { + + final Plane otherPlane = (Plane) hyperplane; + final Plane thisPlane = (Plane) getHyperplane(); + final Line inter = otherPlane.intersection(thisPlane); + final double tolerance = thisPlane.getTolerance(); + + if (inter == null) { + // the hyperplanes are parallel + final double global = otherPlane.getOffset(thisPlane); + if (global < -tolerance) { + return new SplitSubHyperplane<Euclidean3D>(null, this); + } else if (global > tolerance) { + return new SplitSubHyperplane<Euclidean3D>(this, null); + } else { + return new SplitSubHyperplane<Euclidean3D>(null, null); + } + } + + // the hyperplanes do intersect + Vector2D p = thisPlane.toSubSpace((Point<Euclidean3D>) inter.toSpace((Point<Euclidean1D>) Vector1D.ZERO)); + Vector2D q = thisPlane.toSubSpace((Point<Euclidean3D>) inter.toSpace((Point<Euclidean1D>) Vector1D.ONE)); + Vector3D crossP = Vector3D.crossProduct(inter.getDirection(), thisPlane.getNormal()); + if (crossP.dotProduct(otherPlane.getNormal()) < 0) { + final Vector2D tmp = p; + p = q; + q = tmp; + } + final SubHyperplane<Euclidean2D> l2DMinus = + new org.apache.commons.math3.geometry.euclidean.twod.Line(p, q, tolerance).wholeHyperplane(); + final SubHyperplane<Euclidean2D> l2DPlus = + new org.apache.commons.math3.geometry.euclidean.twod.Line(q, p, tolerance).wholeHyperplane(); + + final BSPTree<Euclidean2D> splitTree = getRemainingRegion().getTree(false).split(l2DMinus); + final BSPTree<Euclidean2D> plusTree = getRemainingRegion().isEmpty(splitTree.getPlus()) ? + new BSPTree<Euclidean2D>(Boolean.FALSE) : + new BSPTree<Euclidean2D>(l2DPlus, new BSPTree<Euclidean2D>(Boolean.FALSE), + splitTree.getPlus(), null); + + final BSPTree<Euclidean2D> minusTree = getRemainingRegion().isEmpty(splitTree.getMinus()) ? + new BSPTree<Euclidean2D>(Boolean.FALSE) : + new BSPTree<Euclidean2D>(l2DMinus, new BSPTree<Euclidean2D>(Boolean.FALSE), + splitTree.getMinus(), null); + + return new SplitSubHyperplane<Euclidean3D>(new SubPlane(thisPlane.copySelf(), new PolygonsSet(plusTree, tolerance)), + new SubPlane(thisPlane.copySelf(), new PolygonsSet(minusTree, tolerance))); + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3D.java new file mode 100644 index 0000000..3eaea3a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3D.java @@ -0,0 +1,588 @@ +/* + * 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.exception.DimensionMismatchException; +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.Vector; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.MathUtils; + +/** + * This class implements vectors in a three-dimensional space. + * <p>Instance of this class are guaranteed to be immutable.</p> + * @since 1.2 + */ +public class Vector3D implements Serializable, Vector<Euclidean3D> { + + /** Null vector (coordinates: 0, 0, 0). */ + public static final Vector3D ZERO = new Vector3D(0, 0, 0); + + /** First canonical vector (coordinates: 1, 0, 0). */ + public static final Vector3D PLUS_I = new Vector3D(1, 0, 0); + + /** Opposite of the first canonical vector (coordinates: -1, 0, 0). */ + public static final Vector3D MINUS_I = new Vector3D(-1, 0, 0); + + /** Second canonical vector (coordinates: 0, 1, 0). */ + public static final Vector3D PLUS_J = new Vector3D(0, 1, 0); + + /** Opposite of the second canonical vector (coordinates: 0, -1, 0). */ + public static final Vector3D MINUS_J = new Vector3D(0, -1, 0); + + /** Third canonical vector (coordinates: 0, 0, 1). */ + public static final Vector3D PLUS_K = new Vector3D(0, 0, 1); + + /** Opposite of the third canonical vector (coordinates: 0, 0, -1). */ + public static final Vector3D MINUS_K = new Vector3D(0, 0, -1); + + // CHECKSTYLE: stop ConstantName + /** A vector with all coordinates set to NaN. */ + public static final Vector3D NaN = new Vector3D(Double.NaN, Double.NaN, Double.NaN); + // CHECKSTYLE: resume ConstantName + + /** A vector with all coordinates set to positive infinity. */ + public static final Vector3D POSITIVE_INFINITY = + new Vector3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); + + /** A vector with all coordinates set to negative infinity. */ + public static final Vector3D NEGATIVE_INFINITY = + new Vector3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY); + + /** Serializable version identifier. */ + private static final long serialVersionUID = 1313493323784566947L; + + /** Abscissa. */ + private final double x; + + /** Ordinate. */ + private final double y; + + /** Height. */ + private final double 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 Vector3D(double x, double y, double 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 Vector3D(double[] 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 Vector3D(double alpha, double delta) { + double cosDelta = FastMath.cos(delta); + this.x = FastMath.cos(alpha) * cosDelta; + this.y = FastMath.sin(alpha) * cosDelta; + this.z = FastMath.sin(delta); + } + + /** 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 Vector3D(double a, Vector3D u) { + this.x = a * u.x; + this.y = a * u.y; + this.z = a * u.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 Vector3D(double a1, Vector3D u1, double a2, Vector3D u2) { + this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x); + this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y); + this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.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 Vector3D(double a1, Vector3D u1, double a2, Vector3D u2, + double a3, Vector3D u3) { + this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x); + this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y); + this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.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 Vector3D(double a1, Vector3D u1, double a2, Vector3D u2, + double a3, Vector3D u3, double a4, Vector3D u4) { + this.x = MathArrays.linearCombination(a1, u1.x, a2, u2.x, a3, u3.x, a4, u4.x); + this.y = MathArrays.linearCombination(a1, u1.y, a2, u2.y, a3, u3.y, a4, u4.y); + this.z = MathArrays.linearCombination(a1, u1.z, a2, u2.z, a3, u3.z, a4, u4.z); + } + + /** Get the abscissa of the vector. + * @return abscissa of the vector + * @see #Vector3D(double, double, double) + */ + public double getX() { + return x; + } + + /** Get the ordinate of the vector. + * @return ordinate of the vector + * @see #Vector3D(double, double, double) + */ + public double getY() { + return y; + } + + /** Get the height of the vector. + * @return height of the vector + * @see #Vector3D(double, double, double) + */ + public double getZ() { + return z; + } + + /** Get the vector coordinates as a dimension 3 array. + * @return vector coordinates + * @see #Vector3D(double[]) + */ + public double[] toArray() { + return new double[] { x, y, z }; + } + + /** {@inheritDoc} */ + public Space getSpace() { + return Euclidean3D.getInstance(); + } + + /** {@inheritDoc} */ + public Vector3D getZero() { + return ZERO; + } + + /** {@inheritDoc} */ + public double getNorm1() { + return FastMath.abs(x) + FastMath.abs(y) + FastMath.abs(z); + } + + /** {@inheritDoc} */ + public double getNorm() { + // there are no cancellation problems here, so we use the straightforward formula + return FastMath.sqrt (x * x + y * y + z * z); + } + + /** {@inheritDoc} */ + public double getNormSq() { + // there are no cancellation problems here, so we use the straightforward formula + return x * x + y * y + z * z; + } + + /** {@inheritDoc} */ + public double getNormInf() { + return FastMath.max(FastMath.max(FastMath.abs(x), FastMath.abs(y)), FastMath.abs(z)); + } + + /** Get the azimuth of the vector. + * @return azimuth (α) of the vector, between -π and +π + * @see #Vector3D(double, double) + */ + public double getAlpha() { + return FastMath.atan2(y, x); + } + + /** Get the elevation of the vector. + * @return elevation (δ) of the vector, between -π/2 and +π/2 + * @see #Vector3D(double, double) + */ + public double getDelta() { + return FastMath.asin(z / getNorm()); + } + + /** {@inheritDoc} */ + public Vector3D add(final Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + return new Vector3D(x + v3.x, y + v3.y, z + v3.z); + } + + /** {@inheritDoc} */ + public Vector3D add(double factor, final Vector<Euclidean3D> v) { + return new Vector3D(1, this, factor, (Vector3D) v); + } + + /** {@inheritDoc} */ + public Vector3D subtract(final Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + return new Vector3D(x - v3.x, y - v3.y, z - v3.z); + } + + /** {@inheritDoc} */ + public Vector3D subtract(final double factor, final Vector<Euclidean3D> v) { + return new Vector3D(1, this, -factor, (Vector3D) v); + } + + /** {@inheritDoc} */ + public Vector3D normalize() throws MathArithmeticException { + double s = getNorm(); + if (s == 0) { + throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); + } + return scalarMultiply(1 / s); + } + + /** Get a vector orthogonal to the instance. + * <p>There are an infinite number of normalized vectors orthogonal + * to the instance. This method picks up one of them almost + * arbitrarily. It is useful when one needs to compute a reference + * frame with one of the axes in a predefined direction. The + * following example shows how to build a frame having the k axis + * aligned with the known vector u : + * <pre><code> + * Vector3D k = u.normalize(); + * Vector3D i = k.orthogonal(); + * Vector3D j = Vector3D.crossProduct(k, i); + * </code></pre></p> + * @return a new normalized vector orthogonal to the instance + * @exception MathArithmeticException if the norm of the instance is null + */ + public Vector3D orthogonal() throws MathArithmeticException { + + double threshold = 0.6 * getNorm(); + if (threshold == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + if (FastMath.abs(x) <= threshold) { + double inverse = 1 / FastMath.sqrt(y * y + z * z); + return new Vector3D(0, inverse * z, -inverse * y); + } else if (FastMath.abs(y) <= threshold) { + double inverse = 1 / FastMath.sqrt(x * x + z * z); + return new Vector3D(-inverse * z, 0, inverse * x); + } + double inverse = 1 / FastMath.sqrt(x * x + y * y); + return new Vector3D(inverse * y, -inverse * x, 0); + + } + + /** Compute the angular separation between two vectors. + * <p>This method computes the angular separation between two + * vectors using the dot product for well separated vectors and the + * cross product for almost aligned vectors. This allows to have a + * good accuracy in all cases, even for vectors very close to each + * other.</p> + * @param v1 first vector + * @param v2 second vector + * @return angular separation between v1 and v2 + * @exception MathArithmeticException if either vector has a null norm + */ + public static double angle(Vector3D v1, Vector3D v2) throws MathArithmeticException { + + double normProduct = v1.getNorm() * v2.getNorm(); + if (normProduct == 0) { + throw new MathArithmeticException(LocalizedFormats.ZERO_NORM); + } + + double dot = v1.dotProduct(v2); + double threshold = normProduct * 0.9999; + if ((dot < -threshold) || (dot > threshold)) { + // the vectors are almost aligned, compute using the sine + Vector3D v3 = crossProduct(v1, v2); + if (dot >= 0) { + return FastMath.asin(v3.getNorm() / normProduct); + } + return FastMath.PI - FastMath.asin(v3.getNorm() / normProduct); + } + + // the vectors are sufficiently separated to use the cosine + return FastMath.acos(dot / normProduct); + + } + + /** {@inheritDoc} */ + public Vector3D negate() { + return new Vector3D(-x, -y, -z); + } + + /** {@inheritDoc} */ + public Vector3D scalarMultiply(double a) { + return new Vector3D(a * x, a * y, a * z); + } + + /** {@inheritDoc} */ + public boolean isNaN() { + return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z); + } + + /** {@inheritDoc} */ + public boolean isInfinite() { + return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y) || Double.isInfinite(z)); + } + + /** + * Test for the equality of two 3D vectors. + * <p> + * If all coordinates of two 3D vectors are exactly the same, and none are + * <code>Double.NaN</code>, the two 3D vectors are considered to be equal. + * </p> + * <p> + * <code>NaN</code> coordinates are considered to affect globally the vector + * and be equals to each other - i.e, if either (or all) coordinates of the + * 3D vector are equal to <code>Double.NaN</code>, the 3D vector is equal to + * {@link #NaN}. + * </p> + * + * @param other Object to test for equality to this + * @return true if two 3D vector objects are equal, false if + * object is null, not an instance of Vector3D, or + * not equal to this Vector3D instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof Vector3D) { + final Vector3D rhs = (Vector3D)other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return (x == rhs.x) && (y == rhs.y) && (z == rhs.z); + } + return false; + } + + /** + * Get a hashCode for the 3D vector. + * <p> + * All NaN values have the same hash code.</p> + * + * @return a hash code value for this object + */ + @Override + public int hashCode() { + if (isNaN()) { + return 642; + } + return 643 * (164 * MathUtils.hash(x) + 3 * MathUtils.hash(y) + MathUtils.hash(z)); + } + + /** {@inheritDoc} + * <p> + * The implementation uses specific multiplication and addition + * algorithms to preserve accuracy and reduce cancellation effects. + * It should be very accurate even for nearly orthogonal vectors. + * </p> + * @see MathArrays#linearCombination(double, double, double, double, double, double) + */ + public double dotProduct(final Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + return MathArrays.linearCombination(x, v3.x, y, v3.y, z, v3.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 Vector3D crossProduct(final Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + return new Vector3D(MathArrays.linearCombination(y, v3.z, -z, v3.y), + MathArrays.linearCombination(z, v3.x, -x, v3.z), + MathArrays.linearCombination(x, v3.y, -y, v3.x)); + } + + /** {@inheritDoc} */ + public double distance1(Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + final double dx = FastMath.abs(v3.x - x); + final double dy = FastMath.abs(v3.y - y); + final double dz = FastMath.abs(v3.z - z); + return dx + dy + dz; + } + + /** {@inheritDoc} */ + public double distance(Vector<Euclidean3D> v) { + return distance((Point<Euclidean3D>) v); + } + + /** {@inheritDoc} */ + public double distance(Point<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + final double dx = v3.x - x; + final double dy = v3.y - y; + final double dz = v3.z - z; + return FastMath.sqrt(dx * dx + dy * dy + dz * dz); + } + + /** {@inheritDoc} */ + public double distanceInf(Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + final double dx = FastMath.abs(v3.x - x); + final double dy = FastMath.abs(v3.y - y); + final double dz = FastMath.abs(v3.z - z); + return FastMath.max(FastMath.max(dx, dy), dz); + } + + /** {@inheritDoc} */ + public double distanceSq(Vector<Euclidean3D> v) { + final Vector3D v3 = (Vector3D) v; + final double dx = v3.x - x; + final double dy = v3.y - y; + final double dz = v3.z - z; + return dx * dx + dy * dy + dz * dz; + } + + /** Compute the dot-product of two vectors. + * @param v1 first vector + * @param v2 second vector + * @return the dot product v1.v2 + */ + public static double dotProduct(Vector3D v1, Vector3D v2) { + return v1.dotProduct(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 Vector3D crossProduct(final Vector3D v1, final Vector3D v2) { + return v1.crossProduct(v2); + } + + /** Compute the distance between two vectors according to the L<sub>1</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm1()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @return the distance between v1 and v2 according to the L<sub>1</sub> norm + */ + public static double distance1(Vector3D v1, Vector3D v2) { + return v1.distance1(v2); + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @return the distance between v1 and v2 according to the L<sub>2</sub> norm + */ + public static double distance(Vector3D v1, Vector3D v2) { + return v1.distance(v2); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @return the distance between v1 and v2 according to the L<sub>∞</sub> norm + */ + public static double distanceInf(Vector3D v1, Vector3D v2) { + return v1.distanceInf(v2); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>v1.subtract(v2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param v1 first vector + * @param v2 second vector + * @return the square of the distance between v1 and v2 + */ + public static double distanceSq(Vector3D v1, Vector3D v2) { + return v1.distanceSq(v2); + } + + /** Get a string representation of this vector. + * @return a string representation of this vector + */ + @Override + public String toString() { + return Vector3DFormat.getInstance().format(this); + } + + /** {@inheritDoc} */ + public String toString(final NumberFormat format) { + return new Vector3DFormat(format).format(this); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DFormat.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DFormat.java new file mode 100644 index 0000000..da3f71e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Vector3DFormat.java @@ -0,0 +1,155 @@ +/* + * 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.FieldPosition; +import java.text.NumberFormat; +import java.text.ParsePosition; +import java.util.Locale; + +import org.apache.commons.math3.exception.MathParseException; +import org.apache.commons.math3.geometry.Vector; +import org.apache.commons.math3.geometry.VectorFormat; +import org.apache.commons.math3.util.CompositeFormat; + +/** + * Formats a 3D vector in components list format "{x; y; z}". + * <p>The prefix and suffix "{" and "}" and the separator "; " can be replaced by + * any user-defined strings. The number format for components can be configured.</p> + * <p>White space is ignored at parse time, even if it is in the prefix, suffix + * or separator specifications. So even if the default separator does include a space + * character that is used at format time, both input string "{1;1;1}" and + * " { 1 ; 1 ; 1 } " will be parsed without error and the same vector will be + * returned. In the second case, however, the parse position after parsing will be + * just after the closing curly brace, i.e. just before the trailing space.</p> + * <p><b>Note:</b> using "," as a separator may interfere with the grouping separator + * of the default {@link NumberFormat} for the current locale. Thus it is advised + * to use a {@link NumberFormat} instance with disabled grouping in such a case.</p> + * + */ +public class Vector3DFormat extends VectorFormat<Euclidean3D> { + + /** + * Create an instance with default settings. + * <p>The instance uses the default prefix, suffix and separator: + * "{", "}", and "; " and the default number format for components.</p> + */ + public Vector3DFormat() { + super(DEFAULT_PREFIX, DEFAULT_SUFFIX, DEFAULT_SEPARATOR, + CompositeFormat.getDefaultNumberFormat()); + } + + /** + * Create an instance with a custom number format for components. + * @param format the custom format for components. + */ + public Vector3DFormat(final NumberFormat format) { + super(DEFAULT_PREFIX, DEFAULT_SUFFIX, DEFAULT_SEPARATOR, format); + } + + /** + * Create an instance with custom prefix, suffix and separator. + * @param prefix prefix to use instead of the default "{" + * @param suffix suffix to use instead of the default "}" + * @param separator separator to use instead of the default "; " + */ + public Vector3DFormat(final String prefix, final String suffix, + final String separator) { + super(prefix, suffix, separator, CompositeFormat.getDefaultNumberFormat()); + } + + /** + * Create an instance with custom prefix, suffix, separator and format + * for components. + * @param prefix prefix to use instead of the default "{" + * @param suffix suffix to use instead of the default "}" + * @param separator separator to use instead of the default "; " + * @param format the custom format for components. + */ + public Vector3DFormat(final String prefix, final String suffix, + final String separator, final NumberFormat format) { + super(prefix, suffix, separator, format); + } + + /** + * Returns the default 3D vector format for the current locale. + * @return the default 3D vector format. + */ + public static Vector3DFormat getInstance() { + return getInstance(Locale.getDefault()); + } + + /** + * Returns the default 3D vector format for the given locale. + * @param locale the specific locale used by the format. + * @return the 3D vector format specific to the given locale. + */ + public static Vector3DFormat getInstance(final Locale locale) { + return new Vector3DFormat(CompositeFormat.getDefaultNumberFormat(locale)); + } + + /** + * Formats a {@link Vector3D} object to produce a string. + * @param vector the object to format. + * @param toAppendTo where the text is to be appended + * @param pos On input: an alignment field, if desired. On output: the + * offsets of the alignment field + * @return the value passed in as toAppendTo. + */ + @Override + public StringBuffer format(final Vector<Euclidean3D> vector, final StringBuffer toAppendTo, + final FieldPosition pos) { + final Vector3D v3 = (Vector3D) vector; + return format(toAppendTo, pos, v3.getX(), v3.getY(), v3.getZ()); + } + + /** + * Parses a string to produce a {@link Vector3D} object. + * @param source the string to parse + * @return the parsed {@link Vector3D} object. + * @throws MathParseException if the beginning of the specified string + * cannot be parsed. + */ + @Override + public Vector3D parse(final String source) throws MathParseException { + ParsePosition parsePosition = new ParsePosition(0); + Vector3D result = parse(source, parsePosition); + if (parsePosition.getIndex() == 0) { + throw new MathParseException(source, + parsePosition.getErrorIndex(), + Vector3D.class); + } + return result; + } + + /** + * Parses a string to produce a {@link Vector3D} object. + * @param source the string to parse + * @param pos input/ouput parsing parameter. + * @return the parsed {@link Vector3D} object. + */ + @Override + public Vector3D parse(final String source, final ParsePosition pos) { + final double[] coordinates = parseCoordinates(3, source, pos); + if (coordinates == null) { + return null; + } + return new Vector3D(coordinates[0], coordinates[1], coordinates[2]); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/package-info.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/package-info.java new file mode 100644 index 0000000..eaa3c6a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/package-info.java @@ -0,0 +1,24 @@ +/* + * 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. + */ +/** + * + * <p> + * This package provides basic 3D geometry components. + * </p> + * + */ +package org.apache.commons.math3.geometry.euclidean.threed; |