diff options
Diffstat (limited to 'src/main/java/org/apache/commons/math3/geometry/euclidean')
44 files changed, 13570 insertions, 0 deletions
diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Euclidean1D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Euclidean1D.java new file mode 100644 index 0000000..14d130d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Euclidean1D.java @@ -0,0 +1,100 @@ +/* + * 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.oned; + +import java.io.Serializable; + +import org.apache.commons.math3.exception.MathUnsupportedOperationException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.Space; + +/** + * This class implements a one-dimensional space. + * @since 3.0 + */ +public class Euclidean1D implements Serializable, Space { + + /** Serializable version identifier. */ + private static final long serialVersionUID = -1178039568877797126L; + + /** Private constructor for the singleton. + */ + private Euclidean1D() { + } + + /** Get the unique instance. + * @return the unique instance + */ + public static Euclidean1D getInstance() { + return LazyHolder.INSTANCE; + } + + /** {@inheritDoc} */ + public int getDimension() { + return 1; + } + + /** {@inheritDoc} + * <p> + * As the 1-dimension Euclidean space does not have proper sub-spaces, + * this method always throws a {@link NoSubSpaceException} + * </p> + * @return nothing + * @throws NoSubSpaceException in all cases + */ + public Space getSubSpace() throws NoSubSpaceException { + throw new NoSubSpaceException(); + } + + // 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 Euclidean1D INSTANCE = new Euclidean1D(); + } + // CHECKSTYLE: resume HideUtilityClassConstructor + + /** Handle deserialization of the singleton. + * @return the singleton instance + */ + private Object readResolve() { + // return the singleton instance + return LazyHolder.INSTANCE; + } + + /** Specialized exception for inexistent sub-space. + * <p> + * This exception is thrown when attempting to get the sub-space of a one-dimensional space + * </p> + */ + public static class NoSubSpaceException extends MathUnsupportedOperationException { + + /** Serializable UID. */ + private static final long serialVersionUID = 20140225L; + + /** Simple constructor. + */ + public NoSubSpaceException() { + super(LocalizedFormats.NOT_SUPPORTED_IN_DIMENSION_N, 1); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Interval.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Interval.java new file mode 100644 index 0000000..ca15231 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Interval.java @@ -0,0 +1,135 @@ +/* + * 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.oned; + +import org.apache.commons.math3.geometry.partitioning.Region.Location; +import org.apache.commons.math3.exception.NumberIsTooSmallException; +import org.apache.commons.math3.exception.util.LocalizedFormats; + + +/** This class represents a 1D interval. + * @see IntervalsSet + * @since 3.0 + */ +public class Interval { + + /** The lower bound of the interval. */ + private final double lower; + + /** The upper bound of the interval. */ + private final double upper; + + /** Simple constructor. + * @param lower lower bound of the interval + * @param upper upper bound of the interval + */ + public Interval(final double lower, final double upper) { + if (upper < lower) { + throw new NumberIsTooSmallException(LocalizedFormats.ENDPOINTS_NOT_AN_INTERVAL, + upper, lower, true); + } + this.lower = lower; + this.upper = upper; + } + + /** Get the lower bound of the interval. + * @return lower bound of the interval + * @since 3.1 + */ + public double getInf() { + return lower; + } + + /** Get the lower bound of the interval. + * @return lower bound of the interval + * @deprecated as of 3.1, replaced by {@link #getInf()} + */ + @Deprecated + public double getLower() { + return getInf(); + } + + /** Get the upper bound of the interval. + * @return upper bound of the interval + * @since 3.1 + */ + public double getSup() { + return upper; + } + + /** Get the upper bound of the interval. + * @return upper bound of the interval + * @deprecated as of 3.1, replaced by {@link #getSup()} + */ + @Deprecated + public double getUpper() { + return getSup(); + } + + /** Get the size of the interval. + * @return size of the interval + * @since 3.1 + */ + public double getSize() { + return upper - lower; + } + + /** Get the length of the interval. + * @return length of the interval + * @deprecated as of 3.1, replaced by {@link #getSize()} + */ + @Deprecated + public double getLength() { + return getSize(); + } + + /** Get the barycenter of the interval. + * @return barycenter of the interval + * @since 3.1 + */ + public double getBarycenter() { + return 0.5 * (lower + upper); + } + + /** Get the midpoint of the interval. + * @return midpoint of the interval + * @deprecated as of 3.1, replaced by {@link #getBarycenter()} + */ + @Deprecated + public double getMidPoint() { + return getBarycenter(); + } + + /** Check a point with respect to the interval. + * @param point point to check + * @param tolerance tolerance below which points are considered to + * belong to the boundary + * @return a code representing the point status: either {@link + * Location#INSIDE}, {@link Location#OUTSIDE} or {@link Location#BOUNDARY} + * @since 3.1 + */ + public Location checkPoint(final double point, final double tolerance) { + if (point < lower - tolerance || point > upper + tolerance) { + return Location.OUTSIDE; + } else if (point > lower + tolerance && point < upper - tolerance) { + return Location.INSIDE; + } else { + return Location.BOUNDARY; + } + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/IntervalsSet.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/IntervalsSet.java new file mode 100644 index 0000000..5ce7edb --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/IntervalsSet.java @@ -0,0 +1,686 @@ +/* + * 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.oned; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Iterator; +import java.util.List; +import java.util.NoSuchElementException; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.partitioning.AbstractRegion; +import org.apache.commons.math3.geometry.partitioning.BSPTree; +import org.apache.commons.math3.geometry.partitioning.BoundaryProjection; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.util.Precision; + +/** This class represents a 1D region: a set of intervals. + * @since 3.0 + */ +public class IntervalsSet extends AbstractRegion<Euclidean1D, Euclidean1D> implements Iterable<double[]> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Build an intervals set representing the whole real line. + * @param tolerance tolerance below which points are considered identical. + * @since 3.3 + */ + public IntervalsSet(final double tolerance) { + super(tolerance); + } + + /** Build an intervals set corresponding to a single interval. + * @param lower lower bound of the interval, must be lesser or equal + * to {@code upper} (may be {@code Double.NEGATIVE_INFINITY}) + * @param upper upper bound of the interval, must be greater or equal + * to {@code lower} (may be {@code Double.POSITIVE_INFINITY}) + * @param tolerance tolerance below which points are considered identical. + * @since 3.3 + */ + public IntervalsSet(final double lower, final double upper, final double tolerance) { + super(buildTree(lower, upper, tolerance), tolerance); + } + + /** Build an intervals set from an inside/outside 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 intervals set + * @param tolerance tolerance below which points are considered identical. + * @since 3.3 + */ + public IntervalsSet(final BSPTree<Euclidean1D> tree, final double tolerance) { + super(tree, tolerance); + } + + /** Build an intervals 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 polygons with holes + * or a set of disjoints 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 + * org.apache.commons.math3.geometry.partitioning.Region#checkPoint(org.apache.commons.math3.geometry.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 + * @param tolerance tolerance below which points are considered identical. + * @since 3.3 + */ + public IntervalsSet(final Collection<SubHyperplane<Euclidean1D>> boundary, + final double tolerance) { + super(boundary, tolerance); + } + + /** Build an intervals set representing the whole real line. + * @deprecated as of 3.1 replaced with {@link #IntervalsSet(double)} + */ + @Deprecated + public IntervalsSet() { + this(DEFAULT_TOLERANCE); + } + + /** Build an intervals set corresponding to a single interval. + * @param lower lower bound of the interval, must be lesser or equal + * to {@code upper} (may be {@code Double.NEGATIVE_INFINITY}) + * @param upper upper bound of the interval, must be greater or equal + * to {@code lower} (may be {@code Double.POSITIVE_INFINITY}) + * @deprecated as of 3.3 replaced with {@link #IntervalsSet(double, double, double)} + */ + @Deprecated + public IntervalsSet(final double lower, final double upper) { + this(lower, upper, DEFAULT_TOLERANCE); + } + + /** Build an intervals set from an inside/outside 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 intervals set + * @deprecated as of 3.3, replaced with {@link #IntervalsSet(BSPTree, double)} + */ + @Deprecated + public IntervalsSet(final BSPTree<Euclidean1D> tree) { + this(tree, DEFAULT_TOLERANCE); + } + + /** Build an intervals 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 polygons with holes + * or a set of disjoints 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 + * org.apache.commons.math3.geometry.partitioning.Region#checkPoint(org.apache.commons.math3.geometry.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 + * @deprecated as of 3.3, replaced with {@link #IntervalsSet(Collection, double)} + */ + @Deprecated + public IntervalsSet(final Collection<SubHyperplane<Euclidean1D>> boundary) { + this(boundary, DEFAULT_TOLERANCE); + } + + /** Build an inside/outside tree representing a single interval. + * @param lower lower bound of the interval, must be lesser or equal + * to {@code upper} (may be {@code Double.NEGATIVE_INFINITY}) + * @param upper upper bound of the interval, must be greater or equal + * to {@code lower} (may be {@code Double.POSITIVE_INFINITY}) + * @param tolerance tolerance below which points are considered identical. + * @return the built tree + */ + private static BSPTree<Euclidean1D> buildTree(final double lower, final double upper, + final double tolerance) { + if (Double.isInfinite(lower) && (lower < 0)) { + if (Double.isInfinite(upper) && (upper > 0)) { + // the tree must cover the whole real line + return new BSPTree<Euclidean1D>(Boolean.TRUE); + } + // the tree must be open on the negative infinity side + final SubHyperplane<Euclidean1D> upperCut = + new OrientedPoint(new Vector1D(upper), true, tolerance).wholeHyperplane(); + return new BSPTree<Euclidean1D>(upperCut, + new BSPTree<Euclidean1D>(Boolean.FALSE), + new BSPTree<Euclidean1D>(Boolean.TRUE), + null); + } + final SubHyperplane<Euclidean1D> lowerCut = + new OrientedPoint(new Vector1D(lower), false, tolerance).wholeHyperplane(); + if (Double.isInfinite(upper) && (upper > 0)) { + // the tree must be open on the positive infinity side + return new BSPTree<Euclidean1D>(lowerCut, + new BSPTree<Euclidean1D>(Boolean.FALSE), + new BSPTree<Euclidean1D>(Boolean.TRUE), + null); + } + + // the tree must be bounded on the two sides + final SubHyperplane<Euclidean1D> upperCut = + new OrientedPoint(new Vector1D(upper), true, tolerance).wholeHyperplane(); + return new BSPTree<Euclidean1D>(lowerCut, + new BSPTree<Euclidean1D>(Boolean.FALSE), + new BSPTree<Euclidean1D>(upperCut, + new BSPTree<Euclidean1D>(Boolean.FALSE), + new BSPTree<Euclidean1D>(Boolean.TRUE), + null), + null); + + } + + /** {@inheritDoc} */ + @Override + public IntervalsSet buildNew(final BSPTree<Euclidean1D> tree) { + return new IntervalsSet(tree, getTolerance()); + } + + /** {@inheritDoc} */ + @Override + protected void computeGeometricalProperties() { + if (getTree(false).getCut() == null) { + setBarycenter((Point<Euclidean1D>) Vector1D.NaN); + setSize(((Boolean) getTree(false).getAttribute()) ? Double.POSITIVE_INFINITY : 0); + } else { + double size = 0.0; + double sum = 0.0; + for (final Interval interval : asList()) { + size += interval.getSize(); + sum += interval.getSize() * interval.getBarycenter(); + } + setSize(size); + if (Double.isInfinite(size)) { + setBarycenter((Point<Euclidean1D>) Vector1D.NaN); + } else if (size >= Precision.SAFE_MIN) { + setBarycenter((Point<Euclidean1D>) new Vector1D(sum / size)); + } else { + setBarycenter((Point<Euclidean1D>) ((OrientedPoint) getTree(false).getCut().getHyperplane()).getLocation()); + } + } + } + + /** Get the lowest value belonging to the instance. + * @return lowest value belonging to the instance + * ({@code Double.NEGATIVE_INFINITY} if the instance doesn't + * have any low bound, {@code Double.POSITIVE_INFINITY} if the + * instance is empty) + */ + public double getInf() { + BSPTree<Euclidean1D> node = getTree(false); + double inf = Double.POSITIVE_INFINITY; + while (node.getCut() != null) { + final OrientedPoint op = (OrientedPoint) node.getCut().getHyperplane(); + inf = op.getLocation().getX(); + node = op.isDirect() ? node.getMinus() : node.getPlus(); + } + return ((Boolean) node.getAttribute()) ? Double.NEGATIVE_INFINITY : inf; + } + + /** Get the highest value belonging to the instance. + * @return highest value belonging to the instance + * ({@code Double.POSITIVE_INFINITY} if the instance doesn't + * have any high bound, {@code Double.NEGATIVE_INFINITY} if the + * instance is empty) + */ + public double getSup() { + BSPTree<Euclidean1D> node = getTree(false); + double sup = Double.NEGATIVE_INFINITY; + while (node.getCut() != null) { + final OrientedPoint op = (OrientedPoint) node.getCut().getHyperplane(); + sup = op.getLocation().getX(); + node = op.isDirect() ? node.getPlus() : node.getMinus(); + } + return ((Boolean) node.getAttribute()) ? Double.POSITIVE_INFINITY : sup; + } + + /** {@inheritDoc} + * @since 3.3 + */ + @Override + public BoundaryProjection<Euclidean1D> projectToBoundary(final Point<Euclidean1D> point) { + + // get position of test point + final double x = ((Vector1D) point).getX(); + + double previous = Double.NEGATIVE_INFINITY; + for (final double[] a : this) { + if (x < a[0]) { + // the test point lies between the previous and the current intervals + // offset will be positive + final double previousOffset = x - previous; + final double currentOffset = a[0] - x; + if (previousOffset < currentOffset) { + return new BoundaryProjection<Euclidean1D>(point, finiteOrNullPoint(previous), previousOffset); + } else { + return new BoundaryProjection<Euclidean1D>(point, finiteOrNullPoint(a[0]), currentOffset); + } + } else if (x <= a[1]) { + // the test point lies within the current interval + // offset will be negative + final double offset0 = a[0] - x; + final double offset1 = x - a[1]; + if (offset0 < offset1) { + return new BoundaryProjection<Euclidean1D>(point, finiteOrNullPoint(a[1]), offset1); + } else { + return new BoundaryProjection<Euclidean1D>(point, finiteOrNullPoint(a[0]), offset0); + } + } + previous = a[1]; + } + + // the test point if past the last sub-interval + return new BoundaryProjection<Euclidean1D>(point, finiteOrNullPoint(previous), x - previous); + + } + + /** Build a finite point. + * @param x abscissa of the point + * @return a new point for finite abscissa, null otherwise + */ + private Vector1D finiteOrNullPoint(final double x) { + return Double.isInfinite(x) ? null : new Vector1D(x); + } + + /** Build an ordered list of intervals representing the instance. + * <p>This method builds this intervals set as an ordered list of + * {@link Interval Interval} elements. If the intervals set has no + * lower limit, the first interval will have its low bound equal to + * {@code Double.NEGATIVE_INFINITY}. If the intervals set has + * no upper limit, the last interval will have its upper bound equal + * to {@code Double.POSITIVE_INFINITY}. An empty tree will + * build an empty list while a tree representing the whole real line + * will build a one element list with both bounds being + * infinite.</p> + * @return a new ordered list containing {@link Interval Interval} + * elements + */ + public List<Interval> asList() { + final List<Interval> list = new ArrayList<Interval>(); + for (final double[] a : this) { + list.add(new Interval(a[0], a[1])); + } + return list; + } + + /** Get the first leaf node of a tree. + * @param root tree root + * @return first leaf node + */ + private BSPTree<Euclidean1D> getFirstLeaf(final BSPTree<Euclidean1D> root) { + + if (root.getCut() == null) { + return root; + } + + // find the smallest internal node + BSPTree<Euclidean1D> smallest = null; + for (BSPTree<Euclidean1D> n = root; n != null; n = previousInternalNode(n)) { + smallest = n; + } + + return leafBefore(smallest); + + } + + /** Get the node corresponding to the first interval boundary. + * @return smallest internal node, + * or null if there are no internal nodes (i.e. the set is either empty or covers the real line) + */ + private BSPTree<Euclidean1D> getFirstIntervalBoundary() { + + // start search at the tree root + BSPTree<Euclidean1D> node = getTree(false); + if (node.getCut() == null) { + return null; + } + + // walk tree until we find the smallest internal node + node = getFirstLeaf(node).getParent(); + + // walk tree until we find an interval boundary + while (node != null && !(isIntervalStart(node) || isIntervalEnd(node))) { + node = nextInternalNode(node); + } + + return node; + + } + + /** Check if an internal node corresponds to the start abscissa of an interval. + * @param node internal node to check + * @return true if the node corresponds to the start abscissa of an interval + */ + private boolean isIntervalStart(final BSPTree<Euclidean1D> node) { + + if ((Boolean) leafBefore(node).getAttribute()) { + // it has an inside cell before it, it may end an interval but not start it + return false; + } + + if (!(Boolean) leafAfter(node).getAttribute()) { + // it has an outside cell after it, it is a dummy cut away from real intervals + return false; + } + + // the cell has an outside before and an inside after it + // it is the start of an interval + return true; + + } + + /** Check if an internal node corresponds to the end abscissa of an interval. + * @param node internal node to check + * @return true if the node corresponds to the end abscissa of an interval + */ + private boolean isIntervalEnd(final BSPTree<Euclidean1D> node) { + + if (!(Boolean) leafBefore(node).getAttribute()) { + // it has an outside cell before it, it may start an interval but not end it + return false; + } + + if ((Boolean) leafAfter(node).getAttribute()) { + // it has an inside cell after it, it is a dummy cut in the middle of an interval + return false; + } + + // the cell has an inside before and an outside after it + // it is the end of an interval + return true; + + } + + /** Get the next internal node. + * @param node current internal node + * @return next internal node in ascending order, or null + * if this is the last internal node + */ + private BSPTree<Euclidean1D> nextInternalNode(BSPTree<Euclidean1D> node) { + + if (childAfter(node).getCut() != null) { + // the next node is in the sub-tree + return leafAfter(node).getParent(); + } + + // there is nothing left deeper in the tree, we backtrack + while (isAfterParent(node)) { + node = node.getParent(); + } + return node.getParent(); + + } + + /** Get the previous internal node. + * @param node current internal node + * @return previous internal node in ascending order, or null + * if this is the first internal node + */ + private BSPTree<Euclidean1D> previousInternalNode(BSPTree<Euclidean1D> node) { + + if (childBefore(node).getCut() != null) { + // the next node is in the sub-tree + return leafBefore(node).getParent(); + } + + // there is nothing left deeper in the tree, we backtrack + while (isBeforeParent(node)) { + node = node.getParent(); + } + return node.getParent(); + + } + + /** Find the leaf node just before an internal node. + * @param node internal node at which the sub-tree starts + * @return leaf node just before the internal node + */ + private BSPTree<Euclidean1D> leafBefore(BSPTree<Euclidean1D> node) { + + node = childBefore(node); + while (node.getCut() != null) { + node = childAfter(node); + } + + return node; + + } + + /** Find the leaf node just after an internal node. + * @param node internal node at which the sub-tree starts + * @return leaf node just after the internal node + */ + private BSPTree<Euclidean1D> leafAfter(BSPTree<Euclidean1D> node) { + + node = childAfter(node); + while (node.getCut() != null) { + node = childBefore(node); + } + + return node; + + } + + /** Check if a node is the child before its parent in ascending order. + * @param node child node considered + * @return true is the node has a parent end is before it in ascending order + */ + private boolean isBeforeParent(final BSPTree<Euclidean1D> node) { + final BSPTree<Euclidean1D> parent = node.getParent(); + if (parent == null) { + return false; + } else { + return node == childBefore(parent); + } + } + + /** Check if a node is the child after its parent in ascending order. + * @param node child node considered + * @return true is the node has a parent end is after it in ascending order + */ + private boolean isAfterParent(final BSPTree<Euclidean1D> node) { + final BSPTree<Euclidean1D> parent = node.getParent(); + if (parent == null) { + return false; + } else { + return node == childAfter(parent); + } + } + + /** Find the child node just before an internal node. + * @param node internal node at which the sub-tree starts + * @return child node just before the internal node + */ + private BSPTree<Euclidean1D> childBefore(BSPTree<Euclidean1D> node) { + if (isDirect(node)) { + // smaller abscissas are on minus side, larger abscissas are on plus side + return node.getMinus(); + } else { + // smaller abscissas are on plus side, larger abscissas are on minus side + return node.getPlus(); + } + } + + /** Find the child node just after an internal node. + * @param node internal node at which the sub-tree starts + * @return child node just after the internal node + */ + private BSPTree<Euclidean1D> childAfter(BSPTree<Euclidean1D> node) { + if (isDirect(node)) { + // smaller abscissas are on minus side, larger abscissas are on plus side + return node.getPlus(); + } else { + // smaller abscissas are on plus side, larger abscissas are on minus side + return node.getMinus(); + } + } + + /** Check if an internal node has a direct oriented point. + * @param node internal node to check + * @return true if the oriented point is direct + */ + private boolean isDirect(final BSPTree<Euclidean1D> node) { + return ((OrientedPoint) node.getCut().getHyperplane()).isDirect(); + } + + /** Get the abscissa of an internal node. + * @param node internal node to check + * @return abscissa + */ + private double getAngle(final BSPTree<Euclidean1D> node) { + return ((OrientedPoint) node.getCut().getHyperplane()).getLocation().getX(); + } + + /** {@inheritDoc} + * <p> + * The iterator returns the limit values of sub-intervals in ascending order. + * </p> + * <p> + * The iterator does <em>not</em> support the optional {@code remove} operation. + * </p> + * @since 3.3 + */ + public Iterator<double[]> iterator() { + return new SubIntervalsIterator(); + } + + /** Local iterator for sub-intervals. */ + private class SubIntervalsIterator implements Iterator<double[]> { + + /** Current node. */ + private BSPTree<Euclidean1D> current; + + /** Sub-interval no yet returned. */ + private double[] pending; + + /** Simple constructor. + */ + SubIntervalsIterator() { + + current = getFirstIntervalBoundary(); + + if (current == null) { + // all the leaf tree nodes share the same inside/outside status + if ((Boolean) getFirstLeaf(getTree(false)).getAttribute()) { + // it is an inside node, it represents the full real line + pending = new double[] { + Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY + }; + } else { + pending = null; + } + } else if (isIntervalEnd(current)) { + // the first boundary is an interval end, + // so the first interval starts at infinity + pending = new double[] { + Double.NEGATIVE_INFINITY, getAngle(current) + }; + } else { + selectPending(); + } + } + + /** Walk the tree to select the pending sub-interval. + */ + private void selectPending() { + + // look for the start of the interval + BSPTree<Euclidean1D> start = current; + while (start != null && !isIntervalStart(start)) { + start = nextInternalNode(start); + } + + if (start == null) { + // we have exhausted the iterator + current = null; + pending = null; + return; + } + + // look for the end of the interval + BSPTree<Euclidean1D> end = start; + while (end != null && !isIntervalEnd(end)) { + end = nextInternalNode(end); + } + + if (end != null) { + + // we have identified the interval + pending = new double[] { + getAngle(start), getAngle(end) + }; + + // prepare search for next interval + current = end; + + } else { + + // the final interval is open toward infinity + pending = new double[] { + getAngle(start), Double.POSITIVE_INFINITY + }; + + // there won't be any other intervals + current = null; + + } + + } + + /** {@inheritDoc} */ + public boolean hasNext() { + return pending != null; + } + + /** {@inheritDoc} */ + public double[] next() { + if (pending == null) { + throw new NoSuchElementException(); + } + final double[] next = pending; + selectPending(); + return next; + } + + /** {@inheritDoc} */ + public void remove() { + throw new UnsupportedOperationException(); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/OrientedPoint.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/OrientedPoint.java new file mode 100644 index 0000000..512bf5d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/OrientedPoint.java @@ -0,0 +1,153 @@ +/* + * 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.oned; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Vector; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; + +/** This class represents a 1D oriented hyperplane. + * <p>An hyperplane in 1D is a simple point, its orientation being a + * boolean.</p> + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.0 + */ +public class OrientedPoint implements Hyperplane<Euclidean1D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Vector location. */ + private Vector1D location; + + /** Orientation. */ + private boolean direct; + + /** Tolerance below which points are considered to belong to the hyperplane. */ + private final double tolerance; + + /** Simple constructor. + * @param location location of the hyperplane + * @param direct if true, the plus side of the hyperplane is towards + * abscissas greater than {@code location} + * @param tolerance tolerance below which points are considered to belong to the hyperplane + * @since 3.3 + */ + public OrientedPoint(final Vector1D location, final boolean direct, final double tolerance) { + this.location = location; + this.direct = direct; + this.tolerance = tolerance; + } + + /** Simple constructor. + * @param location location of the hyperplane + * @param direct if true, the plus side of the hyperplane is towards + * abscissas greater than {@code location} + * @deprecated as of 3.3, replaced with {@link #OrientedPoint(Vector1D, boolean, double)} + */ + @Deprecated + public OrientedPoint(final Vector1D location, final boolean direct) { + this(location, direct, DEFAULT_TOLERANCE); + } + + /** Copy the instance. + * <p>Since instances are immutable, this method directly returns + * the instance.</p> + * @return the instance itself + */ + public OrientedPoint copySelf() { + return this; + } + + /** Get the offset (oriented distance) of a vector. + * @param vector vector to check + * @return offset of the vector + */ + public double getOffset(Vector<Euclidean1D> vector) { + return getOffset((Point<Euclidean1D>) vector); + } + + /** {@inheritDoc} */ + public double getOffset(final Point<Euclidean1D> point) { + final double delta = ((Vector1D) point).getX() - location.getX(); + return direct ? delta : -delta; + } + + /** Build a region covering the whole hyperplane. + * <p>Since this class represent zero dimension spaces which does + * not have lower dimension sub-spaces, this method returns a dummy + * implementation of a {@link + * org.apache.commons.math3.geometry.partitioning.SubHyperplane SubHyperplane}. + * This implementation is only used to allow the {@link + * org.apache.commons.math3.geometry.partitioning.SubHyperplane + * SubHyperplane} class implementation to work properly, it should + * <em>not</em> be used otherwise.</p> + * @return a dummy sub hyperplane + */ + public SubOrientedPoint wholeHyperplane() { + return new SubOrientedPoint(this, null); + } + + /** Build a region covering the whole space. + * @return a region containing the instance (really an {@link + * IntervalsSet IntervalsSet} instance) + */ + public IntervalsSet wholeSpace() { + return new IntervalsSet(tolerance); + } + + /** {@inheritDoc} */ + public boolean sameOrientationAs(final Hyperplane<Euclidean1D> other) { + return !(direct ^ ((OrientedPoint) other).direct); + } + + /** {@inheritDoc} + * @since 3.3 + */ + public Point<Euclidean1D> project(Point<Euclidean1D> point) { + return location; + } + + /** {@inheritDoc} + * @since 3.3 + */ + public double getTolerance() { + return tolerance; + } + + /** Get the hyperplane location on the real line. + * @return the hyperplane location + */ + public Vector1D getLocation() { + return location; + } + + /** Check if the hyperplane orientation is direct. + * @return true if the plus side of the hyperplane is towards + * abscissae greater than hyperplane location + */ + public boolean isDirect() { + return direct; + } + + /** Revert the instance. + */ + public void revertSelf() { + direct = !direct; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/SubOrientedPoint.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/SubOrientedPoint.java new file mode 100644 index 0000000..a0288bb --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/SubOrientedPoint.java @@ -0,0 +1,72 @@ +/* + * 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.oned; + +import org.apache.commons.math3.geometry.partitioning.AbstractSubHyperplane; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.Region; + +/** This class represents sub-hyperplane for {@link OrientedPoint}. + * <p>An hyperplane in 1D is a simple point, its orientation being a + * boolean.</p> + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.0 + */ +public class SubOrientedPoint extends AbstractSubHyperplane<Euclidean1D, Euclidean1D> { + + /** Simple constructor. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + public SubOrientedPoint(final Hyperplane<Euclidean1D> hyperplane, + final Region<Euclidean1D> remainingRegion) { + super(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public double getSize() { + return 0; + } + + /** {@inheritDoc} */ + @Override + public boolean isEmpty() { + return false; + } + + /** {@inheritDoc} */ + @Override + protected AbstractSubHyperplane<Euclidean1D, Euclidean1D> buildNew(final Hyperplane<Euclidean1D> hyperplane, + final Region<Euclidean1D> remainingRegion) { + return new SubOrientedPoint(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public SplitSubHyperplane<Euclidean1D> split(final Hyperplane<Euclidean1D> hyperplane) { + final double global = hyperplane.getOffset(((OrientedPoint) getHyperplane()).getLocation()); + if (global < -1.0e-10) { + return new SplitSubHyperplane<Euclidean1D>(null, this); + } else if (global > 1.0e-10) { + return new SplitSubHyperplane<Euclidean1D>(this, null); + } else { + return new SplitSubHyperplane<Euclidean1D>(null, null); + } + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1D.java new file mode 100644 index 0000000..1ec7a4e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1D.java @@ -0,0 +1,356 @@ +/* + * 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.oned; + +import java.text.NumberFormat; + +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.MathUtils; + +/** This class represents a 1D vector. + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.0 + */ +public class Vector1D implements Vector<Euclidean1D> { + + /** Origin (coordinates: 0). */ + public static final Vector1D ZERO = new Vector1D(0.0); + + /** Unit (coordinates: 1). */ + public static final Vector1D ONE = new Vector1D(1.0); + + // CHECKSTYLE: stop ConstantName + /** A vector with all coordinates set to NaN. */ + public static final Vector1D NaN = new Vector1D(Double.NaN); + // CHECKSTYLE: resume ConstantName + + /** A vector with all coordinates set to positive infinity. */ + public static final Vector1D POSITIVE_INFINITY = + new Vector1D(Double.POSITIVE_INFINITY); + + /** A vector with all coordinates set to negative infinity. */ + public static final Vector1D NEGATIVE_INFINITY = + new Vector1D(Double.NEGATIVE_INFINITY); + + /** Serializable UID. */ + private static final long serialVersionUID = 7556674948671647925L; + + /** Abscissa. */ + private final double x; + + /** Simple constructor. + * Build a vector from its coordinates + * @param x abscissa + * @see #getX() + */ + public Vector1D(double x) { + this.x = x; + } + + /** 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 Vector1D(double a, Vector1D u) { + this.x = a * u.x; + } + + /** 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 Vector1D(double a1, Vector1D u1, double a2, Vector1D u2) { + this.x = a1 * u1.x + a2 * u2.x; + } + + /** 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 Vector1D(double a1, Vector1D u1, double a2, Vector1D u2, + double a3, Vector1D u3) { + this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x; + } + + /** 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 Vector1D(double a1, Vector1D u1, double a2, Vector1D u2, + double a3, Vector1D u3, double a4, Vector1D u4) { + this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x + a4 * u4.x; + } + + /** Get the abscissa of the vector. + * @return abscissa of the vector + * @see #Vector1D(double) + */ + public double getX() { + return x; + } + + /** {@inheritDoc} */ + public Space getSpace() { + return Euclidean1D.getInstance(); + } + + /** {@inheritDoc} */ + public Vector1D getZero() { + return ZERO; + } + + /** {@inheritDoc} */ + public double getNorm1() { + return FastMath.abs(x); + } + + /** {@inheritDoc} */ + public double getNorm() { + return FastMath.abs(x); + } + + /** {@inheritDoc} */ + public double getNormSq() { + return x * x; + } + + /** {@inheritDoc} */ + public double getNormInf() { + return FastMath.abs(x); + } + + /** {@inheritDoc} */ + public Vector1D add(Vector<Euclidean1D> v) { + Vector1D v1 = (Vector1D) v; + return new Vector1D(x + v1.getX()); + } + + /** {@inheritDoc} */ + public Vector1D add(double factor, Vector<Euclidean1D> v) { + Vector1D v1 = (Vector1D) v; + return new Vector1D(x + factor * v1.getX()); + } + + /** {@inheritDoc} */ + public Vector1D subtract(Vector<Euclidean1D> p) { + Vector1D p3 = (Vector1D) p; + return new Vector1D(x - p3.x); + } + + /** {@inheritDoc} */ + public Vector1D subtract(double factor, Vector<Euclidean1D> v) { + Vector1D v1 = (Vector1D) v; + return new Vector1D(x - factor * v1.getX()); + } + + /** {@inheritDoc} */ + public Vector1D normalize() throws MathArithmeticException { + double s = getNorm(); + if (s == 0) { + throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); + } + return scalarMultiply(1 / s); + } + /** {@inheritDoc} */ + public Vector1D negate() { + return new Vector1D(-x); + } + + /** {@inheritDoc} */ + public Vector1D scalarMultiply(double a) { + return new Vector1D(a * x); + } + + /** {@inheritDoc} */ + public boolean isNaN() { + return Double.isNaN(x); + } + + /** {@inheritDoc} */ + public boolean isInfinite() { + return !isNaN() && Double.isInfinite(x); + } + + /** {@inheritDoc} */ + public double distance1(Vector<Euclidean1D> p) { + Vector1D p3 = (Vector1D) p; + final double dx = FastMath.abs(p3.x - x); + return dx; + } + + /** {@inheritDoc} + * @deprecated as of 3.3, replaced with {@link #distance(Point)} + */ + @Deprecated + public double distance(Vector<Euclidean1D> p) { + return distance((Point<Euclidean1D>) p); + } + + /** {@inheritDoc} */ + public double distance(Point<Euclidean1D> p) { + Vector1D p3 = (Vector1D) p; + final double dx = p3.x - x; + return FastMath.abs(dx); + } + + /** {@inheritDoc} */ + public double distanceInf(Vector<Euclidean1D> p) { + Vector1D p3 = (Vector1D) p; + final double dx = FastMath.abs(p3.x - x); + return dx; + } + + /** {@inheritDoc} */ + public double distanceSq(Vector<Euclidean1D> p) { + Vector1D p3 = (Vector1D) p; + final double dx = p3.x - x; + return dx * dx; + } + + /** {@inheritDoc} */ + public double dotProduct(final Vector<Euclidean1D> v) { + final Vector1D v1 = (Vector1D) v; + return x * v1.x; + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the distance between p1 and p2 according to the L<sub>2</sub> norm + */ + public static double distance(Vector1D p1, Vector1D p2) { + return p1.distance(p2); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the distance between p1 and p2 according to the L<sub>∞</sub> norm + */ + public static double distanceInf(Vector1D p1, Vector1D p2) { + return p1.distanceInf(p2); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the square of the distance between p1 and p2 + */ + public static double distanceSq(Vector1D p1, Vector1D p2) { + return p1.distanceSq(p2); + } + + /** + * Test for the equality of two 1D vectors. + * <p> + * If all coordinates of two 1D vectors are exactly the same, and none are + * <code>Double.NaN</code>, the two 1D 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 + * 1D vector are equal to <code>Double.NaN</code>, the 1D vector is equal to + * {@link #NaN}. + * </p> + * + * @param other Object to test for equality to this + * @return true if two 1D vector objects are equal, false if + * object is null, not an instance of Vector1D, or + * not equal to this Vector1D instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof Vector1D) { + final Vector1D rhs = (Vector1D)other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return x == rhs.x; + } + return false; + } + + /** + * Get a hashCode for the 1D 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 7785; + } + return 997 * MathUtils.hash(x); + } + + /** Get a string representation of this vector. + * @return a string representation of this vector + */ + @Override + public String toString() { + return Vector1DFormat.getInstance().format(this); + } + + /** {@inheritDoc} */ + public String toString(final NumberFormat format) { + return new Vector1DFormat(format).format(this); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1DFormat.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1DFormat.java new file mode 100644 index 0000000..27f1905 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/Vector1DFormat.java @@ -0,0 +1,135 @@ +/* + * 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.oned; + +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 1D vector in components list format "{x}". + * <p>The prefix and suffix "{" and "}" 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}" and + * " { 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> + * + * @since 3.0 + */ +public class Vector1DFormat extends VectorFormat<Euclidean1D> { + + /** + * 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 Vector1DFormat() { + 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 Vector1DFormat(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 "}" + */ + public Vector1DFormat(final String prefix, final String suffix) { + super(prefix, suffix, DEFAULT_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 format the custom format for components. + */ + public Vector1DFormat(final String prefix, final String suffix, + final NumberFormat format) { + super(prefix, suffix, DEFAULT_SEPARATOR, format); + } + + /** + * Returns the default 1D vector format for the current locale. + * @return the default 1D vector format. + */ + public static Vector1DFormat getInstance() { + return getInstance(Locale.getDefault()); + } + + /** + * Returns the default 1D vector format for the given locale. + * @param locale the specific locale used by the format. + * @return the 1D vector format specific to the given locale. + */ + public static Vector1DFormat getInstance(final Locale locale) { + return new Vector1DFormat(CompositeFormat.getDefaultNumberFormat(locale)); + } + + /** {@inheritDoc} */ + @Override + public StringBuffer format(final Vector<Euclidean1D> vector, final StringBuffer toAppendTo, + final FieldPosition pos) { + final Vector1D p1 = (Vector1D) vector; + return format(toAppendTo, pos, p1.getX()); + } + + /** {@inheritDoc} */ + @Override + public Vector1D parse(final String source) throws MathParseException { + ParsePosition parsePosition = new ParsePosition(0); + Vector1D result = parse(source, parsePosition); + if (parsePosition.getIndex() == 0) { + throw new MathParseException(source, + parsePosition.getErrorIndex(), + Vector1D.class); + } + return result; + } + + /** {@inheritDoc} */ + @Override + public Vector1D parse(final String source, final ParsePosition pos) { + final double[] coordinates = parseCoordinates(1, source, pos); + if (coordinates == null) { + return null; + } + return new Vector1D(coordinates[0]); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/package-info.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/package-info.java new file mode 100644 index 0000000..0fa3788 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/oned/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 1D geometry components. + * </p> + * + */ +package org.apache.commons.math3.geometry.euclidean.oned; 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; diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/DiskGenerator.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/DiskGenerator.java new file mode 100644 index 0000000..332b1b7 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/DiskGenerator.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.twod; + +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.util.FastMath; + +/** Class generating an enclosing ball from its support points. + * @since 3.3 + */ +public class DiskGenerator implements SupportBallGenerator<Euclidean2D, Vector2D> { + + /** {@inheritDoc} */ + public EnclosingBall<Euclidean2D, Vector2D> ballOnSupport(final List<Vector2D> support) { + + if (support.size() < 1) { + return new EnclosingBall<Euclidean2D, Vector2D>(Vector2D.ZERO, Double.NEGATIVE_INFINITY); + } else { + final Vector2D vA = support.get(0); + if (support.size() < 2) { + return new EnclosingBall<Euclidean2D, Vector2D>(vA, 0, vA); + } else { + final Vector2D vB = support.get(1); + if (support.size() < 3) { + return new EnclosingBall<Euclidean2D, Vector2D>(new Vector2D(0.5, vA, 0.5, vB), + 0.5 * vA.distance(vB), + vA, vB); + } else { + final Vector2D vC = support.get(2); + // a disk is 2D can be defined as: + // (1) (x - x_0)^2 + (y - y_0)^2 = r^2 + // which can be written: + // (2) (x^2 + y^2) - 2 x_0 x - 2 y_0 y + (x_0^2 + y_0^2 - r^2) = 0 + // or simply: + // (3) (x^2 + y^2) + a x + b y + c = 0 + // with disk center coordinates -a/2, -b/2 + // If the disk exists, a, b and c are a non-zero solution to + // [ (x^2 + y^2 ) x y 1 ] [ 1 ] [ 0 ] + // [ (xA^2 + yA^2) xA yA 1 ] [ a ] [ 0 ] + // [ (xB^2 + yB^2) xB yB 1 ] * [ b ] = [ 0 ] + // [ (xC^2 + yC^2) xC yC 1 ] [ c ] [ 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) - m_12 x + m_13 y - m_14 = 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) + // Note that the minors m_11, m_12 and m_13 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()) + }; + final BigFraction[] c3 = new BigFraction[] { + new BigFraction(vA.getY()), new BigFraction(vB.getY()), new BigFraction(vC.getY()) + }; + final BigFraction[] c1 = new BigFraction[] { + c2[0].multiply(c2[0]).add(c3[0].multiply(c3[0])), + c2[1].multiply(c2[1]).add(c3[1].multiply(c3[1])), + c2[2].multiply(c2[2]).add(c3[2].multiply(c3[2])) + }; + final BigFraction twoM11 = minor(c2, c3).multiply(2); + final BigFraction m12 = minor(c1, c3); + final BigFraction m13 = minor(c1, c2); + final BigFraction centerX = m12.divide(twoM11); + final BigFraction centerY = m13.divide(twoM11).negate(); + final BigFraction dx = c2[0].subtract(centerX); + final BigFraction dy = c3[0].subtract(centerY); + final BigFraction r2 = dx.multiply(dx).add(dy.multiply(dy)); + return new EnclosingBall<Euclidean2D, Vector2D>(new Vector2D(centerX.doubleValue(), + centerY.doubleValue()), + FastMath.sqrt(r2.doubleValue()), + vA, vB, vC); + } + } + } + } + + /** Compute a dimension 3 minor, when 3<sup>d</sup> column is known to be filled with 1.0. + * @param c1 first column + * @param c2 second column + * @return value of the minor computed has an exact fraction + */ + private BigFraction minor(final BigFraction[] c1, final BigFraction[] c2) { + return c2[0].multiply(c1[2].subtract(c1[1])). + add(c2[1].multiply(c1[0].subtract(c1[2]))). + add(c2[2].multiply(c1[1].subtract(c1[0]))); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Euclidean2D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Euclidean2D.java new file mode 100644 index 0000000..af7630d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Euclidean2D.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.twod; + +import java.io.Serializable; + +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D; + +/** + * This class implements a two-dimensional space. + * @since 3.0 + */ +public class Euclidean2D implements Serializable, Space { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 4793432849757649566L; + + /** Private constructor for the singleton. + */ + private Euclidean2D() { + } + + /** Get the unique instance. + * @return the unique instance + */ + public static Euclidean2D getInstance() { + return LazyHolder.INSTANCE; + } + + /** {@inheritDoc} */ + public int getDimension() { + return 2; + } + + /** {@inheritDoc} */ + public Euclidean1D getSubSpace() { + return Euclidean1D.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 Euclidean2D INSTANCE = new Euclidean2D(); + } + // 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/twod/Line.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Line.java new file mode 100644 index 0000000..c300fa1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Line.java @@ -0,0 +1,587 @@ +/* + * 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.twod; + +import java.awt.geom.AffineTransform; + +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.OrientedPoint; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +import org.apache.commons.math3.geometry.partitioning.Embedding; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.geometry.partitioning.Transform; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.MathUtils; + +/** This class represents an oriented line in the 2D plane. + + * <p>An oriented line can be defined either by prolongating a line + * segment between two points past these points, or by one point and + * an angular direction (in trigonometric orientation).</p> + + * <p>Since it is oriented the two half planes at its two sides are + * unambiguously identified as a left half plane and a right half + * plane. This can be used to identify the interior and the exterior + * in a simple way by local properties only when part of a line is + * used to define part of a polygon boundary.</p> + + * <p>A line can also be used to completely define a reference frame + * in the plane. It is sufficient to select one specific point in the + * line (the orthogonal projection of the original reference frame on + * the line) and to use the unit vector in the line direction and the + * orthogonal vector oriented from left half plane to right half + * plane. We define two coordinates by the process, the + * <em>abscissa</em> along the line, and the <em>offset</em> across + * the line. All points of the plane are uniquely identified by these + * two coordinates. The line is the set of points at zero offset, the + * left half plane is the set of points with negative offsets and the + * right half plane is the set of points with positive offsets.</p> + + * @since 3.0 + */ +public class Line implements Hyperplane<Euclidean2D>, Embedding<Euclidean2D, Euclidean1D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Angle with respect to the abscissa axis. */ + private double angle; + + /** Cosine of the line angle. */ + private double cos; + + /** Sine of the line angle. */ + private double sin; + + /** Offset of the frame origin. */ + private double originOffset; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** Reverse line. */ + private Line reverse; + + /** Build a line from two points. + * <p>The line is oriented from p1 to p2</p> + * @param p1 first point + * @param p2 second point + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public Line(final Vector2D p1, final Vector2D p2, final double tolerance) { + reset(p1, p2); + this.tolerance = tolerance; + } + + /** Build a line from a point and an angle. + * @param p point belonging to the line + * @param angle angle of the line with respect to abscissa axis + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public Line(final Vector2D p, final double angle, final double tolerance) { + reset(p, angle); + this.tolerance = tolerance; + } + + /** Build a line from its internal characteristics. + * @param angle angle of the line with respect to abscissa axis + * @param cos cosine of the angle + * @param sin sine of the angle + * @param originOffset offset of the origin + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + private Line(final double angle, final double cos, final double sin, + final double originOffset, final double tolerance) { + this.angle = angle; + this.cos = cos; + this.sin = sin; + this.originOffset = originOffset; + this.tolerance = tolerance; + this.reverse = null; + } + + /** Build a line from two points. + * <p>The line is oriented from p1 to p2</p> + * @param p1 first point + * @param p2 second point + * @deprecated as of 3.3, replaced with {@link #Line(Vector2D, Vector2D, double)} + */ + @Deprecated + public Line(final Vector2D p1, final Vector2D p2) { + this(p1, p2, DEFAULT_TOLERANCE); + } + + /** Build a line from a point and an angle. + * @param p point belonging to the line + * @param angle angle of the line with respect to abscissa axis + * @deprecated as of 3.3, replaced with {@link #Line(Vector2D, double, double)} + */ + @Deprecated + public Line(final Vector2D p, final double angle) { + this(p, angle, DEFAULT_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) { + angle = MathUtils.normalizeAngle(line.angle, FastMath.PI); + cos = line.cos; + sin = line.sin; + originOffset = line.originOffset; + tolerance = line.tolerance; + reverse = null; + } + + /** {@inheritDoc} */ + public Line copySelf() { + return new Line(this); + } + + /** Reset the instance as if built from two points. + * <p>The line is oriented from p1 to p2</p> + * @param p1 first point + * @param p2 second point + */ + public void reset(final Vector2D p1, final Vector2D p2) { + unlinkReverse(); + final double dx = p2.getX() - p1.getX(); + final double dy = p2.getY() - p1.getY(); + final double d = FastMath.hypot(dx, dy); + if (d == 0.0) { + angle = 0.0; + cos = 1.0; + sin = 0.0; + originOffset = p1.getY(); + } else { + angle = FastMath.PI + FastMath.atan2(-dy, -dx); + cos = dx / d; + sin = dy / d; + originOffset = MathArrays.linearCombination(p2.getX(), p1.getY(), -p1.getX(), p2.getY()) / d; + } + } + + /** Reset the instance as if built from a line and an angle. + * @param p point belonging to the line + * @param alpha angle of the line with respect to abscissa axis + */ + public void reset(final Vector2D p, final double alpha) { + unlinkReverse(); + this.angle = MathUtils.normalizeAngle(alpha, FastMath.PI); + cos = FastMath.cos(this.angle); + sin = FastMath.sin(this.angle); + originOffset = MathArrays.linearCombination(cos, p.getY(), -sin, p.getX()); + } + + /** Revert the instance. + */ + public void revertSelf() { + unlinkReverse(); + if (angle < FastMath.PI) { + angle += FastMath.PI; + } else { + angle -= FastMath.PI; + } + cos = -cos; + sin = -sin; + originOffset = -originOffset; + } + + /** Unset the link between an instance and its reverse. + */ + private void unlinkReverse() { + if (reverse != null) { + reverse.reverse = null; + } + reverse = null; + } + + /** Get the reverse of the instance. + * <p>Get a line with reversed orientation with respect to the + * instance.</p> + * <p> + * As long as neither the instance nor its reverse are modified + * (i.e. as long as none of the {@link #reset(Vector2D, Vector2D)}, + * {@link #reset(Vector2D, double)}, {@link #revertSelf()}, + * {@link #setAngle(double)} or {@link #setOriginOffset(double)} + * methods are called), then the line and its reverse remain linked + * together so that {@code line.getReverse().getReverse() == line}. + * When one of the line is modified, the link is deleted as both + * instance becomes independent. + * </p> + * @return a new line, with orientation opposite to the instance orientation + */ + public Line getReverse() { + if (reverse == null) { + reverse = new Line((angle < FastMath.PI) ? (angle + FastMath.PI) : (angle - FastMath.PI), + -cos, -sin, -originOffset, tolerance); + reverse.reverse = this; + } + return reverse; + } + + /** 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<Euclidean2D> vector) { + return toSubSpace((Point<Euclidean2D>) 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 Vector2D toSpace(Vector<Euclidean1D> vector) { + return toSpace((Point<Euclidean1D>) vector); + } + + /** {@inheritDoc} */ + public Vector1D toSubSpace(final Point<Euclidean2D> point) { + Vector2D p2 = (Vector2D) point; + return new Vector1D(MathArrays.linearCombination(cos, p2.getX(), sin, p2.getY())); + } + + /** {@inheritDoc} */ + public Vector2D toSpace(final Point<Euclidean1D> point) { + final double abscissa = ((Vector1D) point).getX(); + return new Vector2D(MathArrays.linearCombination(abscissa, cos, -originOffset, sin), + MathArrays.linearCombination(abscissa, sin, originOffset, cos)); + } + + /** Get the intersection point of the instance and another line. + * @param other other line + * @return intersection point of the instance and the other line + * or null if there are no intersection points + */ + public Vector2D intersection(final Line other) { + final double d = MathArrays.linearCombination(sin, other.cos, -other.sin, cos); + if (FastMath.abs(d) < tolerance) { + return null; + } + return new Vector2D(MathArrays.linearCombination(cos, other.originOffset, -other.cos, originOffset) / d, + MathArrays.linearCombination(sin, other.originOffset, -other.sin, originOffset) / d); + } + + /** {@inheritDoc} + * @since 3.3 + */ + public Point<Euclidean2D> project(Point<Euclidean2D> point) { + return toSpace(toSubSpace(point)); + } + + /** {@inheritDoc} + * @since 3.3 + */ + public double getTolerance() { + return tolerance; + } + + /** {@inheritDoc} */ + public SubLine wholeHyperplane() { + return new SubLine(this, new IntervalsSet(tolerance)); + } + + /** Build a region covering the whole space. + * @return a region containing the instance (really a {@link + * PolygonsSet PolygonsSet} instance) + */ + public PolygonsSet wholeSpace() { + return new PolygonsSet(tolerance); + } + + /** Get the offset (oriented distance) of a parallel line. + * <p>This method should be called only for parallel lines otherwise + * the result is not meaningful.</p> + * <p>The offset is 0 if both lines are the same, it is + * positive if the line is on the right side of the instance and + * negative if it is on the left side, according to its natural + * orientation.</p> + * @param line line to check + * @return offset of the line + */ + public double getOffset(final Line line) { + return originOffset + + (MathArrays.linearCombination(cos, line.cos, sin, line.sin) > 0 ? -line.originOffset : line.originOffset); + } + + /** Get the offset (oriented distance) of a vector. + * @param vector vector to check + * @return offset of the vector + */ + public double getOffset(Vector<Euclidean2D> vector) { + return getOffset((Point<Euclidean2D>) vector); + } + + /** {@inheritDoc} */ + public double getOffset(final Point<Euclidean2D> point) { + Vector2D p2 = (Vector2D) point; + return MathArrays.linearCombination(sin, p2.getX(), -cos, p2.getY(), 1.0, originOffset); + } + + /** {@inheritDoc} */ + public boolean sameOrientationAs(final Hyperplane<Euclidean2D> other) { + final Line otherL = (Line) other; + return MathArrays.linearCombination(sin, otherL.sin, cos, otherL.cos) >= 0.0; + } + + /** Get one point from the plane. + * @param abscissa desired abscissa for the point + * @param offset desired offset for the point + * @return one point in the plane, with given abscissa and offset + * relative to the line + */ + public Vector2D getPointAt(final Vector1D abscissa, final double offset) { + final double x = abscissa.getX(); + final double dOffset = offset - originOffset; + return new Vector2D(MathArrays.linearCombination(x, cos, dOffset, sin), + MathArrays.linearCombination(x, sin, -dOffset, cos)); + } + + /** Check if the line contains a point. + * @param p point to check + * @return true if p belongs to the line + */ + public boolean contains(final Vector2D p) { + return FastMath.abs(getOffset(p)) < tolerance; + } + + /** Compute the distance between the instance and a point. + * <p>This is a shortcut for invoking FastMath.abs(getOffset(p)), + * and provides consistency with what is in the + * org.apache.commons.math3.geometry.euclidean.threed.Line class.</p> + * + * @param p to check + * @return distance between the instance and the point + * @since 3.1 + */ + public double distance(final Vector2D p) { + return FastMath.abs(getOffset(p)); + } + + /** Check the instance is parallel to another line. + * @param line other line to check + * @return true if the instance is parallel to the other line + * (they can have either the same or opposite orientations) + */ + public boolean isParallelTo(final Line line) { + return FastMath.abs(MathArrays.linearCombination(sin, line.cos, -cos, line.sin)) < tolerance; + } + + /** Translate the line to force it passing by a point. + * @param p point by which the line should pass + */ + public void translateToPoint(final Vector2D p) { + originOffset = MathArrays.linearCombination(cos, p.getY(), -sin, p.getX()); + } + + /** Get the angle of the line. + * @return the angle of the line with respect to the abscissa axis + */ + public double getAngle() { + return MathUtils.normalizeAngle(angle, FastMath.PI); + } + + /** Set the angle of the line. + * @param angle new angle of the line with respect to the abscissa axis + */ + public void setAngle(final double angle) { + unlinkReverse(); + this.angle = MathUtils.normalizeAngle(angle, FastMath.PI); + cos = FastMath.cos(this.angle); + sin = FastMath.sin(this.angle); + } + + /** Get the offset of the origin. + * @return the offset of the origin + */ + public double getOriginOffset() { + return originOffset; + } + + /** Set the offset of the origin. + * @param offset offset of the origin + */ + public void setOriginOffset(final double offset) { + unlinkReverse(); + originOffset = offset; + } + + /** Get a {@link org.apache.commons.math3.geometry.partitioning.Transform + * Transform} embedding an affine transform. + * @param transform affine transform to embed (must be inversible + * otherwise the {@link + * org.apache.commons.math3.geometry.partitioning.Transform#apply(Hyperplane) + * apply(Hyperplane)} method would work only for some lines, and + * fail for other ones) + * @return a new transform that can be applied to either {@link + * Vector2D Vector2D}, {@link Line Line} or {@link + * org.apache.commons.math3.geometry.partitioning.SubHyperplane + * SubHyperplane} instances + * @exception MathIllegalArgumentException if the transform is non invertible + * @deprecated as of 3.6, replaced with {@link #getTransform(double, double, double, double, double, double)} + */ + @Deprecated + public static Transform<Euclidean2D, Euclidean1D> getTransform(final AffineTransform transform) + throws MathIllegalArgumentException { + final double[] m = new double[6]; + transform.getMatrix(m); + return new LineTransform(m[0], m[1], m[2], m[3], m[4], m[5]); + } + + /** Get a {@link org.apache.commons.math3.geometry.partitioning.Transform + * Transform} embedding an affine transform. + * @param cXX transform factor between input abscissa and output abscissa + * @param cYX transform factor between input abscissa and output ordinate + * @param cXY transform factor between input ordinate and output abscissa + * @param cYY transform factor between input ordinate and output ordinate + * @param cX1 transform addendum for output abscissa + * @param cY1 transform addendum for output ordinate + * @return a new transform that can be applied to either {@link + * Vector2D Vector2D}, {@link Line Line} or {@link + * org.apache.commons.math3.geometry.partitioning.SubHyperplane + * SubHyperplane} instances + * @exception MathIllegalArgumentException if the transform is non invertible + * @since 3.6 + */ + public static Transform<Euclidean2D, Euclidean1D> getTransform(final double cXX, + final double cYX, + final double cXY, + final double cYY, + final double cX1, + final double cY1) + throws MathIllegalArgumentException { + return new LineTransform(cXX, cYX, cXY, cYY, cX1, cY1); + } + + /** Class embedding an affine transform. + * <p>This class is used in order to apply an affine transform to a + * line. Using a specific object allow to perform some computations + * on the transform only once even if the same transform is to be + * applied to a large number of lines (for example to a large + * polygon)./<p> + */ + private static class LineTransform implements Transform<Euclidean2D, Euclidean1D> { + + /** Transform factor between input abscissa and output abscissa. */ + private double cXX; + + /** Transform factor between input abscissa and output ordinate. */ + private double cYX; + + /** Transform factor between input ordinate and output abscissa. */ + private double cXY; + + /** Transform factor between input ordinate and output ordinate. */ + private double cYY; + + /** Transform addendum for output abscissa. */ + private double cX1; + + /** Transform addendum for output ordinate. */ + private double cY1; + + /** cXY * cY1 - cYY * cX1. */ + private double c1Y; + + /** cXX * cY1 - cYX * cX1. */ + private double c1X; + + /** cXX * cYY - cYX * cXY. */ + private double c11; + + /** Build an affine line transform from a n {@code AffineTransform}. + * @param cXX transform factor between input abscissa and output abscissa + * @param cYX transform factor between input abscissa and output ordinate + * @param cXY transform factor between input ordinate and output abscissa + * @param cYY transform factor between input ordinate and output ordinate + * @param cX1 transform addendum for output abscissa + * @param cY1 transform addendum for output ordinate + * @exception MathIllegalArgumentException if the transform is non invertible + * @since 3.6 + */ + LineTransform(final double cXX, final double cYX, final double cXY, + final double cYY, final double cX1, final double cY1) + throws MathIllegalArgumentException { + + this.cXX = cXX; + this.cYX = cYX; + this.cXY = cXY; + this.cYY = cYY; + this.cX1 = cX1; + this.cY1 = cY1; + + c1Y = MathArrays.linearCombination(cXY, cY1, -cYY, cX1); + c1X = MathArrays.linearCombination(cXX, cY1, -cYX, cX1); + c11 = MathArrays.linearCombination(cXX, cYY, -cYX, cXY); + + if (FastMath.abs(c11) < 1.0e-20) { + throw new MathIllegalArgumentException(LocalizedFormats.NON_INVERTIBLE_TRANSFORM); + } + + } + + /** {@inheritDoc} */ + public Vector2D apply(final Point<Euclidean2D> point) { + final Vector2D p2D = (Vector2D) point; + final double x = p2D.getX(); + final double y = p2D.getY(); + return new Vector2D(MathArrays.linearCombination(cXX, x, cXY, y, cX1, 1), + MathArrays.linearCombination(cYX, x, cYY, y, cY1, 1)); + } + + /** {@inheritDoc} */ + public Line apply(final Hyperplane<Euclidean2D> hyperplane) { + final Line line = (Line) hyperplane; + final double rOffset = MathArrays.linearCombination(c1X, line.cos, c1Y, line.sin, c11, line.originOffset); + final double rCos = MathArrays.linearCombination(cXX, line.cos, cXY, line.sin); + final double rSin = MathArrays.linearCombination(cYX, line.cos, cYY, line.sin); + final double inv = 1.0 / FastMath.sqrt(rSin * rSin + rCos * rCos); + return new Line(FastMath.PI + FastMath.atan2(-rSin, -rCos), + inv * rCos, inv * rSin, + inv * rOffset, line.tolerance); + } + + /** {@inheritDoc} */ + public SubHyperplane<Euclidean1D> apply(final SubHyperplane<Euclidean1D> sub, + final Hyperplane<Euclidean2D> original, + final Hyperplane<Euclidean2D> transformed) { + final OrientedPoint op = (OrientedPoint) sub.getHyperplane(); + final Line originalLine = (Line) original; + final Line transformedLine = (Line) transformed; + final Vector1D newLoc = + transformedLine.toSubSpace(apply(originalLine.toSpace(op.getLocation()))); + return new OrientedPoint(newLoc, op.isDirect(), originalLine.tolerance).wholeHyperplane(); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/NestedLoops.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/NestedLoops.java new file mode 100644 index 0000000..83928fa --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/NestedLoops.java @@ -0,0 +1,201 @@ +/* + * 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.twod; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; + +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.euclidean.oned.IntervalsSet; +import org.apache.commons.math3.geometry.partitioning.Region; +import org.apache.commons.math3.geometry.partitioning.RegionFactory; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; + +/** This class represent a tree of nested 2D boundary loops. + + * <p>This class is used for piecewise polygons construction. + * Polygons are built using the outline edges as + * representative of boundaries, the orientation of these lines are + * meaningful. However, we want to allow the user to specify its + * outline loops without having to take care of this orientation. This + * class is devoted to correct mis-oriented loops.<p> + + * <p>Orientation is computed assuming the piecewise polygon is finite, + * i.e. the outermost loops have their exterior side facing points at + * infinity, and hence are oriented counter-clockwise. The orientation of + * internal loops is computed as the reverse of the orientation of + * their immediate surrounding loop.</p> + + * @since 3.0 + */ +class NestedLoops { + + /** Boundary loop. */ + private Vector2D[] loop; + + /** Surrounded loops. */ + private List<NestedLoops> surrounded; + + /** Polygon enclosing a finite region. */ + private Region<Euclidean2D> polygon; + + /** Indicator for original loop orientation. */ + private boolean originalIsClockwise; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** Simple Constructor. + * <p>Build an empty tree of nested loops. This instance will become + * the root node of a complete tree, it is not associated with any + * loop by itself, the outermost loops are in the root tree child + * nodes.</p> + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + NestedLoops(final double tolerance) { + this.surrounded = new ArrayList<NestedLoops>(); + this.tolerance = tolerance; + } + + /** Constructor. + * <p>Build a tree node with neither parent nor children</p> + * @param loop boundary loop (will be reversed in place if needed) + * @param tolerance tolerance below which points are considered identical + * @exception MathIllegalArgumentException if an outline has an open boundary loop + * @since 3.3 + */ + private NestedLoops(final Vector2D[] loop, final double tolerance) + throws MathIllegalArgumentException { + + if (loop[0] == null) { + throw new MathIllegalArgumentException(LocalizedFormats.OUTLINE_BOUNDARY_LOOP_OPEN); + } + + this.loop = loop; + this.surrounded = new ArrayList<NestedLoops>(); + this.tolerance = tolerance; + + // build the polygon defined by the loop + final ArrayList<SubHyperplane<Euclidean2D>> edges = new ArrayList<SubHyperplane<Euclidean2D>>(); + Vector2D current = loop[loop.length - 1]; + for (int i = 0; i < loop.length; ++i) { + final Vector2D previous = current; + current = loop[i]; + final Line line = new Line(previous, current, tolerance); + final IntervalsSet region = + new IntervalsSet(line.toSubSpace((Point<Euclidean2D>) previous).getX(), + line.toSubSpace((Point<Euclidean2D>) current).getX(), + tolerance); + edges.add(new SubLine(line, region)); + } + polygon = new PolygonsSet(edges, tolerance); + + // ensure the polygon encloses a finite region of the plane + if (Double.isInfinite(polygon.getSize())) { + polygon = new RegionFactory<Euclidean2D>().getComplement(polygon); + originalIsClockwise = false; + } else { + originalIsClockwise = true; + } + + } + + /** Add a loop in a tree. + * @param bLoop boundary loop (will be reversed in place if needed) + * @exception MathIllegalArgumentException if an outline has crossing + * boundary loops or open boundary loops + */ + public void add(final Vector2D[] bLoop) throws MathIllegalArgumentException { + add(new NestedLoops(bLoop, tolerance)); + } + + /** Add a loop in a tree. + * @param node boundary loop (will be reversed in place if needed) + * @exception MathIllegalArgumentException if an outline has boundary + * loops that cross each other + */ + private void add(final NestedLoops node) throws MathIllegalArgumentException { + + // check if we can go deeper in the tree + for (final NestedLoops child : surrounded) { + if (child.polygon.contains(node.polygon)) { + child.add(node); + return; + } + } + + // check if we can absorb some of the instance children + for (final Iterator<NestedLoops> iterator = surrounded.iterator(); iterator.hasNext();) { + final NestedLoops child = iterator.next(); + if (node.polygon.contains(child.polygon)) { + node.surrounded.add(child); + iterator.remove(); + } + } + + // we should be separate from the remaining children + RegionFactory<Euclidean2D> factory = new RegionFactory<Euclidean2D>(); + for (final NestedLoops child : surrounded) { + if (!factory.intersection(node.polygon, child.polygon).isEmpty()) { + throw new MathIllegalArgumentException(LocalizedFormats.CROSSING_BOUNDARY_LOOPS); + } + } + + surrounded.add(node); + + } + + /** Correct the orientation of the loops contained in the tree. + * <p>This is this method that really inverts the loops that where + * provided through the {@link #add(Vector2D[]) add} method if + * they are mis-oriented</p> + */ + public void correctOrientation() { + for (NestedLoops child : surrounded) { + child.setClockWise(true); + } + } + + /** Set the loop orientation. + * @param clockwise if true, the loop should be set to clockwise + * orientation + */ + private void setClockWise(final boolean clockwise) { + + if (originalIsClockwise ^ clockwise) { + // we need to inverse the original loop + int min = -1; + int max = loop.length; + while (++min < --max) { + final Vector2D tmp = loop[min]; + loop[min] = loop[max]; + loop[max] = tmp; + } + } + + // go deeper in the tree + for (final NestedLoops child : surrounded) { + child.setClockWise(!clockwise); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/PolygonsSet.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/PolygonsSet.java new file mode 100644 index 0000000..61fae9f --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/PolygonsSet.java @@ -0,0 +1,1160 @@ +/* + * 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.twod; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; + +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.AbstractRegion; +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.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.Side; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.Precision; + +/** This class represents a 2D region: a set of polygons. + * @since 3.0 + */ +public class PolygonsSet extends AbstractRegion<Euclidean2D, Euclidean1D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Vertices organized as boundary loops. */ + private Vector2D[][] vertices; + + /** Build a polygons set representing the whole plane. + * @param tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolygonsSet(final double tolerance) { + super(tolerance); + } + + /** Build a polygons 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 PolygonsSet(final BSPTree<Euclidean2D> tree, final double tolerance) { + super(tree, tolerance); + } + + /** Build a polygons 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 polygons with holes + * or a set of disjoint polygons 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 + * org.apache.commons.math3.geometry.partitioning.Region#checkPoint(org.apache.commons.math3.geometry.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 PolygonsSet(final Collection<SubHyperplane<Euclidean2D>> boundary, final double tolerance) { + super(boundary, 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 tolerance tolerance below which points are considered identical + * @since 3.3 + */ + public PolygonsSet(final double xMin, final double xMax, + final double yMin, final double yMax, + final double tolerance) { + super(boxBoundary(xMin, xMax, yMin, yMax, tolerance), tolerance); + } + + /** Build a polygon from a simple list of vertices. + * <p>The boundary is provided as a list of points considering to + * represent the vertices of a simple loop. The interior part of the + * region is on the left side of this path and the exterior is on its + * right side.</p> + * <p>This constructor does not handle polygons with a boundary + * forming several disconnected paths (such as polygons with holes).</p> + * <p>For cases where this simple constructor applies, it is expected to + * be numerically more robust than the {@link #PolygonsSet(Collection) general + * constructor} using {@link SubHyperplane subhyperplanes}.</p> + * <p>If the list is empty, the region will represent the whole + * space.</p> + * <p> + * Polygons with thin pikes or dents are inherently difficult to handle because + * they involve lines with almost opposite directions at some vertices. Polygons + * whose vertices come from some physical measurement with noise are also + * difficult because an edge that should be straight may be broken in lots of + * different pieces with almost equal directions. In both cases, computing the + * lines intersections is not numerically robust due to the almost 0 or almost + * π angle. Such cases need to carefully adjust the {@code hyperplaneThickness} + * parameter. A too small value would often lead to completely wrong polygons + * with large area wrongly identified as inside or outside. Large values are + * often much safer. As a rule of thumb, a value slightly below the size of the + * most accurate detail needed is a good value for the {@code hyperplaneThickness} + * parameter. + * </p> + * @param hyperplaneThickness tolerance below which points are considered to + * belong to the hyperplane (which is therefore more a slab) + * @param vertices vertices of the simple loop boundary + */ + public PolygonsSet(final double hyperplaneThickness, final Vector2D ... vertices) { + super(verticesToTree(hyperplaneThickness, vertices), hyperplaneThickness); + } + + /** Build a polygons set representing the whole real line. + * @deprecated as of 3.3, replaced with {@link #PolygonsSet(double)} + */ + @Deprecated + public PolygonsSet() { + this(DEFAULT_TOLERANCE); + } + + /** Build a polygons 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 #PolygonsSet(BSPTree, double)} + */ + @Deprecated + public PolygonsSet(final BSPTree<Euclidean2D> tree) { + this(tree, DEFAULT_TOLERANCE); + } + + /** Build a polygons 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 polygons with holes + * or a set of disjoint polygons 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 + * org.apache.commons.math3.geometry.partitioning.Region#checkPoint(org.apache.commons.math3.geometry.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 #PolygonsSet(Collection, double)} + */ + @Deprecated + public PolygonsSet(final Collection<SubHyperplane<Euclidean2D>> 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 + * @deprecated as of 3.3, replaced with {@link #PolygonsSet(double, double, double, double, double)} + */ + @Deprecated + public PolygonsSet(final double xMin, final double xMax, + final double yMin, final double yMax) { + this(xMin, xMax, yMin, yMax, DEFAULT_TOLERANCE); + } + + /** Create a list of hyperplanes representing the boundary of a 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 tolerance tolerance below which points are considered identical + * @return boundary of the box + */ + private static Line[] boxBoundary(final double xMin, final double xMax, + final double yMin, final double yMax, + final double tolerance) { + if ((xMin >= xMax - tolerance) || (yMin >= yMax - tolerance)) { + // too thin box, build an empty polygons set + return null; + } + final Vector2D minMin = new Vector2D(xMin, yMin); + final Vector2D minMax = new Vector2D(xMin, yMax); + final Vector2D maxMin = new Vector2D(xMax, yMin); + final Vector2D maxMax = new Vector2D(xMax, yMax); + return new Line[] { + new Line(minMin, maxMin, tolerance), + new Line(maxMin, maxMax, tolerance), + new Line(maxMax, minMax, tolerance), + new Line(minMax, minMin, tolerance) + }; + } + + /** Build the BSP tree of a polygons set from a simple list of vertices. + * <p>The boundary is provided as a list of points considering to + * represent the vertices of a simple loop. The interior part of the + * region is on the left side of this path and the exterior is on its + * right side.</p> + * <p>This constructor does not handle polygons with a boundary + * forming several disconnected paths (such as polygons with holes).</p> + * <p>For cases where this simple constructor applies, it is expected to + * be numerically more robust than the {@link #PolygonsSet(Collection) general + * constructor} using {@link SubHyperplane subhyperplanes}.</p> + * @param hyperplaneThickness tolerance below which points are consider to + * belong to the hyperplane (which is therefore more a slab) + * @param vertices vertices of the simple loop boundary + * @return the BSP tree of the input vertices + */ + private static BSPTree<Euclidean2D> verticesToTree(final double hyperplaneThickness, + final Vector2D ... vertices) { + + final int n = vertices.length; + if (n == 0) { + // the tree represents the whole space + return new BSPTree<Euclidean2D>(Boolean.TRUE); + } + + // build the vertices + final Vertex[] vArray = new Vertex[n]; + for (int i = 0; i < n; ++i) { + vArray[i] = new Vertex(vertices[i]); + } + + // build the edges + List<Edge> edges = new ArrayList<Edge>(n); + for (int i = 0; i < n; ++i) { + + // get the endpoints of the edge + final Vertex start = vArray[i]; + final Vertex end = vArray[(i + 1) % n]; + + // get the line supporting the edge, taking care not to recreate it + // if it was already created earlier due to another edge being aligned + // with the current one + Line line = start.sharedLineWith(end); + if (line == null) { + line = new Line(start.getLocation(), end.getLocation(), hyperplaneThickness); + } + + // create the edge and store it + edges.add(new Edge(start, end, line)); + + // check if another vertex also happens to be on this line + for (final Vertex vertex : vArray) { + if (vertex != start && vertex != end && + FastMath.abs(line.getOffset((Point<Euclidean2D>) vertex.getLocation())) <= hyperplaneThickness) { + vertex.bindWith(line); + } + } + + } + + // build the tree top-down + final BSPTree<Euclidean2D> tree = new BSPTree<Euclidean2D>(); + insertEdges(hyperplaneThickness, tree, edges); + + return tree; + + } + + /** Recursively build a tree by inserting cut sub-hyperplanes. + * @param hyperplaneThickness tolerance below which points are consider to + * belong to the hyperplane (which is therefore more a slab) + * @param node current tree node (it is a leaf node at the beginning + * of the call) + * @param edges list of edges to insert in the cell defined by this node + * (excluding edges not belonging to the cell defined by this node) + */ + private static void insertEdges(final double hyperplaneThickness, + final BSPTree<Euclidean2D> node, + final List<Edge> edges) { + + // find an edge with an hyperplane that can be inserted in the node + int index = 0; + Edge inserted =null; + while (inserted == null && index < edges.size()) { + inserted = edges.get(index++); + if (inserted.getNode() == null) { + if (node.insertCut(inserted.getLine())) { + inserted.setNode(node); + } else { + inserted = null; + } + } else { + inserted = null; + } + } + + if (inserted == null) { + // no suitable edge was found, the node remains a leaf node + // we need to set its inside/outside boolean indicator + final BSPTree<Euclidean2D> parent = node.getParent(); + if (parent == null || node == parent.getMinus()) { + node.setAttribute(Boolean.TRUE); + } else { + node.setAttribute(Boolean.FALSE); + } + return; + } + + // we have split the node by inserting an edge as a cut sub-hyperplane + // distribute the remaining edges in the two sub-trees + final List<Edge> plusList = new ArrayList<Edge>(); + final List<Edge> minusList = new ArrayList<Edge>(); + for (final Edge edge : edges) { + if (edge != inserted) { + final double startOffset = inserted.getLine().getOffset((Point<Euclidean2D>) edge.getStart().getLocation()); + final double endOffset = inserted.getLine().getOffset((Point<Euclidean2D>) edge.getEnd().getLocation()); + Side startSide = (FastMath.abs(startOffset) <= hyperplaneThickness) ? + Side.HYPER : ((startOffset < 0) ? Side.MINUS : Side.PLUS); + Side endSide = (FastMath.abs(endOffset) <= hyperplaneThickness) ? + Side.HYPER : ((endOffset < 0) ? Side.MINUS : Side.PLUS); + switch (startSide) { + case PLUS: + if (endSide == Side.MINUS) { + // we need to insert a split point on the hyperplane + final Vertex splitPoint = edge.split(inserted.getLine()); + minusList.add(splitPoint.getOutgoing()); + plusList.add(splitPoint.getIncoming()); + } else { + plusList.add(edge); + } + break; + case MINUS: + if (endSide == Side.PLUS) { + // we need to insert a split point on the hyperplane + final Vertex splitPoint = edge.split(inserted.getLine()); + minusList.add(splitPoint.getIncoming()); + plusList.add(splitPoint.getOutgoing()); + } else { + minusList.add(edge); + } + break; + default: + if (endSide == Side.PLUS) { + plusList.add(edge); + } else if (endSide == Side.MINUS) { + minusList.add(edge); + } + break; + } + } + } + + // recurse through lower levels + if (!plusList.isEmpty()) { + insertEdges(hyperplaneThickness, node.getPlus(), plusList); + } else { + node.getPlus().setAttribute(Boolean.FALSE); + } + if (!minusList.isEmpty()) { + insertEdges(hyperplaneThickness, node.getMinus(), minusList); + } else { + node.getMinus().setAttribute(Boolean.TRUE); + } + + } + + /** Internal class for holding vertices while they are processed to build a BSP tree. */ + private static class Vertex { + + /** Vertex location. */ + private final Vector2D location; + + /** Incoming edge. */ + private Edge incoming; + + /** Outgoing edge. */ + private Edge outgoing; + + /** Lines bound with this vertex. */ + private final List<Line> lines; + + /** Build a non-processed vertex not owned by any node yet. + * @param location vertex location + */ + Vertex(final Vector2D location) { + this.location = location; + this.incoming = null; + this.outgoing = null; + this.lines = new ArrayList<Line>(); + } + + /** Get Vertex location. + * @return vertex location + */ + public Vector2D getLocation() { + return location; + } + + /** Bind a line considered to contain this vertex. + * @param line line to bind with this vertex + */ + public void bindWith(final Line line) { + lines.add(line); + } + + /** Get the common line bound with both the instance and another vertex, if any. + * <p> + * When two vertices are both bound to the same line, this means they are + * already handled by node associated with this line, so there is no need + * to create a cut hyperplane for them. + * </p> + * @param vertex other vertex to check instance against + * @return line bound with both the instance and another vertex, or null if the + * two vertices do not share a line yet + */ + public Line sharedLineWith(final Vertex vertex) { + for (final Line line1 : lines) { + for (final Line line2 : vertex.lines) { + if (line1 == line2) { + return line1; + } + } + } + return null; + } + + /** Set incoming edge. + * <p> + * The line supporting the incoming edge is automatically bound + * with the instance. + * </p> + * @param incoming incoming edge + */ + public void setIncoming(final Edge incoming) { + this.incoming = incoming; + bindWith(incoming.getLine()); + } + + /** Get incoming edge. + * @return incoming edge + */ + public Edge getIncoming() { + return incoming; + } + + /** Set outgoing edge. + * <p> + * The line supporting the outgoing edge is automatically bound + * with the instance. + * </p> + * @param outgoing outgoing edge + */ + public void setOutgoing(final Edge outgoing) { + this.outgoing = outgoing; + bindWith(outgoing.getLine()); + } + + /** Get outgoing edge. + * @return outgoing edge + */ + public Edge getOutgoing() { + return outgoing; + } + + } + + /** Internal class for holding edges while they are processed to build a BSP tree. */ + private static class Edge { + + /** Start vertex. */ + private final Vertex start; + + /** End vertex. */ + private final Vertex end; + + /** Line supporting the edge. */ + private final Line line; + + /** Node whose cut hyperplane contains this edge. */ + private BSPTree<Euclidean2D> node; + + /** Build an edge not contained in any node yet. + * @param start start vertex + * @param end end vertex + * @param line line supporting the edge + */ + Edge(final Vertex start, final Vertex end, final Line line) { + + this.start = start; + this.end = end; + this.line = line; + this.node = null; + + // connect the vertices back to the edge + start.setOutgoing(this); + end.setIncoming(this); + + } + + /** Get start vertex. + * @return start vertex + */ + public Vertex getStart() { + return start; + } + + /** Get end vertex. + * @return end vertex + */ + public Vertex getEnd() { + return end; + } + + /** Get the line supporting this edge. + * @return line supporting this edge + */ + public Line getLine() { + return line; + } + + /** Set the node whose cut hyperplane contains this edge. + * @param node node whose cut hyperplane contains this edge + */ + public void setNode(final BSPTree<Euclidean2D> node) { + this.node = node; + } + + /** Get the node whose cut hyperplane contains this edge. + * @return node whose cut hyperplane contains this edge + * (null if edge has not yet been inserted into the BSP tree) + */ + public BSPTree<Euclidean2D> getNode() { + return node; + } + + /** Split the edge. + * <p> + * Once split, this edge is not referenced anymore by the vertices, + * it is replaced by the two half-edges and an intermediate splitting + * vertex is introduced to connect these two halves. + * </p> + * @param splitLine line splitting the edge in two halves + * @return split vertex (its incoming and outgoing edges are the two halves) + */ + public Vertex split(final Line splitLine) { + final Vertex splitVertex = new Vertex(line.intersection(splitLine)); + splitVertex.bindWith(splitLine); + final Edge startHalf = new Edge(start, splitVertex, line); + final Edge endHalf = new Edge(splitVertex, end, line); + startHalf.node = node; + endHalf.node = node; + return splitVertex; + } + + } + + /** {@inheritDoc} */ + @Override + public PolygonsSet buildNew(final BSPTree<Euclidean2D> tree) { + return new PolygonsSet(tree, getTolerance()); + } + + /** {@inheritDoc} */ + @Override + protected void computeGeometricalProperties() { + + final Vector2D[][] v = getVertices(); + + if (v.length == 0) { + final BSPTree<Euclidean2D> tree = getTree(false); + if (tree.getCut() == null && (Boolean) tree.getAttribute()) { + // the instance covers the whole space + setSize(Double.POSITIVE_INFINITY); + setBarycenter((Point<Euclidean2D>) Vector2D.NaN); + } else { + setSize(0); + setBarycenter((Point<Euclidean2D>) new Vector2D(0, 0)); + } + } else if (v[0][0] == null) { + // there is at least one open-loop: the polygon is infinite + setSize(Double.POSITIVE_INFINITY); + setBarycenter((Point<Euclidean2D>) Vector2D.NaN); + } else { + // all loops are closed, we compute some integrals around the shape + + double sum = 0; + double sumX = 0; + double sumY = 0; + + for (Vector2D[] loop : v) { + double x1 = loop[loop.length - 1].getX(); + double y1 = loop[loop.length - 1].getY(); + for (final Vector2D point : loop) { + final double x0 = x1; + final double y0 = y1; + x1 = point.getX(); + y1 = point.getY(); + final double factor = x0 * y1 - y0 * x1; + sum += factor; + sumX += factor * (x0 + x1); + sumY += factor * (y0 + y1); + } + } + + if (sum < 0) { + // the polygon as a finite outside surrounded by an infinite inside + setSize(Double.POSITIVE_INFINITY); + setBarycenter((Point<Euclidean2D>) Vector2D.NaN); + } else { + setSize(sum / 2); + setBarycenter((Point<Euclidean2D>) new Vector2D(sumX / (3 * sum), sumY / (3 * sum))); + } + + } + + } + + /** Get the vertices of the polygon. + * <p>The polygon boundary can be represented as an array of loops, + * each loop being itself an array of vertices.</p> + * <p>In order to identify open loops which start and end by + * infinite edges, the open loops arrays start with a null point. In + * this case, the first non null point and the last point of the + * array do not represent real vertices, they are dummy points + * intended only to get the direction of the first and last edge. An + * open loop consisting of a single infinite line will therefore be + * represented by a three elements array with one null point + * followed by two dummy points. The open loops are always the first + * ones in the loops array.</p> + * <p>If the polygon has no boundary at all, a zero length loop + * array will be returned.</p> + * <p>All line segments in the various loops have the inside of the + * region on their left side and the outside on their right side + * when moving in the underlying line direction. This means that + * closed loops surrounding finite areas obey the direct + * trigonometric orientation.</p> + * @return vertices of the polygon, organized as oriented boundary + * loops with the open loops first (the returned value is guaranteed + * to be non-null) + */ + public Vector2D[][] getVertices() { + if (vertices == null) { + if (getTree(false).getCut() == null) { + vertices = new Vector2D[0][]; + } else { + + // build the unconnected segments + final SegmentsBuilder visitor = new SegmentsBuilder(getTolerance()); + getTree(true).visit(visitor); + final List<ConnectableSegment> segments = visitor.getSegments(); + + // connect all segments, using topological criteria first + // and using Euclidean distance only as a last resort + int pending = segments.size(); + pending -= naturalFollowerConnections(segments); + if (pending > 0) { + pending -= splitEdgeConnections(segments); + } + if (pending > 0) { + pending -= closeVerticesConnections(segments); + } + + // create the segment loops + final ArrayList<List<Segment>> loops = new ArrayList<List<Segment>>(); + for (ConnectableSegment s = getUnprocessed(segments); s != null; s = getUnprocessed(segments)) { + final List<Segment> loop = followLoop(s); + if (loop != null) { + if (loop.get(0).getStart() == null) { + // this is an open loop, we put it on the front + loops.add(0, loop); + } else { + // this is a closed loop, we put it on the back + loops.add(loop); + } + } + } + + // transform the loops in an array of arrays of points + vertices = new Vector2D[loops.size()][]; + int i = 0; + + for (final List<Segment> loop : loops) { + if (loop.size() < 2 || + (loop.size() == 2 && loop.get(0).getStart() == null && loop.get(1).getEnd() == null)) { + // single infinite line + final Line line = loop.get(0).getLine(); + vertices[i++] = new Vector2D[] { + null, + line.toSpace((Point<Euclidean1D>) new Vector1D(-Float.MAX_VALUE)), + line.toSpace((Point<Euclidean1D>) new Vector1D(+Float.MAX_VALUE)) + }; + } else if (loop.get(0).getStart() == null) { + // open loop with at least one real point + final Vector2D[] array = new Vector2D[loop.size() + 2]; + int j = 0; + for (Segment segment : loop) { + + if (j == 0) { + // null point and first dummy point + double x = segment.getLine().toSubSpace((Point<Euclidean2D>) segment.getEnd()).getX(); + x -= FastMath.max(1.0, FastMath.abs(x / 2)); + array[j++] = null; + array[j++] = segment.getLine().toSpace((Point<Euclidean1D>) new Vector1D(x)); + } + + if (j < (array.length - 1)) { + // current point + array[j++] = segment.getEnd(); + } + + if (j == (array.length - 1)) { + // last dummy point + double x = segment.getLine().toSubSpace((Point<Euclidean2D>) segment.getStart()).getX(); + x += FastMath.max(1.0, FastMath.abs(x / 2)); + array[j++] = segment.getLine().toSpace((Point<Euclidean1D>) new Vector1D(x)); + } + + } + vertices[i++] = array; + } else { + final Vector2D[] array = new Vector2D[loop.size()]; + int j = 0; + for (Segment segment : loop) { + array[j++] = segment.getStart(); + } + vertices[i++] = array; + } + } + + } + } + + return vertices.clone(); + + } + + /** Connect the segments using only natural follower information. + * @param segments segments complete segments list + * @return number of connections performed + */ + private int naturalFollowerConnections(final List<ConnectableSegment> segments) { + int connected = 0; + for (final ConnectableSegment segment : segments) { + if (segment.getNext() == null) { + final BSPTree<Euclidean2D> node = segment.getNode(); + final BSPTree<Euclidean2D> end = segment.getEndNode(); + for (final ConnectableSegment candidateNext : segments) { + if (candidateNext.getPrevious() == null && + candidateNext.getNode() == end && + candidateNext.getStartNode() == node) { + // connect the two segments + segment.setNext(candidateNext); + candidateNext.setPrevious(segment); + ++connected; + break; + } + } + } + } + return connected; + } + + /** Connect the segments resulting from a line splitting a straight edge. + * @param segments segments complete segments list + * @return number of connections performed + */ + private int splitEdgeConnections(final List<ConnectableSegment> segments) { + int connected = 0; + for (final ConnectableSegment segment : segments) { + if (segment.getNext() == null) { + final Hyperplane<Euclidean2D> hyperplane = segment.getNode().getCut().getHyperplane(); + final BSPTree<Euclidean2D> end = segment.getEndNode(); + for (final ConnectableSegment candidateNext : segments) { + if (candidateNext.getPrevious() == null && + candidateNext.getNode().getCut().getHyperplane() == hyperplane && + candidateNext.getStartNode() == end) { + // connect the two segments + segment.setNext(candidateNext); + candidateNext.setPrevious(segment); + ++connected; + break; + } + } + } + } + return connected; + } + + /** Connect the segments using Euclidean distance. + * <p> + * This connection heuristic should be used last, as it relies + * only on a fuzzy distance criterion. + * </p> + * @param segments segments complete segments list + * @return number of connections performed + */ + private int closeVerticesConnections(final List<ConnectableSegment> segments) { + int connected = 0; + for (final ConnectableSegment segment : segments) { + if (segment.getNext() == null && segment.getEnd() != null) { + final Vector2D end = segment.getEnd(); + ConnectableSegment selectedNext = null; + double min = Double.POSITIVE_INFINITY; + for (final ConnectableSegment candidateNext : segments) { + if (candidateNext.getPrevious() == null && candidateNext.getStart() != null) { + final double distance = Vector2D.distance(end, candidateNext.getStart()); + if (distance < min) { + selectedNext = candidateNext; + min = distance; + } + } + } + if (min <= getTolerance()) { + // connect the two segments + segment.setNext(selectedNext); + selectedNext.setPrevious(segment); + ++connected; + } + } + } + return connected; + } + + /** Get first unprocessed segment from a list. + * @param segments segments list + * @return first segment that has not been processed yet + * or null if all segments have been processed + */ + private ConnectableSegment getUnprocessed(final List<ConnectableSegment> segments) { + for (final ConnectableSegment segment : segments) { + if (!segment.isProcessed()) { + return segment; + } + } + return null; + } + + /** Build the loop containing a segment. + * <p> + * The segment put in the loop will be marked as processed. + * </p> + * @param defining segment used to define the loop + * @return loop containing the segment (may be null if the loop is a + * degenerated infinitely thin 2 points loop + */ + private List<Segment> followLoop(final ConnectableSegment defining) { + + final List<Segment> loop = new ArrayList<Segment>(); + loop.add(defining); + defining.setProcessed(true); + + // add segments in connection order + ConnectableSegment next = defining.getNext(); + while (next != defining && next != null) { + loop.add(next); + next.setProcessed(true); + next = next.getNext(); + } + + if (next == null) { + // the loop is open and we have found its end, + // we need to find its start too + ConnectableSegment previous = defining.getPrevious(); + while (previous != null) { + loop.add(0, previous); + previous.setProcessed(true); + previous = previous.getPrevious(); + } + } + + // filter out spurious vertices + filterSpuriousVertices(loop); + + if (loop.size() == 2 && loop.get(0).getStart() != null) { + // this is a degenerated infinitely thin closed loop, we simply ignore it + return null; + } else { + return loop; + } + + } + + /** Filter out spurious vertices on straight lines (at machine precision). + * @param loop segments loop to filter (will be modified in-place) + */ + private void filterSpuriousVertices(final List<Segment> loop) { + for (int i = 0; i < loop.size(); ++i) { + final Segment previous = loop.get(i); + int j = (i + 1) % loop.size(); + final Segment next = loop.get(j); + if (next != null && + Precision.equals(previous.getLine().getAngle(), next.getLine().getAngle(), Precision.EPSILON)) { + // the vertex between the two edges is a spurious one + // replace the two segments by a single one + loop.set(j, new Segment(previous.getStart(), next.getEnd(), previous.getLine())); + loop.remove(i--); + } + } + } + + /** Private extension of Segment allowing connection. */ + private static class ConnectableSegment extends Segment { + + /** Node containing segment. */ + private final BSPTree<Euclidean2D> node; + + /** Node whose intersection with current node defines start point. */ + private final BSPTree<Euclidean2D> startNode; + + /** Node whose intersection with current node defines end point. */ + private final BSPTree<Euclidean2D> endNode; + + /** Previous segment. */ + private ConnectableSegment previous; + + /** Next segment. */ + private ConnectableSegment next; + + /** Indicator for completely processed segments. */ + private boolean processed; + + /** Build a segment. + * @param start start point of the segment + * @param end end point of the segment + * @param line line containing the segment + * @param node node containing the segment + * @param startNode node whose intersection with current node defines start point + * @param endNode node whose intersection with current node defines end point + */ + ConnectableSegment(final Vector2D start, final Vector2D end, final Line line, + final BSPTree<Euclidean2D> node, + final BSPTree<Euclidean2D> startNode, + final BSPTree<Euclidean2D> endNode) { + super(start, end, line); + this.node = node; + this.startNode = startNode; + this.endNode = endNode; + this.previous = null; + this.next = null; + this.processed = false; + } + + /** Get the node containing segment. + * @return node containing segment + */ + public BSPTree<Euclidean2D> getNode() { + return node; + } + + /** Get the node whose intersection with current node defines start point. + * @return node whose intersection with current node defines start point + */ + public BSPTree<Euclidean2D> getStartNode() { + return startNode; + } + + /** Get the node whose intersection with current node defines end point. + * @return node whose intersection with current node defines end point + */ + public BSPTree<Euclidean2D> getEndNode() { + return endNode; + } + + /** Get the previous segment. + * @return previous segment + */ + public ConnectableSegment getPrevious() { + return previous; + } + + /** Set the previous segment. + * @param previous previous segment + */ + public void setPrevious(final ConnectableSegment previous) { + this.previous = previous; + } + + /** Get the next segment. + * @return next segment + */ + public ConnectableSegment getNext() { + return next; + } + + /** Set the next segment. + * @param next previous segment + */ + public void setNext(final ConnectableSegment next) { + this.next = next; + } + + /** Set the processed flag. + * @param processed processed flag to set + */ + public void setProcessed(final boolean processed) { + this.processed = processed; + } + + /** Check if the segment has been processed. + * @return true if the segment has been processed + */ + public boolean isProcessed() { + return processed; + } + + } + + /** Visitor building segments. */ + private static class SegmentsBuilder implements BSPTreeVisitor<Euclidean2D> { + + /** Tolerance for close nodes connection. */ + private final double tolerance; + + /** Built segments. */ + private final List<ConnectableSegment> segments; + + /** Simple constructor. + * @param tolerance tolerance for close nodes connection + */ + SegmentsBuilder(final double tolerance) { + this.tolerance = tolerance; + this.segments = new ArrayList<ConnectableSegment>(); + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<Euclidean2D> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<Euclidean2D> node) { + @SuppressWarnings("unchecked") + final BoundaryAttribute<Euclidean2D> attribute = (BoundaryAttribute<Euclidean2D>) node.getAttribute(); + final Iterable<BSPTree<Euclidean2D>> splitters = attribute.getSplitters(); + if (attribute.getPlusOutside() != null) { + addContribution(attribute.getPlusOutside(), node, splitters, false); + } + if (attribute.getPlusInside() != null) { + addContribution(attribute.getPlusInside(), node, splitters, true); + } + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<Euclidean2D> node) { + } + + /** Add the contribution of a boundary facet. + * @param sub boundary facet + * @param node node containing segment + * @param splitters splitters for the boundary facet + * @param reversed if true, the facet has the inside on its plus side + */ + private void addContribution(final SubHyperplane<Euclidean2D> sub, + final BSPTree<Euclidean2D> node, + final Iterable<BSPTree<Euclidean2D>> splitters, + final boolean reversed) { + @SuppressWarnings("unchecked") + final AbstractSubHyperplane<Euclidean2D, Euclidean1D> absSub = + (AbstractSubHyperplane<Euclidean2D, Euclidean1D>) sub; + final Line line = (Line) sub.getHyperplane(); + final List<Interval> intervals = ((IntervalsSet) absSub.getRemainingRegion()).asList(); + for (final Interval i : intervals) { + + // find the 2D points + final Vector2D startV = Double.isInfinite(i.getInf()) ? + null : (Vector2D) line.toSpace((Point<Euclidean1D>) new Vector1D(i.getInf())); + final Vector2D endV = Double.isInfinite(i.getSup()) ? + null : (Vector2D) line.toSpace((Point<Euclidean1D>) new Vector1D(i.getSup())); + + // recover the connectivity information + final BSPTree<Euclidean2D> startN = selectClosest(startV, splitters); + final BSPTree<Euclidean2D> endN = selectClosest(endV, splitters); + + if (reversed) { + segments.add(new ConnectableSegment(endV, startV, line.getReverse(), + node, endN, startN)); + } else { + segments.add(new ConnectableSegment(startV, endV, line, + node, startN, endN)); + } + + } + } + + /** Select the node whose cut sub-hyperplane is closest to specified point. + * @param point reference point + * @param candidates candidate nodes + * @return node closest to point, or null if no node is closer than tolerance + */ + private BSPTree<Euclidean2D> selectClosest(final Vector2D point, final Iterable<BSPTree<Euclidean2D>> candidates) { + + BSPTree<Euclidean2D> selected = null; + double min = Double.POSITIVE_INFINITY; + + for (final BSPTree<Euclidean2D> node : candidates) { + final double distance = FastMath.abs(node.getCut().getHyperplane().getOffset(point)); + if (distance < min) { + selected = node; + min = distance; + } + } + + return min <= tolerance ? selected : null; + + } + + /** Get the segments. + * @return built segments + */ + public List<ConnectableSegment> getSegments() { + return segments; + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Segment.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Segment.java new file mode 100644 index 0000000..2ef7f4e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Segment.java @@ -0,0 +1,112 @@ +/* + * 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.twod; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.util.FastMath; + +/** Simple container for a two-points segment. + * @since 3.0 + */ +public class Segment { + + /** Start point of the segment. */ + private final Vector2D start; + + /** End point of the segment. */ + private final Vector2D 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 Vector2D start, final Vector2D 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 Vector2D getStart() { + return start; + } + + /** Get the end point of the segment. + * @return end point of the segment + */ + public Vector2D getEnd() { + return end; + } + + /** Get the line containing the segment. + * @return line containing the segment + */ + public Line getLine() { + return line; + } + + /** Calculates the shortest distance from a point to this line segment. + * <p> + * If the perpendicular extension from the point to the line does not + * cross in the bounds of the line segment, the shortest distance to + * the two end points will be returned. + * </p> + * + * Algorithm adapted from: + * <a href="http://www.codeguru.com/forum/printthread.php?s=cc8cf0596231f9a7dba4da6e77c29db3&t=194400&pp=15&page=1"> + * Thread @ Codeguru</a> + * + * @param p to check + * @return distance between the instance and the point + * @since 3.1 + */ + public double distance(final Vector2D p) { + final double deltaX = end.getX() - start.getX(); + final double deltaY = end.getY() - start.getY(); + + final double r = ((p.getX() - start.getX()) * deltaX + (p.getY() - start.getY()) * deltaY) / + (deltaX * deltaX + deltaY * deltaY); + + // r == 0 => P = startPt + // r == 1 => P = endPt + // r < 0 => P is on the backward extension of the segment + // r > 1 => P is on the forward extension of the segment + // 0 < r < 1 => P is on the segment + + // if point isn't on the line segment, just return the shortest distance to the end points + if (r < 0 || r > 1) { + final double dist1 = getStart().distance((Point<Euclidean2D>) p); + final double dist2 = getEnd().distance((Point<Euclidean2D>) p); + + return FastMath.min(dist1, dist2); + } + else { + // find point on line and see if it is in the line segment + final double px = start.getX() + r * deltaX; + final double py = start.getY() + r * deltaY; + + final Vector2D interPt = new Vector2D(px, py); + return interPt.distance((Point<Euclidean2D>) p); + } + } +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/SubLine.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/SubLine.java new file mode 100644 index 0000000..d930b76 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/SubLine.java @@ -0,0 +1,214 @@ +/* + * 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.twod; + +import java.util.ArrayList; +import java.util.List; + +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.OrientedPoint; +import org.apache.commons.math3.geometry.euclidean.oned.Vector1D; +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.Region.Location; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.util.FastMath; + +/** This class represents a sub-hyperplane for {@link Line}. + * @since 3.0 + */ +public class SubLine extends AbstractSubHyperplane<Euclidean2D, Euclidean1D> { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1.0e-10; + + /** Simple constructor. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + public SubLine(final Hyperplane<Euclidean2D> hyperplane, + final Region<Euclidean1D> remainingRegion) { + super(hyperplane, 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 + * @since 3.3 + */ + public SubLine(final Vector2D start, final Vector2D end, final double tolerance) { + super(new Line(start, end, tolerance), buildIntervalSet(start, end, tolerance)); + } + + /** Create a sub-line from two endpoints. + * @param start start point + * @param end end point + * @deprecated as of 3.3, replaced with {@link #SubLine(Vector2D, Vector2D, double)} + */ + @Deprecated + public SubLine(final Vector2D start, final Vector2D end) { + this(start, end, DEFAULT_TOLERANCE); + } + + /** Create a sub-line from a segment. + * @param segment single segment forming the sub-line + */ + public SubLine(final Segment segment) { + super(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 Line line = (Line) getHyperplane(); + final List<Interval> list = ((IntervalsSet) getRemainingRegion()).asList(); + final List<Segment> segments = new ArrayList<Segment>(list.size()); + + for (final Interval interval : list) { + final Vector2D start = line.toSpace((Point<Euclidean1D>) new Vector1D(interval.getInf())); + final Vector2D 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 Vector2D intersection(final SubLine subLine, final boolean includeEndPoints) { + + // retrieve the underlying lines + Line line1 = (Line) getHyperplane(); + Line line2 = (Line) subLine.getHyperplane(); + + // compute the intersection on infinite line + Vector2D v2D = line1.intersection(line2); + if (v2D == null) { + return null; + } + + // check location of point with respect to first sub-line + Location loc1 = getRemainingRegion().checkPoint(line1.toSubSpace((Point<Euclidean2D>) v2D)); + + // check location of point with respect to second sub-line + Location loc2 = subLine.getRemainingRegion().checkPoint(line2.toSubSpace((Point<Euclidean2D>) v2D)); + + if (includeEndPoints) { + return ((loc1 != Location.OUTSIDE) && (loc2 != Location.OUTSIDE)) ? v2D : null; + } else { + return ((loc1 == Location.INSIDE) && (loc2 == Location.INSIDE)) ? v2D : null; + } + + } + + /** Build an interval set from two points. + * @param start start point + * @param end end point + * @param tolerance tolerance below which points are considered identical + * @return an interval set + */ + private static IntervalsSet buildIntervalSet(final Vector2D start, final Vector2D end, final double tolerance) { + final Line line = new Line(start, end, tolerance); + return new IntervalsSet(line.toSubSpace((Point<Euclidean2D>) start).getX(), + line.toSubSpace((Point<Euclidean2D>) end).getX(), + tolerance); + } + + /** {@inheritDoc} */ + @Override + protected AbstractSubHyperplane<Euclidean2D, Euclidean1D> buildNew(final Hyperplane<Euclidean2D> hyperplane, + final Region<Euclidean1D> remainingRegion) { + return new SubLine(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public SplitSubHyperplane<Euclidean2D> split(final Hyperplane<Euclidean2D> hyperplane) { + + final Line thisLine = (Line) getHyperplane(); + final Line otherLine = (Line) hyperplane; + final Vector2D crossing = thisLine.intersection(otherLine); + final double tolerance = thisLine.getTolerance(); + + if (crossing == null) { + // the lines are parallel + final double global = otherLine.getOffset(thisLine); + if (global < -tolerance) { + return new SplitSubHyperplane<Euclidean2D>(null, this); + } else if (global > tolerance) { + return new SplitSubHyperplane<Euclidean2D>(this, null); + } else { + return new SplitSubHyperplane<Euclidean2D>(null, null); + } + } + + // the lines do intersect + final boolean direct = FastMath.sin(thisLine.getAngle() - otherLine.getAngle()) < 0; + final Vector1D x = thisLine.toSubSpace((Point<Euclidean2D>) crossing); + final SubHyperplane<Euclidean1D> subPlus = + new OrientedPoint(x, !direct, tolerance).wholeHyperplane(); + final SubHyperplane<Euclidean1D> subMinus = + new OrientedPoint(x, direct, tolerance).wholeHyperplane(); + + final BSPTree<Euclidean1D> splitTree = getRemainingRegion().getTree(false).split(subMinus); + final BSPTree<Euclidean1D> plusTree = getRemainingRegion().isEmpty(splitTree.getPlus()) ? + new BSPTree<Euclidean1D>(Boolean.FALSE) : + new BSPTree<Euclidean1D>(subPlus, new BSPTree<Euclidean1D>(Boolean.FALSE), + splitTree.getPlus(), null); + final BSPTree<Euclidean1D> minusTree = getRemainingRegion().isEmpty(splitTree.getMinus()) ? + new BSPTree<Euclidean1D>(Boolean.FALSE) : + new BSPTree<Euclidean1D>(subMinus, new BSPTree<Euclidean1D>(Boolean.FALSE), + splitTree.getMinus(), null); + return new SplitSubHyperplane<Euclidean2D>(new SubLine(thisLine.copySelf(), new IntervalsSet(plusTree, tolerance)), + new SubLine(thisLine.copySelf(), new IntervalsSet(minusTree, tolerance))); + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2D.java new file mode 100644 index 0000000..191d225 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2D.java @@ -0,0 +1,460 @@ +/* + * 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.twod; + +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 represents a 2D vector. + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.0 + */ +public class Vector2D implements Vector<Euclidean2D> { + + /** Origin (coordinates: 0, 0). */ + public static final Vector2D ZERO = new Vector2D(0, 0); + + // CHECKSTYLE: stop ConstantName + /** A vector with all coordinates set to NaN. */ + public static final Vector2D NaN = new Vector2D(Double.NaN, Double.NaN); + // CHECKSTYLE: resume ConstantName + + /** A vector with all coordinates set to positive infinity. */ + public static final Vector2D POSITIVE_INFINITY = + new Vector2D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); + + /** A vector with all coordinates set to negative infinity. */ + public static final Vector2D NEGATIVE_INFINITY = + new Vector2D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY); + + /** Serializable UID. */ + private static final long serialVersionUID = 266938651998679754L; + + /** Abscissa. */ + private final double x; + + /** Ordinate. */ + private final double y; + + /** Simple constructor. + * Build a vector from its coordinates + * @param x abscissa + * @param y ordinate + * @see #getX() + * @see #getY() + */ + public Vector2D(double x, double y) { + this.x = x; + this.y = y; + } + + /** Simple constructor. + * Build a vector from its coordinates + * @param v coordinates array + * @exception DimensionMismatchException if array does not have 2 elements + * @see #toArray() + */ + public Vector2D(double[] v) throws DimensionMismatchException { + if (v.length != 2) { + throw new DimensionMismatchException(v.length, 2); + } + this.x = v[0]; + this.y = v[1]; + } + + /** 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 Vector2D(double a, Vector2D u) { + this.x = a * u.x; + this.y = a * u.y; + } + + /** 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 Vector2D(double a1, Vector2D u1, double a2, Vector2D u2) { + this.x = a1 * u1.x + a2 * u2.x; + this.y = a1 * u1.y + a2 * u2.y; + } + + /** 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 Vector2D(double a1, Vector2D u1, double a2, Vector2D u2, + double a3, Vector2D u3) { + this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x; + this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y; + } + + /** 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 Vector2D(double a1, Vector2D u1, double a2, Vector2D u2, + double a3, Vector2D u3, double a4, Vector2D u4) { + this.x = a1 * u1.x + a2 * u2.x + a3 * u3.x + a4 * u4.x; + this.y = a1 * u1.y + a2 * u2.y + a3 * u3.y + a4 * u4.y; + } + + /** Get the abscissa of the vector. + * @return abscissa of the vector + * @see #Vector2D(double, double) + */ + public double getX() { + return x; + } + + /** Get the ordinate of the vector. + * @return ordinate of the vector + * @see #Vector2D(double, double) + */ + public double getY() { + return y; + } + + /** Get the vector coordinates as a dimension 2 array. + * @return vector coordinates + * @see #Vector2D(double[]) + */ + public double[] toArray() { + return new double[] { x, y }; + } + + /** {@inheritDoc} */ + public Space getSpace() { + return Euclidean2D.getInstance(); + } + + /** {@inheritDoc} */ + public Vector2D getZero() { + return ZERO; + } + + /** {@inheritDoc} */ + public double getNorm1() { + return FastMath.abs(x) + FastMath.abs(y); + } + + /** {@inheritDoc} */ + public double getNorm() { + return FastMath.sqrt (x * x + y * y); + } + + /** {@inheritDoc} */ + public double getNormSq() { + return x * x + y * y; + } + + /** {@inheritDoc} */ + public double getNormInf() { + return FastMath.max(FastMath.abs(x), FastMath.abs(y)); + } + + /** {@inheritDoc} */ + public Vector2D add(Vector<Euclidean2D> v) { + Vector2D v2 = (Vector2D) v; + return new Vector2D(x + v2.getX(), y + v2.getY()); + } + + /** {@inheritDoc} */ + public Vector2D add(double factor, Vector<Euclidean2D> v) { + Vector2D v2 = (Vector2D) v; + return new Vector2D(x + factor * v2.getX(), y + factor * v2.getY()); + } + + /** {@inheritDoc} */ + public Vector2D subtract(Vector<Euclidean2D> p) { + Vector2D p3 = (Vector2D) p; + return new Vector2D(x - p3.x, y - p3.y); + } + + /** {@inheritDoc} */ + public Vector2D subtract(double factor, Vector<Euclidean2D> v) { + Vector2D v2 = (Vector2D) v; + return new Vector2D(x - factor * v2.getX(), y - factor * v2.getY()); + } + + /** {@inheritDoc} */ + public Vector2D normalize() throws MathArithmeticException { + double s = getNorm(); + if (s == 0) { + throw new MathArithmeticException(LocalizedFormats.CANNOT_NORMALIZE_A_ZERO_NORM_VECTOR); + } + return scalarMultiply(1 / s); + } + + /** 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(Vector2D v1, Vector2D 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 + final double n = FastMath.abs(MathArrays.linearCombination(v1.x, v2.y, -v1.y, v2.x)); + if (dot >= 0) { + return FastMath.asin(n / normProduct); + } + return FastMath.PI - FastMath.asin(n / normProduct); + } + + // the vectors are sufficiently separated to use the cosine + return FastMath.acos(dot / normProduct); + + } + + /** {@inheritDoc} */ + public Vector2D negate() { + return new Vector2D(-x, -y); + } + + /** {@inheritDoc} */ + public Vector2D scalarMultiply(double a) { + return new Vector2D(a * x, a * y); + } + + /** {@inheritDoc} */ + public boolean isNaN() { + return Double.isNaN(x) || Double.isNaN(y); + } + + /** {@inheritDoc} */ + public boolean isInfinite() { + return !isNaN() && (Double.isInfinite(x) || Double.isInfinite(y)); + } + + /** {@inheritDoc} */ + public double distance1(Vector<Euclidean2D> p) { + Vector2D p3 = (Vector2D) p; + final double dx = FastMath.abs(p3.x - x); + final double dy = FastMath.abs(p3.y - y); + return dx + dy; + } + + /** {@inheritDoc} + */ + public double distance(Vector<Euclidean2D> p) { + return distance((Point<Euclidean2D>) p); + } + + /** {@inheritDoc} */ + public double distance(Point<Euclidean2D> p) { + Vector2D p3 = (Vector2D) p; + final double dx = p3.x - x; + final double dy = p3.y - y; + return FastMath.sqrt(dx * dx + dy * dy); + } + + /** {@inheritDoc} */ + public double distanceInf(Vector<Euclidean2D> p) { + Vector2D p3 = (Vector2D) p; + final double dx = FastMath.abs(p3.x - x); + final double dy = FastMath.abs(p3.y - y); + return FastMath.max(dx, dy); + } + + /** {@inheritDoc} */ + public double distanceSq(Vector<Euclidean2D> p) { + Vector2D p3 = (Vector2D) p; + final double dx = p3.x - x; + final double dy = p3.y - y; + return dx * dx + dy * dy; + } + + /** {@inheritDoc} */ + public double dotProduct(final Vector<Euclidean2D> v) { + final Vector2D v2 = (Vector2D) v; + return MathArrays.linearCombination(x, v2.x, y, v2.y); + } + + /** + * Compute the cross-product of the instance and the given points. + * <p> + * The cross product can be used to determine the location of a point + * with regard to the line formed by (p1, p2) and is calculated as: + * \[ + * P = (x_2 - x_1)(y_3 - y_1) - (y_2 - y_1)(x_3 - x_1) + * \] + * with \(p3 = (x_3, y_3)\) being this instance. + * <p> + * If the result is 0, the points are collinear, i.e. lie on a single straight line L; + * if it is positive, this point lies to the left, otherwise to the right of the line + * formed by (p1, p2). + * + * @param p1 first point of the line + * @param p2 second point of the line + * @return the cross-product + * + * @see <a href="http://en.wikipedia.org/wiki/Cross_product">Cross product (Wikipedia)</a> + */ + public double crossProduct(final Vector2D p1, final Vector2D p2) { + final double x1 = p2.getX() - p1.getX(); + final double y1 = getY() - p1.getY(); + final double x2 = getX() - p1.getX(); + final double y2 = p2.getY() - p1.getY(); + return MathArrays.linearCombination(x1, y1, -x2, y2); + } + + /** Compute the distance between two vectors according to the L<sub>2</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNorm()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the distance between p1 and p2 according to the L<sub>2</sub> norm + */ + public static double distance(Vector2D p1, Vector2D p2) { + return p1.distance(p2); + } + + /** Compute the distance between two vectors according to the L<sub>∞</sub> norm. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNormInf()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the distance between p1 and p2 according to the L<sub>∞</sub> norm + */ + public static double distanceInf(Vector2D p1, Vector2D p2) { + return p1.distanceInf(p2); + } + + /** Compute the square of the distance between two vectors. + * <p>Calling this method is equivalent to calling: + * <code>p1.subtract(p2).getNormSq()</code> except that no intermediate + * vector is built</p> + * @param p1 first vector + * @param p2 second vector + * @return the square of the distance between p1 and p2 + */ + public static double distanceSq(Vector2D p1, Vector2D p2) { + return p1.distanceSq(p2); + } + + /** + * Test for the equality of two 2D vectors. + * <p> + * If all coordinates of two 2D vectors are exactly the same, and none are + * <code>Double.NaN</code>, the two 2D 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 + * 2D vector are equal to <code>Double.NaN</code>, the 2D vector is equal to + * {@link #NaN}. + * </p> + * + * @param other Object to test for equality to this + * @return true if two 2D vector objects are equal, false if + * object is null, not an instance of Vector2D, or + * not equal to this Vector2D instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof Vector2D) { + final Vector2D rhs = (Vector2D)other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return (x == rhs.x) && (y == rhs.y); + } + return false; + } + + /** + * Get a hashCode for the 2D 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 542; + } + return 122 * (76 * MathUtils.hash(x) + MathUtils.hash(y)); + } + + /** Get a string representation of this vector. + * @return a string representation of this vector + */ + @Override + public String toString() { + return Vector2DFormat.getInstance().format(this); + } + + /** {@inheritDoc} */ + public String toString(final NumberFormat format) { + return new Vector2DFormat(format).format(this); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2DFormat.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2DFormat.java new file mode 100644 index 0000000..21261c5 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/Vector2DFormat.java @@ -0,0 +1,138 @@ +/* + * 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.twod; + +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 2D vector in components list format "{x; y}". + * <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}" and + * " { 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> + * + * @since 3.0 + */ +public class Vector2DFormat extends VectorFormat<Euclidean2D> { + + /** + * 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 Vector2DFormat() { + 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 Vector2DFormat(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 Vector2DFormat(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 Vector2DFormat(final String prefix, final String suffix, + final String separator, final NumberFormat format) { + super(prefix, suffix, separator, format); + } + + /** + * Returns the default 2D vector format for the current locale. + * @return the default 2D vector format. + */ + public static Vector2DFormat getInstance() { + return getInstance(Locale.getDefault()); + } + + /** + * Returns the default 2D vector format for the given locale. + * @param locale the specific locale used by the format. + * @return the 2D vector format specific to the given locale. + */ + public static Vector2DFormat getInstance(final Locale locale) { + return new Vector2DFormat(CompositeFormat.getDefaultNumberFormat(locale)); + } + + /** {@inheritDoc} */ + @Override + public StringBuffer format(final Vector<Euclidean2D> vector, final StringBuffer toAppendTo, + final FieldPosition pos) { + final Vector2D p2 = (Vector2D) vector; + return format(toAppendTo, pos, p2.getX(), p2.getY()); + } + + /** {@inheritDoc} */ + @Override + public Vector2D parse(final String source) throws MathParseException { + ParsePosition parsePosition = new ParsePosition(0); + Vector2D result = parse(source, parsePosition); + if (parsePosition.getIndex() == 0) { + throw new MathParseException(source, + parsePosition.getErrorIndex(), + Vector2D.class); + } + return result; + } + + /** {@inheritDoc} */ + @Override + public Vector2D parse(final String source, final ParsePosition pos) { + final double[] coordinates = parseCoordinates(2, source, pos); + if (coordinates == null) { + return null; + } + return new Vector2D(coordinates[0], coordinates[1]); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AbstractConvexHullGenerator2D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AbstractConvexHullGenerator2D.java new file mode 100644 index 0000000..b234ad5 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AbstractConvexHullGenerator2D.java @@ -0,0 +1,116 @@ +/* + * 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.twod.hull; + +import java.util.Collection; + +import org.apache.commons.math3.exception.ConvergenceException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.NullArgumentException; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.util.MathUtils; + +/** + * Abstract base class for convex hull generators in the two-dimensional euclidean space. + * + * @since 3.3 + */ +abstract class AbstractConvexHullGenerator2D implements ConvexHullGenerator2D { + + /** Default value for tolerance. */ + private static final double DEFAULT_TOLERANCE = 1e-10; + + /** Tolerance below which points are considered identical. */ + private final double tolerance; + + /** + * Indicates if collinear points on the hull shall be present in the output. + * If {@code false}, only the extreme points are added to the hull. + */ + private final boolean includeCollinearPoints; + + /** + * Simple constructor. + * <p> + * The default tolerance (1e-10) will be used to determine identical points. + * + * @param includeCollinearPoints indicates if collinear points on the hull shall be + * added as hull vertices + */ + protected AbstractConvexHullGenerator2D(final boolean includeCollinearPoints) { + this(includeCollinearPoints, DEFAULT_TOLERANCE); + } + + /** + * Simple constructor. + * + * @param includeCollinearPoints indicates if collinear points on the hull shall be + * added as hull vertices + * @param tolerance tolerance below which points are considered identical + */ + protected AbstractConvexHullGenerator2D(final boolean includeCollinearPoints, final double tolerance) { + this.includeCollinearPoints = includeCollinearPoints; + this.tolerance = tolerance; + } + + /** + * Get the tolerance below which points are considered identical. + * @return the tolerance below which points are considered identical + */ + public double getTolerance() { + return tolerance; + } + + /** + * Returns if collinear points on the hull will be added as hull vertices. + * @return {@code true} if collinear points are added as hull vertices, or {@code false} + * if only extreme points are present. + */ + public boolean isIncludeCollinearPoints() { + return includeCollinearPoints; + } + + /** {@inheritDoc} */ + public ConvexHull2D generate(final Collection<Vector2D> points) + throws NullArgumentException, ConvergenceException { + // check for null points + MathUtils.checkNotNull(points); + + Collection<Vector2D> hullVertices = null; + if (points.size() < 2) { + hullVertices = points; + } else { + hullVertices = findHullVertices(points); + } + + try { + return new ConvexHull2D(hullVertices.toArray(new Vector2D[hullVertices.size()]), + tolerance); + } catch (MathIllegalArgumentException e) { + // the hull vertices may not form a convex hull if the tolerance value is to large + throw new ConvergenceException(); + } + } + + /** + * Find the convex hull vertices from the set of input points. + * @param points the set of input points + * @return the convex hull vertices in CCW winding + */ + protected abstract Collection<Vector2D> findHullVertices(Collection<Vector2D> points); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AklToussaintHeuristic.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AklToussaintHeuristic.java new file mode 100644 index 0000000..f5d1b84 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/AklToussaintHeuristic.java @@ -0,0 +1,153 @@ +/* + * 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.twod.hull; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.List; + +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; + +/** + * A simple heuristic to improve the performance of convex hull algorithms. + * <p> + * The heuristic is based on the idea of a convex quadrilateral, which is formed by + * four points with the lowest and highest x / y coordinates. Any point that lies inside + * this quadrilateral can not be part of the convex hull and can thus be safely discarded + * before generating the convex hull itself. + * <p> + * The complexity of the operation is O(n), and may greatly improve the time it takes to + * construct the convex hull afterwards, depending on the point distribution. + * + * @see <a href="http://en.wikipedia.org/wiki/Convex_hull_algorithms#Akl-Toussaint_heuristic"> + * Akl-Toussaint heuristic (Wikipedia)</a> + * @since 3.3 + */ +public final class AklToussaintHeuristic { + + /** Hide utility constructor. */ + private AklToussaintHeuristic() { + } + + /** + * Returns a point set that is reduced by all points for which it is safe to assume + * that they are not part of the convex hull. + * + * @param points the original point set + * @return a reduced point set, useful as input for convex hull algorithms + */ + public static Collection<Vector2D> reducePoints(final Collection<Vector2D> points) { + + // find the leftmost point + int size = 0; + Vector2D minX = null; + Vector2D maxX = null; + Vector2D minY = null; + Vector2D maxY = null; + for (Vector2D p : points) { + if (minX == null || p.getX() < minX.getX()) { + minX = p; + } + if (maxX == null || p.getX() > maxX.getX()) { + maxX = p; + } + if (minY == null || p.getY() < minY.getY()) { + minY = p; + } + if (maxY == null || p.getY() > maxY.getY()) { + maxY = p; + } + size++; + } + + if (size < 4) { + return points; + } + + final List<Vector2D> quadrilateral = buildQuadrilateral(minY, maxX, maxY, minX); + // if the quadrilateral is not well formed, e.g. only 2 points, do not attempt to reduce + if (quadrilateral.size() < 3) { + return points; + } + + final List<Vector2D> reducedPoints = new ArrayList<Vector2D>(quadrilateral); + for (final Vector2D p : points) { + // check all points if they are within the quadrilateral + // in which case they can not be part of the convex hull + if (!insideQuadrilateral(p, quadrilateral)) { + reducedPoints.add(p); + } + } + + return reducedPoints; + } + + /** + * Build the convex quadrilateral with the found corner points (with min/max x/y coordinates). + * + * @param points the respective points with min/max x/y coordinate + * @return the quadrilateral + */ + private static List<Vector2D> buildQuadrilateral(final Vector2D... points) { + List<Vector2D> quadrilateral = new ArrayList<Vector2D>(); + for (Vector2D p : points) { + if (!quadrilateral.contains(p)) { + quadrilateral.add(p); + } + } + return quadrilateral; + } + + /** + * Checks if the given point is located within the convex quadrilateral. + * @param point the point to check + * @param quadrilateralPoints the convex quadrilateral, represented by 4 points + * @return {@code true} if the point is inside the quadrilateral, {@code false} otherwise + */ + private static boolean insideQuadrilateral(final Vector2D point, + final List<Vector2D> quadrilateralPoints) { + + Vector2D p1 = quadrilateralPoints.get(0); + Vector2D p2 = quadrilateralPoints.get(1); + + if (point.equals(p1) || point.equals(p2)) { + return true; + } + + // get the location of the point relative to the first two vertices + final double last = point.crossProduct(p1, p2); + final int size = quadrilateralPoints.size(); + // loop through the rest of the vertices + for (int i = 1; i < size; i++) { + p1 = p2; + p2 = quadrilateralPoints.get((i + 1) == size ? 0 : i + 1); + + if (point.equals(p1) || point.equals(p2)) { + return true; + } + + // do side of line test: multiply the last location with this location + // if they are the same sign then the operation will yield a positive result + // -x * -y = +xy, x * y = +xy, -x * y = -xy, x * -y = -xy + if (last * point.crossProduct(p1, p2) < 0) { + return false; + } + } + return true; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHull2D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHull2D.java new file mode 100644 index 0000000..5d9734b --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHull2D.java @@ -0,0 +1,172 @@ +/* + * 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.twod.hull; + +import java.io.Serializable; + +import org.apache.commons.math3.exception.InsufficientDataException; +import org.apache.commons.math3.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.Line; +import org.apache.commons.math3.geometry.euclidean.twod.Segment; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.hull.ConvexHull; +import org.apache.commons.math3.geometry.partitioning.Region; +import org.apache.commons.math3.geometry.partitioning.RegionFactory; +import org.apache.commons.math3.util.MathArrays; +import org.apache.commons.math3.util.Precision; + +/** + * This class represents a convex hull in an two-dimensional euclidean space. + * + * @since 3.3 + */ +public class ConvexHull2D implements ConvexHull<Euclidean2D, Vector2D>, Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20140129L; + + /** Vertices of the hull. */ + private final Vector2D[] vertices; + + /** Tolerance threshold used during creation of the hull vertices. */ + private final double tolerance; + + /** + * Line segments of the hull. + * The array is not serialized and will be created from the vertices on first access. + */ + private transient Segment[] lineSegments; + + /** + * Simple constructor. + * @param vertices the vertices of the convex hull, must be ordered + * @param tolerance tolerance below which points are considered identical + * @throws MathIllegalArgumentException if the vertices do not form a convex hull + */ + public ConvexHull2D(final Vector2D[] vertices, final double tolerance) + throws MathIllegalArgumentException { + + // assign tolerance as it will be used by the isConvex method + this.tolerance = tolerance; + + if (!isConvex(vertices)) { + throw new MathIllegalArgumentException(LocalizedFormats.NOT_CONVEX); + } + + this.vertices = vertices.clone(); + } + + /** + * Checks whether the given hull vertices form a convex hull. + * @param hullVertices the hull vertices + * @return {@code true} if the vertices form a convex hull, {@code false} otherwise + */ + private boolean isConvex(final Vector2D[] hullVertices) { + if (hullVertices.length < 3) { + return true; + } + + int sign = 0; + for (int i = 0; i < hullVertices.length; i++) { + final Vector2D p1 = hullVertices[i == 0 ? hullVertices.length - 1 : i - 1]; + final Vector2D p2 = hullVertices[i]; + final Vector2D p3 = hullVertices[i == hullVertices.length - 1 ? 0 : i + 1]; + + final Vector2D d1 = p2.subtract(p1); + final Vector2D d2 = p3.subtract(p2); + + final double crossProduct = MathArrays.linearCombination(d1.getX(), d2.getY(), -d1.getY(), d2.getX()); + final int cmp = Precision.compareTo(crossProduct, 0.0, tolerance); + // in case of collinear points the cross product will be zero + if (cmp != 0.0) { + if (sign != 0.0 && cmp != sign) { + return false; + } + sign = cmp; + } + } + + return true; + } + + /** {@inheritDoc} */ + public Vector2D[] getVertices() { + return vertices.clone(); + } + + /** + * Get the line segments of the convex hull, ordered. + * @return the line segments of the convex hull + */ + public Segment[] getLineSegments() { + return retrieveLineSegments().clone(); + } + + /** + * Retrieve the line segments from the cached array or create them if needed. + * + * @return the array of line segments + */ + private Segment[] retrieveLineSegments() { + if (lineSegments == null) { + // construct the line segments - handle special cases of 1 or 2 points + final int size = vertices.length; + if (size <= 1) { + this.lineSegments = new Segment[0]; + } else if (size == 2) { + this.lineSegments = new Segment[1]; + final Vector2D p1 = vertices[0]; + final Vector2D p2 = vertices[1]; + this.lineSegments[0] = new Segment(p1, p2, new Line(p1, p2, tolerance)); + } else { + this.lineSegments = new Segment[size]; + Vector2D firstPoint = null; + Vector2D lastPoint = null; + int index = 0; + for (Vector2D point : vertices) { + if (lastPoint == null) { + firstPoint = point; + lastPoint = point; + } else { + this.lineSegments[index++] = + new Segment(lastPoint, point, new Line(lastPoint, point, tolerance)); + lastPoint = point; + } + } + this.lineSegments[index] = + new Segment(lastPoint, firstPoint, new Line(lastPoint, firstPoint, tolerance)); + } + } + return lineSegments; + } + + /** {@inheritDoc} */ + public Region<Euclidean2D> createRegion() throws InsufficientDataException { + if (vertices.length < 3) { + throw new InsufficientDataException(); + } + final RegionFactory<Euclidean2D> factory = new RegionFactory<Euclidean2D>(); + final Segment[] segments = retrieveLineSegments(); + final Line[] lineArray = new Line[segments.length]; + for (int i = 0; i < segments.length; i++) { + lineArray[i] = segments[i].getLine(); + } + return factory.buildConvex(lineArray); + } +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHullGenerator2D.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHullGenerator2D.java new file mode 100644 index 0000000..3e13e1a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/ConvexHullGenerator2D.java @@ -0,0 +1,37 @@ +/* + * 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.twod.hull; + +import java.util.Collection; + +import org.apache.commons.math3.exception.ConvergenceException; +import org.apache.commons.math3.exception.NullArgumentException; +import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.geometry.hull.ConvexHullGenerator; + +/** + * Interface for convex hull generators in the two-dimensional euclidean space. + * + * @since 3.3 + */ +public interface ConvexHullGenerator2D extends ConvexHullGenerator<Euclidean2D, Vector2D> { + + /** {@inheritDoc} */ + ConvexHull2D generate(Collection<Vector2D> points) throws NullArgumentException, ConvergenceException; + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/MonotoneChain.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/MonotoneChain.java new file mode 100644 index 0000000..4421344 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/MonotoneChain.java @@ -0,0 +1,181 @@ +/* + * 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.twod.hull; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; + +import org.apache.commons.math3.geometry.euclidean.twod.Line; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.Precision; + +/** + * Implements Andrew's monotone chain method to generate the convex hull of a finite set of + * points in the two-dimensional euclidean space. + * <p> + * The runtime complexity is O(n log n), with n being the number of input points. If the + * point set is already sorted (by x-coordinate), the runtime complexity is O(n). + * <p> + * The implementation is not sensitive to collinear points on the hull. The parameter + * {@code includeCollinearPoints} allows to control the behavior with regard to collinear points. + * If {@code true}, all points on the boundary of the hull will be added to the hull vertices, + * otherwise only the extreme points will be present. By default, collinear points are not added + * as hull vertices. + * <p> + * The {@code tolerance} parameter (default: 1e-10) is used as epsilon criteria to determine + * identical and collinear points. + * + * @see <a href="http://en.wikibooks.org/wiki/Algorithm_Implementation/Geometry/Convex_hull/Monotone_chain"> + * Andrew's monotone chain algorithm (Wikibooks)</a> + * @since 3.3 + */ +public class MonotoneChain extends AbstractConvexHullGenerator2D { + + /** + * Create a new MonotoneChain instance. + */ + public MonotoneChain() { + this(false); + } + + /** + * Create a new MonotoneChain instance. + * @param includeCollinearPoints whether collinear points shall be added as hull vertices + */ + public MonotoneChain(final boolean includeCollinearPoints) { + super(includeCollinearPoints); + } + + /** + * Create a new MonotoneChain instance. + * @param includeCollinearPoints whether collinear points shall be added as hull vertices + * @param tolerance tolerance below which points are considered identical + */ + public MonotoneChain(final boolean includeCollinearPoints, final double tolerance) { + super(includeCollinearPoints, tolerance); + } + + /** {@inheritDoc} */ + @Override + public Collection<Vector2D> findHullVertices(final Collection<Vector2D> points) { + + final List<Vector2D> pointsSortedByXAxis = new ArrayList<Vector2D>(points); + + // sort the points in increasing order on the x-axis + Collections.sort(pointsSortedByXAxis, new Comparator<Vector2D>() { + /** {@inheritDoc} */ + public int compare(final Vector2D o1, final Vector2D o2) { + final double tolerance = getTolerance(); + // need to take the tolerance value into account, otherwise collinear points + // will not be handled correctly when building the upper/lower hull + final int diff = Precision.compareTo(o1.getX(), o2.getX(), tolerance); + if (diff == 0) { + return Precision.compareTo(o1.getY(), o2.getY(), tolerance); + } else { + return diff; + } + } + }); + + // build lower hull + final List<Vector2D> lowerHull = new ArrayList<Vector2D>(); + for (Vector2D p : pointsSortedByXAxis) { + updateHull(p, lowerHull); + } + + // build upper hull + final List<Vector2D> upperHull = new ArrayList<Vector2D>(); + for (int idx = pointsSortedByXAxis.size() - 1; idx >= 0; idx--) { + final Vector2D p = pointsSortedByXAxis.get(idx); + updateHull(p, upperHull); + } + + // concatenate the lower and upper hulls + // the last point of each list is omitted as it is repeated at the beginning of the other list + final List<Vector2D> hullVertices = new ArrayList<Vector2D>(lowerHull.size() + upperHull.size() - 2); + for (int idx = 0; idx < lowerHull.size() - 1; idx++) { + hullVertices.add(lowerHull.get(idx)); + } + for (int idx = 0; idx < upperHull.size() - 1; idx++) { + hullVertices.add(upperHull.get(idx)); + } + + // special case: if the lower and upper hull may contain only 1 point if all are identical + if (hullVertices.isEmpty() && ! lowerHull.isEmpty()) { + hullVertices.add(lowerHull.get(0)); + } + + return hullVertices; + } + + /** + * Update the partial hull with the current point. + * + * @param point the current point + * @param hull the partial hull + */ + private void updateHull(final Vector2D point, final List<Vector2D> hull) { + final double tolerance = getTolerance(); + + if (hull.size() == 1) { + // ensure that we do not add an identical point + final Vector2D p1 = hull.get(0); + if (p1.distance(point) < tolerance) { + return; + } + } + + while (hull.size() >= 2) { + final int size = hull.size(); + final Vector2D p1 = hull.get(size - 2); + final Vector2D p2 = hull.get(size - 1); + + final double offset = new Line(p1, p2, tolerance).getOffset(point); + if (FastMath.abs(offset) < tolerance) { + // the point is collinear to the line (p1, p2) + + final double distanceToCurrent = p1.distance(point); + if (distanceToCurrent < tolerance || p2.distance(point) < tolerance) { + // the point is assumed to be identical to either p1 or p2 + return; + } + + final double distanceToLast = p1.distance(p2); + if (isIncludeCollinearPoints()) { + final int index = distanceToCurrent < distanceToLast ? size - 1 : size; + hull.add(index, point); + } else { + if (distanceToCurrent > distanceToLast) { + hull.remove(size - 1); + hull.add(point); + } + } + return; + } else if (offset > 0) { + hull.remove(size - 1); + } else { + break; + } + } + hull.add(point); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/package-info.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/package-info.java new file mode 100644 index 0000000..d0469a4 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/hull/package-info.java @@ -0,0 +1,25 @@ +/* + * 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 algorithms to generate the convex hull + * for a set of points in an two-dimensional euclidean space. + * </p> + * + */ +package org.apache.commons.math3.geometry.euclidean.twod.hull; diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/package-info.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/package-info.java new file mode 100644 index 0000000..feb43b1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/twod/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 2D geometry components. + * </p> + * + */ +package org.apache.commons.math3.geometry.euclidean.twod; |