diff options
Diffstat (limited to 'src/main/java/org/apache/commons/math3/geometry')
98 files changed, 23146 insertions, 0 deletions
diff --git a/src/main/java/org/apache/commons/math3/geometry/Point.java b/src/main/java/org/apache/commons/math3/geometry/Point.java new file mode 100644 index 0000000..49f290a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/Point.java @@ -0,0 +1,52 @@ +/* + * 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; + +import java.io.Serializable; + +/** + * This interface represents a generic geometrical point. + * + * @param <S> Type of the space. + * @see Space + * @see Vector + * @since 3.3 + */ +public interface Point<S extends Space> extends Serializable { + + /** + * Get the space to which the point belongs. + * + * @return containing space + */ + Space getSpace(); + + /** + * Returns true if any coordinate of this point is NaN; false otherwise + * + * @return true if any coordinate of this point is NaN; false otherwise + */ + boolean isNaN(); + + /** + * Compute the distance between the instance and another point. + * + * @param p second point + * @return the distance between the instance and p + */ + double distance(Point<S> p); +} diff --git a/src/main/java/org/apache/commons/math3/geometry/Space.java b/src/main/java/org/apache/commons/math3/geometry/Space.java new file mode 100644 index 0000000..7b563c6 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/Space.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; + +import org.apache.commons.math3.exception.MathUnsupportedOperationException; + +import java.io.Serializable; + +/** + * This interface represents a generic space, with affine and vectorial counterparts. + * + * @see Vector + * @since 3.0 + */ +public interface Space extends Serializable { + + /** + * Get the dimension of the space. + * + * @return dimension of the space + */ + int getDimension(); + + /** + * Get the n-1 dimension subspace of this space. + * + * @return n-1 dimension sub-space of this space + * @see #getDimension() + * @exception MathUnsupportedOperationException for dimension-1 spaces which do not have + * sub-spaces + */ + Space getSubSpace() throws MathUnsupportedOperationException; +} diff --git a/src/main/java/org/apache/commons/math3/geometry/Vector.java b/src/main/java/org/apache/commons/math3/geometry/Vector.java new file mode 100644 index 0000000..92ad04a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/Vector.java @@ -0,0 +1,194 @@ +/* + * 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; + +import org.apache.commons.math3.exception.MathArithmeticException; + +import java.text.NumberFormat; + +/** + * This interface represents a generic vector in a vectorial space or a point in an affine space. + * + * @param <S> Type of the space. + * @see Space + * @see Point + * @since 3.0 + */ +public interface Vector<S extends Space> extends Point<S> { + + /** + * Get the null vector of the vectorial space or origin point of the affine space. + * + * @return null vector of the vectorial space or origin point of the affine space + */ + Vector<S> getZero(); + + /** + * Get the L<sub>1</sub> norm for the vector. + * + * @return L<sub>1</sub> norm for the vector + */ + double getNorm1(); + + /** + * Get the L<sub>2</sub> norm for the vector. + * + * @return Euclidean norm for the vector + */ + double getNorm(); + + /** + * Get the square of the norm for the vector. + * + * @return square of the Euclidean norm for the vector + */ + double getNormSq(); + + /** + * Get the L<sub>∞</sub> norm for the vector. + * + * @return L<sub>∞</sub> norm for the vector + */ + double getNormInf(); + + /** + * Add a vector to the instance. + * + * @param v vector to add + * @return a new vector + */ + Vector<S> add(Vector<S> 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 + */ + Vector<S> add(double factor, Vector<S> v); + + /** + * Subtract a vector from the instance. + * + * @param v vector to subtract + * @return a new vector + */ + Vector<S> subtract(Vector<S> 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 + */ + Vector<S> subtract(double factor, Vector<S> v); + + /** + * Get the opposite of the instance. + * + * @return a new vector which is opposite to the instance + */ + Vector<S> negate(); + + /** + * Get a normalized vector aligned with the instance. + * + * @return a new normalized vector + * @exception MathArithmeticException if the norm is zero + */ + Vector<S> normalize() throws MathArithmeticException; + + /** + * Multiply the instance by a scalar. + * + * @param a scalar + * @return a new vector + */ + Vector<S> scalarMultiply(double a); + + /** + * 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 + */ + boolean isInfinite(); + + /** + * 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 + * + * @param v second vector + * @return the distance between the instance and p according to the L<sub>1</sub> norm + */ + double distance1(Vector<S> v); + + /** + * 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 + * + * @param v second vector + * @return the distance between the instance and p according to the L<sub>2</sub> norm + */ + double distance(Vector<S> v); + + /** + * 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 + * + * @param v second vector + * @return the distance between the instance and p according to the L<sub>∞</sub> norm + */ + double distanceInf(Vector<S> v); + + /** + * 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 + * + * @param v second vector + * @return the square of the distance between the instance and p + */ + double distanceSq(Vector<S> v); + + /** + * Compute the dot-product of the instance and another vector. + * + * @param v second vector + * @return the dot product this.v + */ + double dotProduct(Vector<S> v); + + /** + * Get a string representation of this vector. + * + * @param format the custom format for components + * @return a string representation of this vector + */ + String toString(final NumberFormat format); +} diff --git a/src/main/java/org/apache/commons/math3/geometry/VectorFormat.java b/src/main/java/org/apache/commons/math3/geometry/VectorFormat.java new file mode 100644 index 0000000..7c6d0c5 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/VectorFormat.java @@ -0,0 +1,307 @@ +/* + * 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; + +import org.apache.commons.math3.exception.MathParseException; +import org.apache.commons.math3.util.CompositeFormat; + +import java.text.FieldPosition; +import java.text.NumberFormat; +import java.text.ParsePosition; +import java.util.Locale; + +/** + * Formats a 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>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><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. + * + * @param <S> Type of the space. + * @since 3.0 + */ +public abstract class VectorFormat<S extends Space> { + + /** The default prefix: "{". */ + public static final String DEFAULT_PREFIX = "{"; + + /** The default suffix: "}". */ + public static final String DEFAULT_SUFFIX = "}"; + + /** The default separator: ", ". */ + public static final String DEFAULT_SEPARATOR = "; "; + + /** Prefix. */ + private final String prefix; + + /** Suffix. */ + private final String suffix; + + /** Separator. */ + private final String separator; + + /** Trimmed prefix. */ + private final String trimmedPrefix; + + /** Trimmed suffix. */ + private final String trimmedSuffix; + + /** Trimmed separator. */ + private final String trimmedSeparator; + + /** The format used for components. */ + private final NumberFormat format; + + /** + * Create an instance with default settings. + * + * <p>The instance uses the default prefix, suffix and separator: "{", "}", and "; " and the + * default number format for components. + */ + protected VectorFormat() { + this( + 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. + */ + protected VectorFormat(final NumberFormat format) { + this(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 "; " + */ + protected VectorFormat(final String prefix, final String suffix, final String separator) { + this(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. + */ + protected VectorFormat( + final String prefix, + final String suffix, + final String separator, + final NumberFormat format) { + this.prefix = prefix; + this.suffix = suffix; + this.separator = separator; + trimmedPrefix = prefix.trim(); + trimmedSuffix = suffix.trim(); + trimmedSeparator = separator.trim(); + this.format = format; + } + + /** + * Get the set of locales for which point/vector formats are available. + * + * <p>This is the same set as the {@link NumberFormat} set. + * + * @return available point/vector format locales. + */ + public static Locale[] getAvailableLocales() { + return NumberFormat.getAvailableLocales(); + } + + /** + * Get the format prefix. + * + * @return format prefix. + */ + public String getPrefix() { + return prefix; + } + + /** + * Get the format suffix. + * + * @return format suffix. + */ + public String getSuffix() { + return suffix; + } + + /** + * Get the format separator between components. + * + * @return format separator. + */ + public String getSeparator() { + return separator; + } + + /** + * Get the components format. + * + * @return components format. + */ + public NumberFormat getFormat() { + return format; + } + + /** + * Formats a {@link Vector} object to produce a string. + * + * @param vector the object to format. + * @return a formatted string. + */ + public String format(Vector<S> vector) { + return format(vector, new StringBuffer(), new FieldPosition(0)).toString(); + } + + /** + * Formats a {@link Vector} 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. + */ + public abstract StringBuffer format( + Vector<S> vector, StringBuffer toAppendTo, FieldPosition pos); + + /** + * Formats the coordinates of a {@link Vector} to produce a string. + * + * @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 + * @param coordinates coordinates of the object to format. + * @return the value passed in as toAppendTo. + */ + protected StringBuffer format( + StringBuffer toAppendTo, FieldPosition pos, double... coordinates) { + + pos.setBeginIndex(0); + pos.setEndIndex(0); + + // format prefix + toAppendTo.append(prefix); + + // format components + for (int i = 0; i < coordinates.length; ++i) { + if (i > 0) { + toAppendTo.append(separator); + } + CompositeFormat.formatDouble(coordinates[i], format, toAppendTo, pos); + } + + // format suffix + toAppendTo.append(suffix); + + return toAppendTo; + } + + /** + * Parses a string to produce a {@link Vector} object. + * + * @param source the string to parse + * @return the parsed {@link Vector} object. + * @throws MathParseException if the beginning of the specified string cannot be parsed. + */ + public abstract Vector<S> parse(String source) throws MathParseException; + + /** + * Parses a string to produce a {@link Vector} object. + * + * @param source the string to parse + * @param pos input/output parsing parameter. + * @return the parsed {@link Vector} object. + */ + public abstract Vector<S> parse(String source, ParsePosition pos); + + /** + * Parses a string to produce an array of coordinates. + * + * @param dimension dimension of the space + * @param source the string to parse + * @param pos input/output parsing parameter. + * @return coordinates array. + */ + protected double[] parseCoordinates(int dimension, String source, ParsePosition pos) { + + int initialIndex = pos.getIndex(); + double[] coordinates = new double[dimension]; + + // parse prefix + CompositeFormat.parseAndIgnoreWhitespace(source, pos); + if (!CompositeFormat.parseFixedstring(source, trimmedPrefix, pos)) { + return null; + } + + for (int i = 0; i < dimension; ++i) { + + // skip whitespace + CompositeFormat.parseAndIgnoreWhitespace(source, pos); + + // parse separator + if (i > 0 && !CompositeFormat.parseFixedstring(source, trimmedSeparator, pos)) { + return null; + } + + // skip whitespace + CompositeFormat.parseAndIgnoreWhitespace(source, pos); + + // parse coordinate + Number c = CompositeFormat.parseNumber(source, format, pos); + if (c == null) { + // invalid coordinate + // set index back to initial, error index should already be set + pos.setIndex(initialIndex); + return null; + } + + // store coordinate + coordinates[i] = c.doubleValue(); + } + + // parse suffix + CompositeFormat.parseAndIgnoreWhitespace(source, pos); + if (!CompositeFormat.parseFixedstring(source, trimmedSuffix, pos)) { + return null; + } + + return coordinates; + } +} diff --git a/src/main/java/org/apache/commons/math3/geometry/enclosing/Encloser.java b/src/main/java/org/apache/commons/math3/geometry/enclosing/Encloser.java new file mode 100644 index 0000000..9b2588a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/enclosing/Encloser.java @@ -0,0 +1,36 @@ +/* + * 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.enclosing; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** Interface for algorithms computing enclosing balls. + * @param <S> Space type. + * @param <P> Point type. + * @see EnclosingBall + * @since 3.3 + */ +public interface Encloser<S extends Space, P extends Point<S>> { + + /** Find a ball enclosing a list of points. + * @param points points to enclose + * @return enclosing ball + */ + EnclosingBall<S, P> enclose(Iterable<P> points); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/enclosing/EnclosingBall.java b/src/main/java/org/apache/commons/math3/geometry/enclosing/EnclosingBall.java new file mode 100644 index 0000000..eedbd46 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/enclosing/EnclosingBall.java @@ -0,0 +1,103 @@ +/* + * 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.enclosing; + +import java.io.Serializable; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** This class represents a ball enclosing some points. + * @param <S> Space type. + * @param <P> Point type. + * @see Space + * @see Point + * @see Encloser + * @since 3.3 + */ +public class EnclosingBall<S extends Space, P extends Point<S>> implements Serializable { + + /** Serializable UID. */ + private static final long serialVersionUID = 20140126L; + + /** Center of the ball. */ + private final P center; + + /** Radius of the ball. */ + private final double radius; + + /** Support points used to define the ball. */ + private final P[] support; + + /** Simple constructor. + * @param center center of the ball + * @param radius radius of the ball + * @param support support points used to define the ball + */ + public EnclosingBall(final P center, final double radius, final P ... support) { + this.center = center; + this.radius = radius; + this.support = support.clone(); + } + + /** Get the center of the ball. + * @return center of the ball + */ + public P getCenter() { + return center; + } + + /** Get the radius of the ball. + * @return radius of the ball (can be negative if the ball is empty) + */ + public double getRadius() { + return radius; + } + + /** Get the support points used to define the ball. + * @return support points used to define the ball + */ + public P[] getSupport() { + return support.clone(); + } + + /** Get the number of support points used to define the ball. + * @return number of support points used to define the ball + */ + public int getSupportSize() { + return support.length; + } + + /** Check if a point is within the ball or at boundary. + * @param point point to test + * @return true if the point is within the ball or at boundary + */ + public boolean contains(final P point) { + return point.distance(center) <= radius; + } + + /** Check if a point is within an enlarged ball or at boundary. + * @param point point to test + * @param margin margin to consider + * @return true if the point is within the ball enlarged + * by the margin or at boundary + */ + public boolean contains(final P point, final double margin) { + return point.distance(center) <= radius + margin; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/enclosing/SupportBallGenerator.java b/src/main/java/org/apache/commons/math3/geometry/enclosing/SupportBallGenerator.java new file mode 100644 index 0000000..3a0f875 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/enclosing/SupportBallGenerator.java @@ -0,0 +1,42 @@ +/* + * 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.enclosing; + +import java.util.List; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** Interface for generating balls based on support points. + * <p> + * This generator is used in the {@link WelzlEncloser Emo Welzl} algorithm + * and its derivatives. + * </p> + * @param <S> Space type. + * @param <P> Point type. + * @see EnclosingBall + * @since 3.3 + */ +public interface SupportBallGenerator<S extends Space, P extends Point<S>> { + + /** Create a ball whose boundary lies on prescribed support points. + * @param support support points (may be empty) + * @return ball whose boundary lies on the prescribed support points + */ + EnclosingBall<S, P> ballOnSupport(List<P> support); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/enclosing/WelzlEncloser.java b/src/main/java/org/apache/commons/math3/geometry/enclosing/WelzlEncloser.java new file mode 100644 index 0000000..987e7d9 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/enclosing/WelzlEncloser.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.enclosing; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.exception.MathInternalError; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** Class implementing Emo Welzl algorithm to find the smallest enclosing ball in linear time. + * <p> + * The class implements the algorithm described in paper <a + * href="http://www.inf.ethz.ch/personal/emo/PublFiles/SmallEnclDisk_LNCS555_91.pdf">Smallest + * Enclosing Disks (Balls and Ellipsoids)</a> by Emo Welzl, Lecture Notes in Computer Science + * 555 (1991) 359-370. The pivoting improvement published in the paper <a + * href="http://www.inf.ethz.ch/personal/gaertner/texts/own_work/esa99_final.pdf">Fast and + * Robust Smallest Enclosing Balls</a>, by Bernd Gärtner and further modified in + * paper <a + * href=http://www.idt.mdh.se/kurser/ct3340/ht12/MINICONFERENCE/FinalPapers/ircse12_submission_30.pdf"> + * Efficient Computation of Smallest Enclosing Balls in Three Dimensions</a> by Linus Källberg + * to avoid performing local copies of data have been included. + * </p> + * @param <S> Space type. + * @param <P> Point type. + * @since 3.3 + */ +public class WelzlEncloser<S extends Space, P extends Point<S>> implements Encloser<S, P> { + + /** Tolerance below which points are consider to be identical. */ + private final double tolerance; + + /** Generator for balls on support. */ + private final SupportBallGenerator<S, P> generator; + + /** Simple constructor. + * @param tolerance below which points are consider to be identical + * @param generator generator for balls on support + */ + public WelzlEncloser(final double tolerance, final SupportBallGenerator<S, P> generator) { + this.tolerance = tolerance; + this.generator = generator; + } + + /** {@inheritDoc} */ + public EnclosingBall<S, P> enclose(final Iterable<P> points) { + + if (points == null || !points.iterator().hasNext()) { + // return an empty ball + return generator.ballOnSupport(new ArrayList<P>()); + } + + // Emo Welzl algorithm with Bernd Gärtner and Linus Källberg improvements + return pivotingBall(points); + + } + + /** Compute enclosing ball using Gärtner's pivoting heuristic. + * @param points points to be enclosed + * @return enclosing ball + */ + private EnclosingBall<S, P> pivotingBall(final Iterable<P> points) { + + final P first = points.iterator().next(); + final List<P> extreme = new ArrayList<P>(first.getSpace().getDimension() + 1); + final List<P> support = new ArrayList<P>(first.getSpace().getDimension() + 1); + + // start with only first point selected as a candidate support + extreme.add(first); + EnclosingBall<S, P> ball = moveToFrontBall(extreme, extreme.size(), support); + + while (true) { + + // select the point farthest to current ball + final P farthest = selectFarthest(points, ball); + + if (ball.contains(farthest, tolerance)) { + // we have found a ball containing all points + return ball; + } + + // recurse search, restricted to the small subset containing support and farthest point + support.clear(); + support.add(farthest); + EnclosingBall<S, P> savedBall = ball; + ball = moveToFrontBall(extreme, extreme.size(), support); + if (ball.getRadius() < savedBall.getRadius()) { + // this should never happen + throw new MathInternalError(); + } + + // it was an interesting point, move it to the front + // according to Gärtner's heuristic + extreme.add(0, farthest); + + // prune the least interesting points + extreme.subList(ball.getSupportSize(), extreme.size()).clear(); + + + } + } + + /** Compute enclosing ball using Welzl's move to front heuristic. + * @param extreme subset of extreme points + * @param nbExtreme number of extreme points to consider + * @param support points that must belong to the ball support + * @return enclosing ball, for the extreme subset only + */ + private EnclosingBall<S, P> moveToFrontBall(final List<P> extreme, final int nbExtreme, + final List<P> support) { + + // create a new ball on the prescribed support + EnclosingBall<S, P> ball = generator.ballOnSupport(support); + + if (ball.getSupportSize() <= ball.getCenter().getSpace().getDimension()) { + + for (int i = 0; i < nbExtreme; ++i) { + final P pi = extreme.get(i); + if (!ball.contains(pi, tolerance)) { + + // we have found an outside point, + // enlarge the ball by adding it to the support + support.add(pi); + ball = moveToFrontBall(extreme, i, support); + support.remove(support.size() - 1); + + // it was an interesting point, move it to the front + // according to Welzl's heuristic + for (int j = i; j > 0; --j) { + extreme.set(j, extreme.get(j - 1)); + } + extreme.set(0, pi); + + } + } + + } + + return ball; + + } + + /** Select the point farthest to the current ball. + * @param points points to be enclosed + * @param ball current ball + * @return farthest point + */ + public P selectFarthest(final Iterable<P> points, final EnclosingBall<S, P> ball) { + + final P center = ball.getCenter(); + P farthest = null; + double dMax = -1.0; + + for (final P point : points) { + final double d = point.distance(center); + if (d > dMax) { + farthest = point; + dMax = d; + } + } + + return farthest; + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/enclosing/package-info.java b/src/main/java/org/apache/commons/math3/geometry/enclosing/package-info.java new file mode 100644 index 0000000..20462a1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/enclosing/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 interfaces and classes related to the smallest enclosing ball problem. + * </p> + * + */ +package org.apache.commons.math3.geometry.enclosing; 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; diff --git a/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHull.java b/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHull.java new file mode 100644 index 0000000..8dfa3f3 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHull.java @@ -0,0 +1,48 @@ +/* + * 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.hull; + +import java.io.Serializable; + +import org.apache.commons.math3.exception.InsufficientDataException; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.partitioning.Region; + +/** + * This class represents a convex hull. + * + * @param <S> Space type. + * @param <P> Point type. + * @since 3.3 + */ +public interface ConvexHull<S extends Space, P extends Point<S>> extends Serializable { + + /** + * Get the vertices of the convex hull. + * @return vertices of the convex hull + */ + P[] getVertices(); + + /** + * Returns a new region that is enclosed by the convex hull. + * @return the region enclosed by the convex hull + * @throws InsufficientDataException if the number of vertices is not enough to + * build a region in the respective space + */ + Region<S> createRegion() throws InsufficientDataException; +} diff --git a/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHullGenerator.java b/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHullGenerator.java new file mode 100644 index 0000000..8f601d2 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/hull/ConvexHullGenerator.java @@ -0,0 +1,49 @@ +/* + * 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.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.Point; +import org.apache.commons.math3.geometry.Space; + +/** + * Interface for convex hull generators. + * + * @param <S> Type of the {@link Space} + * @param <P> Type of the {@link Point} + * + * @see <a href="http://en.wikipedia.org/wiki/Convex_hull">Convex Hull (Wikipedia)</a> + * @see <a href="http://mathworld.wolfram.com/ConvexHull.html">Convex Hull (MathWorld)</a> + * + * @since 3.3 + */ +public interface ConvexHullGenerator<S extends Space, P extends Point<S>> { + + /** + * Builds the convex hull from the set of input points. + * + * @param points the set of input points + * @return the convex hull + * @throws NullArgumentException if the input collection is {@code null} + * @throws ConvergenceException if generator fails to generate a convex hull for + * the given set of input points + */ + ConvexHull<S, P> generate(Collection<P> points) throws NullArgumentException, ConvergenceException; +} diff --git a/src/main/java/org/apache/commons/math3/geometry/hull/package-info.java b/src/main/java/org/apache/commons/math3/geometry/hull/package-info.java new file mode 100644 index 0000000..2246682 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/hull/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 interfaces and classes related to the convex hull problem. + * </p> + * + */ +package org.apache.commons.math3.geometry.hull; diff --git a/src/main/java/org/apache/commons/math3/geometry/package-info.java b/src/main/java/org/apache/commons/math3/geometry/package-info.java new file mode 100644 index 0000000..6c39cff --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/package-info.java @@ -0,0 +1,21 @@ +/* + * 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. + */ +/** + * This package is the top level package for geometry. It provides only a few interfaces related to + * vectorial/affine spaces that are implemented in sub-packages. + */ +package org.apache.commons.math3.geometry; diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractRegion.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractRegion.java new file mode 100644 index 0000000..d901ab4 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractRegion.java @@ -0,0 +1,540 @@ +/* + * 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.partitioning; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Comparator; +import java.util.HashMap; +import java.util.Iterator; +import java.util.Map; +import java.util.TreeSet; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.Vector; + +/** Abstract class for all regions, independently of geometry type or dimension. + + * @param <S> Type of the space. + * @param <T> Type of the sub-space. + + * @since 3.0 + */ +public abstract class AbstractRegion<S extends Space, T extends Space> implements Region<S> { + + /** Inside/Outside BSP tree. */ + private BSPTree<S> tree; + + /** Tolerance below which points are considered to belong to hyperplanes. */ + private final double tolerance; + + /** Size of the instance. */ + private double size; + + /** Barycenter. */ + private Point<S> barycenter; + + /** Build a region representing the whole space. + * @param tolerance tolerance below which points are considered identical. + */ + protected AbstractRegion(final double tolerance) { + this.tree = new BSPTree<S>(Boolean.TRUE); + this.tolerance = tolerance; + } + + /** Build a region 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}. The + * tree also <em>must</em> have either null internal nodes or + * internal nodes representing the boundary as specified in the + * {@link #getTree getTree} method).</p> + * @param tree inside/outside BSP tree representing the region + * @param tolerance tolerance below which points are considered identical. + */ + protected AbstractRegion(final BSPTree<S> tree, final double tolerance) { + this.tree = tree; + this.tolerance = tolerance; + } + + /** Build a Region 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 #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. + */ + protected AbstractRegion(final Collection<SubHyperplane<S>> boundary, final double tolerance) { + + this.tolerance = tolerance; + + if (boundary.size() == 0) { + + // the tree represents the whole space + tree = new BSPTree<S>(Boolean.TRUE); + + } else { + + // sort the boundary elements in decreasing size order + // (we don't want equal size elements to be removed, so + // we use a trick to fool the TreeSet) + final TreeSet<SubHyperplane<S>> ordered = new TreeSet<SubHyperplane<S>>(new Comparator<SubHyperplane<S>>() { + /** {@inheritDoc} */ + public int compare(final SubHyperplane<S> o1, final SubHyperplane<S> o2) { + final double size1 = o1.getSize(); + final double size2 = o2.getSize(); + return (size2 < size1) ? -1 : ((o1 == o2) ? 0 : +1); + } + }); + ordered.addAll(boundary); + + // build the tree top-down + tree = new BSPTree<S>(); + insertCuts(tree, ordered); + + // set up the inside/outside flags + tree.visit(new BSPTreeVisitor<S>() { + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<S> node) { + return Order.PLUS_SUB_MINUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<S> node) { + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<S> node) { + if (node.getParent() == null || node == node.getParent().getMinus()) { + node.setAttribute(Boolean.TRUE); + } else { + node.setAttribute(Boolean.FALSE); + } + } + }); + + } + + } + + /** Build a convex region from an array of bounding hyperplanes. + * @param hyperplanes array of bounding hyperplanes (if null, an + * empty region will be built) + * @param tolerance tolerance below which points are considered identical. + */ + public AbstractRegion(final Hyperplane<S>[] hyperplanes, final double tolerance) { + this.tolerance = tolerance; + if ((hyperplanes == null) || (hyperplanes.length == 0)) { + tree = new BSPTree<S>(Boolean.FALSE); + } else { + + // use the first hyperplane to build the right class + tree = hyperplanes[0].wholeSpace().getTree(false); + + // chop off parts of the space + BSPTree<S> node = tree; + node.setAttribute(Boolean.TRUE); + for (final Hyperplane<S> hyperplane : hyperplanes) { + if (node.insertCut(hyperplane)) { + node.setAttribute(null); + node.getPlus().setAttribute(Boolean.FALSE); + node = node.getMinus(); + node.setAttribute(Boolean.TRUE); + } + } + + } + + } + + /** {@inheritDoc} */ + public abstract AbstractRegion<S, T> buildNew(BSPTree<S> newTree); + + /** Get the tolerance below which points are considered to belong to hyperplanes. + * @return tolerance below which points are considered to belong to hyperplanes + */ + public double getTolerance() { + return tolerance; + } + + /** Recursively build a tree by inserting cut sub-hyperplanes. + * @param node current tree node (it is a leaf node at the beginning + * of the call) + * @param boundary collection of edges belonging to the cell defined + * by the node + */ + private void insertCuts(final BSPTree<S> node, final Collection<SubHyperplane<S>> boundary) { + + final Iterator<SubHyperplane<S>> iterator = boundary.iterator(); + + // build the current level + Hyperplane<S> inserted = null; + while ((inserted == null) && iterator.hasNext()) { + inserted = iterator.next().getHyperplane(); + if (!node.insertCut(inserted.copySelf())) { + inserted = null; + } + } + + if (!iterator.hasNext()) { + return; + } + + // distribute the remaining edges in the two sub-trees + final ArrayList<SubHyperplane<S>> plusList = new ArrayList<SubHyperplane<S>>(); + final ArrayList<SubHyperplane<S>> minusList = new ArrayList<SubHyperplane<S>>(); + while (iterator.hasNext()) { + final SubHyperplane<S> other = iterator.next(); + final SubHyperplane.SplitSubHyperplane<S> split = other.split(inserted); + switch (split.getSide()) { + case PLUS: + plusList.add(other); + break; + case MINUS: + minusList.add(other); + break; + case BOTH: + plusList.add(split.getPlus()); + minusList.add(split.getMinus()); + break; + default: + // ignore the sub-hyperplanes belonging to the cut hyperplane + } + } + + // recurse through lower levels + insertCuts(node.getPlus(), plusList); + insertCuts(node.getMinus(), minusList); + + } + + /** {@inheritDoc} */ + public AbstractRegion<S, T> copySelf() { + return buildNew(tree.copySelf()); + } + + /** {@inheritDoc} */ + public boolean isEmpty() { + return isEmpty(tree); + } + + /** {@inheritDoc} */ + public boolean isEmpty(final BSPTree<S> node) { + + // we use a recursive function rather than the BSPTreeVisitor + // interface because we can stop visiting the tree as soon as we + // have found an inside cell + + if (node.getCut() == null) { + // if we find an inside node, the region is not empty + return !((Boolean) node.getAttribute()); + } + + // check both sides of the sub-tree + return isEmpty(node.getMinus()) && isEmpty(node.getPlus()); + + } + + /** {@inheritDoc} */ + public boolean isFull() { + return isFull(tree); + } + + /** {@inheritDoc} */ + public boolean isFull(final BSPTree<S> node) { + + // we use a recursive function rather than the BSPTreeVisitor + // interface because we can stop visiting the tree as soon as we + // have found an outside cell + + if (node.getCut() == null) { + // if we find an outside node, the region does not cover full space + return (Boolean) node.getAttribute(); + } + + // check both sides of the sub-tree + return isFull(node.getMinus()) && isFull(node.getPlus()); + + } + + /** {@inheritDoc} */ + public boolean contains(final Region<S> region) { + return new RegionFactory<S>().difference(region, this).isEmpty(); + } + + /** {@inheritDoc} + * @since 3.3 + */ + public BoundaryProjection<S> projectToBoundary(final Point<S> point) { + final BoundaryProjector<S, T> projector = new BoundaryProjector<S, T>(point); + getTree(true).visit(projector); + return projector.getProjection(); + } + + /** Check a point with respect to the region. + * @param point point to check + * @return a code representing the point status: either {@link + * Region.Location#INSIDE}, {@link Region.Location#OUTSIDE} or + * {@link Region.Location#BOUNDARY} + */ + public Location checkPoint(final Vector<S> point) { + return checkPoint((Point<S>) point); + } + + /** {@inheritDoc} */ + public Location checkPoint(final Point<S> point) { + return checkPoint(tree, point); + } + + /** Check a point with respect to the region starting at a given node. + * @param node root node of the region + * @param point point to check + * @return a code representing the point status: either {@link + * Region.Location#INSIDE INSIDE}, {@link Region.Location#OUTSIDE + * OUTSIDE} or {@link Region.Location#BOUNDARY BOUNDARY} + */ + protected Location checkPoint(final BSPTree<S> node, final Vector<S> point) { + return checkPoint(node, (Point<S>) point); + } + + /** Check a point with respect to the region starting at a given node. + * @param node root node of the region + * @param point point to check + * @return a code representing the point status: either {@link + * Region.Location#INSIDE INSIDE}, {@link Region.Location#OUTSIDE + * OUTSIDE} or {@link Region.Location#BOUNDARY BOUNDARY} + */ + protected Location checkPoint(final BSPTree<S> node, final Point<S> point) { + final BSPTree<S> cell = node.getCell(point, tolerance); + if (cell.getCut() == null) { + // the point is in the interior of a cell, just check the attribute + return ((Boolean) cell.getAttribute()) ? Location.INSIDE : Location.OUTSIDE; + } + + // the point is on a cut-sub-hyperplane, is it on a boundary ? + final Location minusCode = checkPoint(cell.getMinus(), point); + final Location plusCode = checkPoint(cell.getPlus(), point); + return (minusCode == plusCode) ? minusCode : Location.BOUNDARY; + + } + + /** {@inheritDoc} */ + public BSPTree<S> getTree(final boolean includeBoundaryAttributes) { + if (includeBoundaryAttributes && (tree.getCut() != null) && (tree.getAttribute() == null)) { + // compute the boundary attributes + tree.visit(new BoundaryBuilder<S>()); + } + return tree; + } + + /** {@inheritDoc} */ + public double getBoundarySize() { + final BoundarySizeVisitor<S> visitor = new BoundarySizeVisitor<S>(); + getTree(true).visit(visitor); + return visitor.getSize(); + } + + /** {@inheritDoc} */ + public double getSize() { + if (barycenter == null) { + computeGeometricalProperties(); + } + return size; + } + + /** Set the size of the instance. + * @param size size of the instance + */ + protected void setSize(final double size) { + this.size = size; + } + + /** {@inheritDoc} */ + public Point<S> getBarycenter() { + if (barycenter == null) { + computeGeometricalProperties(); + } + return barycenter; + } + + /** Set the barycenter of the instance. + * @param barycenter barycenter of the instance + */ + protected void setBarycenter(final Vector<S> barycenter) { + setBarycenter((Point<S>) barycenter); + } + + /** Set the barycenter of the instance. + * @param barycenter barycenter of the instance + */ + protected void setBarycenter(final Point<S> barycenter) { + this.barycenter = barycenter; + } + + /** Compute some geometrical properties. + * <p>The properties to compute are the barycenter and the size.</p> + */ + protected abstract void computeGeometricalProperties(); + + /** {@inheritDoc} */ + @Deprecated + public Side side(final Hyperplane<S> hyperplane) { + final InsideFinder<S> finder = new InsideFinder<S>(this); + finder.recurseSides(tree, hyperplane.wholeHyperplane()); + return finder.plusFound() ? + (finder.minusFound() ? Side.BOTH : Side.PLUS) : + (finder.minusFound() ? Side.MINUS : Side.HYPER); + } + + /** {@inheritDoc} */ + public SubHyperplane<S> intersection(final SubHyperplane<S> sub) { + return recurseIntersection(tree, sub); + } + + /** Recursively compute the parts of a sub-hyperplane that are + * contained in the region. + * @param node current BSP tree node + * @param sub sub-hyperplane traversing the region + * @return filtered sub-hyperplane + */ + private SubHyperplane<S> recurseIntersection(final BSPTree<S> node, final SubHyperplane<S> sub) { + + if (node.getCut() == null) { + return (Boolean) node.getAttribute() ? sub.copySelf() : null; + } + + final Hyperplane<S> hyperplane = node.getCut().getHyperplane(); + final SubHyperplane.SplitSubHyperplane<S> split = sub.split(hyperplane); + if (split.getPlus() != null) { + if (split.getMinus() != null) { + // both sides + final SubHyperplane<S> plus = recurseIntersection(node.getPlus(), split.getPlus()); + final SubHyperplane<S> minus = recurseIntersection(node.getMinus(), split.getMinus()); + if (plus == null) { + return minus; + } else if (minus == null) { + return plus; + } else { + return plus.reunite(minus); + } + } else { + // only on plus side + return recurseIntersection(node.getPlus(), sub); + } + } else if (split.getMinus() != null) { + // only on minus side + return recurseIntersection(node.getMinus(), sub); + } else { + // on hyperplane + return recurseIntersection(node.getPlus(), + recurseIntersection(node.getMinus(), sub)); + } + + } + + /** Transform a region. + * <p>Applying a transform to a region consist in applying the + * transform to all the hyperplanes of the underlying BSP tree and + * of the boundary (and also to the sub-hyperplanes embedded in + * these hyperplanes) and to the barycenter. The instance is not + * modified, a new instance is built.</p> + * @param transform transform to apply + * @return a new region, resulting from the application of the + * transform to the instance + */ + public AbstractRegion<S, T> applyTransform(final Transform<S, T> transform) { + + // transform the tree, except for boundary attribute splitters + final Map<BSPTree<S>, BSPTree<S>> map = new HashMap<BSPTree<S>, BSPTree<S>>(); + final BSPTree<S> transformedTree = recurseTransform(getTree(false), transform, map); + + // set up the boundary attributes splitters + for (final Map.Entry<BSPTree<S>, BSPTree<S>> entry : map.entrySet()) { + if (entry.getKey().getCut() != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<S> original = (BoundaryAttribute<S>) entry.getKey().getAttribute(); + if (original != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<S> transformed = (BoundaryAttribute<S>) entry.getValue().getAttribute(); + for (final BSPTree<S> splitter : original.getSplitters()) { + transformed.getSplitters().add(map.get(splitter)); + } + } + } + } + + return buildNew(transformedTree); + + } + + /** Recursively transform an inside/outside BSP-tree. + * @param node current BSP tree node + * @param transform transform to apply + * @param map transformed nodes map + * @return a new tree + */ + @SuppressWarnings("unchecked") + private BSPTree<S> recurseTransform(final BSPTree<S> node, final Transform<S, T> transform, + final Map<BSPTree<S>, BSPTree<S>> map) { + + final BSPTree<S> transformedNode; + if (node.getCut() == null) { + transformedNode = new BSPTree<S>(node.getAttribute()); + } else { + + final SubHyperplane<S> sub = node.getCut(); + final SubHyperplane<S> tSub = ((AbstractSubHyperplane<S, T>) sub).applyTransform(transform); + BoundaryAttribute<S> attribute = (BoundaryAttribute<S>) node.getAttribute(); + if (attribute != null) { + final SubHyperplane<S> tPO = (attribute.getPlusOutside() == null) ? + null : ((AbstractSubHyperplane<S, T>) attribute.getPlusOutside()).applyTransform(transform); + final SubHyperplane<S> tPI = (attribute.getPlusInside() == null) ? + null : ((AbstractSubHyperplane<S, T>) attribute.getPlusInside()).applyTransform(transform); + // we start with an empty list of splitters, it will be filled in out of recursion + attribute = new BoundaryAttribute<S>(tPO, tPI, new NodesSet<S>()); + } + + transformedNode = new BSPTree<S>(tSub, + recurseTransform(node.getPlus(), transform, map), + recurseTransform(node.getMinus(), transform, map), + attribute); + } + + map.put(node, transformedNode); + return transformedNode; + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractSubHyperplane.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractSubHyperplane.java new file mode 100644 index 0000000..396b795 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/AbstractSubHyperplane.java @@ -0,0 +1,191 @@ +/* + * 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.partitioning; + +import java.util.HashMap; +import java.util.Map; + +import org.apache.commons.math3.geometry.Space; + +/** This class implements the dimension-independent parts of {@link SubHyperplane}. + + * <p>sub-hyperplanes are obtained when parts of an {@link + * Hyperplane hyperplane} are chopped off by other hyperplanes that + * intersect it. The remaining part is a convex region. Such objects + * appear in {@link BSPTree BSP trees} as the intersection of a cut + * hyperplane with the convex region which it splits, the chopping + * hyperplanes are the cut hyperplanes closer to the tree root.</p> + + * @param <S> Type of the embedding space. + * @param <T> Type of the embedded sub-space. + + * @since 3.0 + */ +public abstract class AbstractSubHyperplane<S extends Space, T extends Space> + implements SubHyperplane<S> { + + /** Underlying hyperplane. */ + private final Hyperplane<S> hyperplane; + + /** Remaining region of the hyperplane. */ + private final Region<T> remainingRegion; + + /** Build a sub-hyperplane from an hyperplane and a region. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + protected AbstractSubHyperplane(final Hyperplane<S> hyperplane, + final Region<T> remainingRegion) { + this.hyperplane = hyperplane; + this.remainingRegion = remainingRegion; + } + + /** Build a sub-hyperplane from an hyperplane and a region. + * @param hyper underlying hyperplane + * @param remaining remaining region of the hyperplane + * @return a new sub-hyperplane + */ + protected abstract AbstractSubHyperplane<S, T> buildNew(final Hyperplane<S> hyper, + final Region<T> remaining); + + /** {@inheritDoc} */ + public AbstractSubHyperplane<S, T> copySelf() { + return buildNew(hyperplane.copySelf(), remainingRegion); + } + + /** Get the underlying hyperplane. + * @return underlying hyperplane + */ + public Hyperplane<S> getHyperplane() { + return hyperplane; + } + + /** Get the remaining region of the hyperplane. + * <p>The returned region is expressed in the canonical hyperplane + * frame and has the hyperplane dimension. For example a chopped + * hyperplane in the 3D euclidean is a 2D plane and the + * corresponding region is a convex 2D polygon.</p> + * @return remaining region of the hyperplane + */ + public Region<T> getRemainingRegion() { + return remainingRegion; + } + + /** {@inheritDoc} */ + public double getSize() { + return remainingRegion.getSize(); + } + + /** {@inheritDoc} */ + public AbstractSubHyperplane<S, T> reunite(final SubHyperplane<S> other) { + @SuppressWarnings("unchecked") + AbstractSubHyperplane<S, T> o = (AbstractSubHyperplane<S, T>) other; + return buildNew(hyperplane, + new RegionFactory<T>().union(remainingRegion, o.remainingRegion)); + } + + /** Apply a transform to the instance. + * <p>The instance must be a (D-1)-dimension sub-hyperplane with + * respect to the transform <em>not</em> a (D-2)-dimension + * sub-hyperplane the transform knows how to transform by + * itself. The transform will consist in transforming first the + * hyperplane and then the all region using the various methods + * provided by the transform.</p> + * @param transform D-dimension transform to apply + * @return the transformed instance + */ + public AbstractSubHyperplane<S, T> applyTransform(final Transform<S, T> transform) { + final Hyperplane<S> tHyperplane = transform.apply(hyperplane); + + // transform the tree, except for boundary attribute splitters + final Map<BSPTree<T>, BSPTree<T>> map = new HashMap<BSPTree<T>, BSPTree<T>>(); + final BSPTree<T> tTree = + recurseTransform(remainingRegion.getTree(false), tHyperplane, transform, map); + + // set up the boundary attributes splitters + for (final Map.Entry<BSPTree<T>, BSPTree<T>> entry : map.entrySet()) { + if (entry.getKey().getCut() != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<T> original = (BoundaryAttribute<T>) entry.getKey().getAttribute(); + if (original != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<T> transformed = (BoundaryAttribute<T>) entry.getValue().getAttribute(); + for (final BSPTree<T> splitter : original.getSplitters()) { + transformed.getSplitters().add(map.get(splitter)); + } + } + } + } + + return buildNew(tHyperplane, remainingRegion.buildNew(tTree)); + + } + + /** Recursively transform a BSP-tree from a sub-hyperplane. + * @param node current BSP tree node + * @param transformed image of the instance hyperplane by the transform + * @param transform transform to apply + * @param map transformed nodes map + * @return a new tree + */ + private BSPTree<T> recurseTransform(final BSPTree<T> node, + final Hyperplane<S> transformed, + final Transform<S, T> transform, + final Map<BSPTree<T>, BSPTree<T>> map) { + + final BSPTree<T> transformedNode; + if (node.getCut() == null) { + transformedNode = new BSPTree<T>(node.getAttribute()); + } else { + + @SuppressWarnings("unchecked") + BoundaryAttribute<T> attribute = (BoundaryAttribute<T>) node.getAttribute(); + if (attribute != null) { + final SubHyperplane<T> tPO = (attribute.getPlusOutside() == null) ? + null : transform.apply(attribute.getPlusOutside(), hyperplane, transformed); + final SubHyperplane<T> tPI = (attribute.getPlusInside() == null) ? + null : transform.apply(attribute.getPlusInside(), hyperplane, transformed); + // we start with an empty list of splitters, it will be filled in out of recursion + attribute = new BoundaryAttribute<T>(tPO, tPI, new NodesSet<T>()); + } + + transformedNode = new BSPTree<T>(transform.apply(node.getCut(), hyperplane, transformed), + recurseTransform(node.getPlus(), transformed, transform, map), + recurseTransform(node.getMinus(), transformed, transform, map), + attribute); + } + + map.put(node, transformedNode); + return transformedNode; + + } + + /** {@inheritDoc} */ + @Deprecated + public Side side(Hyperplane<S> hyper) { + return split(hyper).getSide(); + } + + /** {@inheritDoc} */ + public abstract SplitSubHyperplane<S> split(Hyperplane<S> hyper); + + /** {@inheritDoc} */ + public boolean isEmpty() { + return remainingRegion.isEmpty(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTree.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTree.java new file mode 100644 index 0000000..1f1a6ea --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTree.java @@ -0,0 +1,821 @@ +/* + * 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.partitioning; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.exception.MathIllegalStateException; +import org.apache.commons.math3.exception.MathInternalError; +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; + +/** This class represent a Binary Space Partition tree. + + * <p>BSP trees are an efficient way to represent space partitions and + * to associate attributes with each cell. Each node in a BSP tree + * represents a convex region which is partitioned in two convex + * sub-regions at each side of a cut hyperplane. The root tree + * contains the complete space.</p> + + * <p>The main use of such partitions is to use a boolean attribute to + * define an inside/outside property, hence representing arbitrary + * polytopes (line segments in 1D, polygons in 2D and polyhedrons in + * 3D) and to operate on them.</p> + + * <p>Another example would be to represent Voronoi tesselations, the + * attribute of each cell holding the defining point of the cell.</p> + + * <p>The application-defined attributes are shared among copied + * instances and propagated to split parts. These attributes are not + * used by the BSP-tree algorithms themselves, so the application can + * use them for any purpose. Since the tree visiting method holds + * internal and leaf nodes differently, it is possible to use + * different classes for internal nodes attributes and leaf nodes + * attributes. This should be used with care, though, because if the + * tree is modified in any way after attributes have been set, some + * internal nodes may become leaf nodes and some leaf nodes may become + * internal nodes.</p> + + * <p>One of the main sources for the development of this package was + * Bruce Naylor, John Amanatides and William Thibault paper <a + * href="http://www.cs.yorku.ca/~amana/research/bsptSetOp.pdf">Merging + * BSP Trees Yields Polyhedral Set Operations</a> Proc. Siggraph '90, + * Computer Graphics 24(4), August 1990, pp 115-124, published by the + * Association for Computing Machinery (ACM).</p> + + * @param <S> Type of the space. + + * @since 3.0 + */ +public class BSPTree<S extends Space> { + + /** Cut sub-hyperplane. */ + private SubHyperplane<S> cut; + + /** Tree at the plus side of the cut hyperplane. */ + private BSPTree<S> plus; + + /** Tree at the minus side of the cut hyperplane. */ + private BSPTree<S> minus; + + /** Parent tree. */ + private BSPTree<S> parent; + + /** Application-defined attribute. */ + private Object attribute; + + /** Build a tree having only one root cell representing the whole space. + */ + public BSPTree() { + cut = null; + plus = null; + minus = null; + parent = null; + attribute = null; + } + + /** Build a tree having only one root cell representing the whole space. + * @param attribute attribute of the tree (may be null) + */ + public BSPTree(final Object attribute) { + cut = null; + plus = null; + minus = null; + parent = null; + this.attribute = attribute; + } + + /** Build a BSPTree from its underlying elements. + * <p>This method does <em>not</em> perform any verification on + * consistency of its arguments, it should therefore be used only + * when then caller knows what it is doing.</p> + * <p>This method is mainly useful to build trees + * bottom-up. Building trees top-down is realized with the help of + * method {@link #insertCut insertCut}.</p> + * @param cut cut sub-hyperplane for the tree + * @param plus plus side sub-tree + * @param minus minus side sub-tree + * @param attribute attribute associated with the node (may be null) + * @see #insertCut + */ + public BSPTree(final SubHyperplane<S> cut, final BSPTree<S> plus, final BSPTree<S> minus, + final Object attribute) { + this.cut = cut; + this.plus = plus; + this.minus = minus; + this.parent = null; + this.attribute = attribute; + plus.parent = this; + minus.parent = this; + } + + /** Insert a cut sub-hyperplane in a node. + * <p>The sub-tree starting at this node will be completely + * overwritten. The new cut sub-hyperplane will be built from the + * intersection of the provided hyperplane with the cell. If the + * hyperplane does intersect the cell, the cell will have two + * children cells with {@code null} attributes on each side of + * the inserted cut sub-hyperplane. If the hyperplane does not + * intersect the cell then <em>no</em> cut hyperplane will be + * inserted and the cell will be changed to a leaf cell. The + * attribute of the node is never changed.</p> + * <p>This method is mainly useful when called on leaf nodes + * (i.e. nodes for which {@link #getCut getCut} returns + * {@code null}), in this case it provides a way to build a + * tree top-down (whereas the {@link #BSPTree(SubHyperplane, + * BSPTree, BSPTree, Object) 4 arguments constructor} is devoted to + * build trees bottom-up).</p> + * @param hyperplane hyperplane to insert, it will be chopped in + * order to fit in the cell defined by the parent nodes of the + * instance + * @return true if a cut sub-hyperplane has been inserted (i.e. if + * the cell now has two leaf child nodes) + * @see #BSPTree(SubHyperplane, BSPTree, BSPTree, Object) + */ + public boolean insertCut(final Hyperplane<S> hyperplane) { + + if (cut != null) { + plus.parent = null; + minus.parent = null; + } + + final SubHyperplane<S> chopped = fitToCell(hyperplane.wholeHyperplane()); + if (chopped == null || chopped.isEmpty()) { + cut = null; + plus = null; + minus = null; + return false; + } + + cut = chopped; + plus = new BSPTree<S>(); + plus.parent = this; + minus = new BSPTree<S>(); + minus.parent = this; + return true; + + } + + /** Copy the instance. + * <p>The instance created is completely independent of the original + * one. A deep copy is used, none of the underlying objects are + * shared (except for the nodes attributes and immutable + * objects).</p> + * @return a new tree, copy of the instance + */ + public BSPTree<S> copySelf() { + + if (cut == null) { + return new BSPTree<S>(attribute); + } + + return new BSPTree<S>(cut.copySelf(), plus.copySelf(), minus.copySelf(), + attribute); + + } + + /** Get the cut sub-hyperplane. + * @return cut sub-hyperplane, null if this is a leaf tree + */ + public SubHyperplane<S> getCut() { + return cut; + } + + /** Get the tree on the plus side of the cut hyperplane. + * @return tree on the plus side of the cut hyperplane, null if this + * is a leaf tree + */ + public BSPTree<S> getPlus() { + return plus; + } + + /** Get the tree on the minus side of the cut hyperplane. + * @return tree on the minus side of the cut hyperplane, null if this + * is a leaf tree + */ + public BSPTree<S> getMinus() { + return minus; + } + + /** Get the parent node. + * @return parent node, null if the node has no parents + */ + public BSPTree<S> getParent() { + return parent; + } + + /** Associate an attribute with the instance. + * @param attribute attribute to associate with the node + * @see #getAttribute + */ + public void setAttribute(final Object attribute) { + this.attribute = attribute; + } + + /** Get the attribute associated with the instance. + * @return attribute associated with the node or null if no + * attribute has been explicitly set using the {@link #setAttribute + * setAttribute} method + * @see #setAttribute + */ + public Object getAttribute() { + return attribute; + } + + /** Visit the BSP tree nodes. + * @param visitor object visiting the tree nodes + */ + public void visit(final BSPTreeVisitor<S> visitor) { + if (cut == null) { + visitor.visitLeafNode(this); + } else { + switch (visitor.visitOrder(this)) { + case PLUS_MINUS_SUB: + plus.visit(visitor); + minus.visit(visitor); + visitor.visitInternalNode(this); + break; + case PLUS_SUB_MINUS: + plus.visit(visitor); + visitor.visitInternalNode(this); + minus.visit(visitor); + break; + case MINUS_PLUS_SUB: + minus.visit(visitor); + plus.visit(visitor); + visitor.visitInternalNode(this); + break; + case MINUS_SUB_PLUS: + minus.visit(visitor); + visitor.visitInternalNode(this); + plus.visit(visitor); + break; + case SUB_PLUS_MINUS: + visitor.visitInternalNode(this); + plus.visit(visitor); + minus.visit(visitor); + break; + case SUB_MINUS_PLUS: + visitor.visitInternalNode(this); + minus.visit(visitor); + plus.visit(visitor); + break; + default: + throw new MathInternalError(); + } + + } + } + + /** Fit a sub-hyperplane inside the cell defined by the instance. + * <p>Fitting is done by chopping off the parts of the + * sub-hyperplane that lie outside of the cell using the + * cut-hyperplanes of the parent nodes of the instance.</p> + * @param sub sub-hyperplane to fit + * @return a new sub-hyperplane, guaranteed to have no part outside + * of the instance cell + */ + private SubHyperplane<S> fitToCell(final SubHyperplane<S> sub) { + SubHyperplane<S> s = sub; + for (BSPTree<S> tree = this; tree.parent != null && s != null; tree = tree.parent) { + if (tree == tree.parent.plus) { + s = s.split(tree.parent.cut.getHyperplane()).getPlus(); + } else { + s = s.split(tree.parent.cut.getHyperplane()).getMinus(); + } + } + return s; + } + + /** Get the cell to which a point belongs. + * <p>If the returned cell is a leaf node the points belongs to the + * interior of the node, if the cell is an internal node the points + * belongs to the node cut sub-hyperplane.</p> + * @param point point to check + * @return the tree cell to which the point belongs + * @deprecated as of 3.3, replaced with {@link #getCell(Point, double)} + */ + @Deprecated + public BSPTree<S> getCell(final Vector<S> point) { + return getCell((Point<S>) point, 1.0e-10); + } + + /** Get the cell to which a point belongs. + * <p>If the returned cell is a leaf node the points belongs to the + * interior of the node, if the cell is an internal node the points + * belongs to the node cut sub-hyperplane.</p> + * @param point point to check + * @param tolerance tolerance below which points close to a cut hyperplane + * are considered to belong to the hyperplane itself + * @return the tree cell to which the point belongs + */ + public BSPTree<S> getCell(final Point<S> point, final double tolerance) { + + if (cut == null) { + return this; + } + + // position of the point with respect to the cut hyperplane + final double offset = cut.getHyperplane().getOffset(point); + + if (FastMath.abs(offset) < tolerance) { + return this; + } else if (offset <= 0) { + // point is on the minus side of the cut hyperplane + return minus.getCell(point, tolerance); + } else { + // point is on the plus side of the cut hyperplane + return plus.getCell(point, tolerance); + } + + } + + /** Get the cells whose cut sub-hyperplanes are close to the point. + * @param point point to check + * @param maxOffset offset below which a cut sub-hyperplane is considered + * close to the point (in absolute value) + * @return close cells (may be empty if all cut sub-hyperplanes are farther + * than maxOffset from the point) + */ + public List<BSPTree<S>> getCloseCuts(final Point<S> point, final double maxOffset) { + final List<BSPTree<S>> close = new ArrayList<BSPTree<S>>(); + recurseCloseCuts(point, maxOffset, close); + return close; + } + + /** Get the cells whose cut sub-hyperplanes are close to the point. + * @param point point to check + * @param maxOffset offset below which a cut sub-hyperplane is considered + * close to the point (in absolute value) + * @param close list to fill + */ + private void recurseCloseCuts(final Point<S> point, final double maxOffset, + final List<BSPTree<S>> close) { + if (cut != null) { + + // position of the point with respect to the cut hyperplane + final double offset = cut.getHyperplane().getOffset(point); + + if (offset < -maxOffset) { + // point is on the minus side of the cut hyperplane + minus.recurseCloseCuts(point, maxOffset, close); + } else if (offset > maxOffset) { + // point is on the plus side of the cut hyperplane + plus.recurseCloseCuts(point, maxOffset, close); + } else { + // point is close to the cut hyperplane + close.add(this); + minus.recurseCloseCuts(point, maxOffset, close); + plus.recurseCloseCuts(point, maxOffset, close); + } + + } + } + + /** Perform condensation on a tree. + * <p>The condensation operation is not recursive, it must be called + * explicitly from leaves to root.</p> + */ + private void condense() { + if ((cut != null) && (plus.cut == null) && (minus.cut == null) && + (((plus.attribute == null) && (minus.attribute == null)) || + ((plus.attribute != null) && plus.attribute.equals(minus.attribute)))) { + attribute = (plus.attribute == null) ? minus.attribute : plus.attribute; + cut = null; + plus = null; + minus = null; + } + } + + /** Merge a BSP tree with the instance. + * <p>All trees are modified (parts of them are reused in the new + * tree), it is the responsibility of the caller to ensure a copy + * has been done before if any of the former tree should be + * preserved, <em>no</em> such copy is done here!</p> + * <p>The algorithm used here is directly derived from the one + * described in the Naylor, Amanatides and Thibault paper (section + * III, Binary Partitioning of a BSP Tree).</p> + * @param tree other tree to merge with the instance (will be + * <em>unusable</em> after the operation, as well as the + * instance itself) + * @param leafMerger object implementing the final merging phase + * (this is where the semantic of the operation occurs, generally + * depending on the attribute of the leaf node) + * @return a new tree, result of <code>instance <op> + * tree</code>, this value can be ignored if parentTree is not null + * since all connections have already been established + */ + public BSPTree<S> merge(final BSPTree<S> tree, final LeafMerger<S> leafMerger) { + return merge(tree, leafMerger, null, false); + } + + /** Merge a BSP tree with the instance. + * @param tree other tree to merge with the instance (will be + * <em>unusable</em> after the operation, as well as the + * instance itself) + * @param leafMerger object implementing the final merging phase + * (this is where the semantic of the operation occurs, generally + * depending on the attribute of the leaf node) + * @param parentTree parent tree to connect to (may be null) + * @param isPlusChild if true and if parentTree is not null, the + * resulting tree should be the plus child of its parent, ignored if + * parentTree is null + * @return a new tree, result of <code>instance <op> + * tree</code>, this value can be ignored if parentTree is not null + * since all connections have already been established + */ + private BSPTree<S> merge(final BSPTree<S> tree, final LeafMerger<S> leafMerger, + final BSPTree<S> parentTree, final boolean isPlusChild) { + if (cut == null) { + // cell/tree operation + return leafMerger.merge(this, tree, parentTree, isPlusChild, true); + } else if (tree.cut == null) { + // tree/cell operation + return leafMerger.merge(tree, this, parentTree, isPlusChild, false); + } else { + // tree/tree operation + final BSPTree<S> merged = tree.split(cut); + if (parentTree != null) { + merged.parent = parentTree; + if (isPlusChild) { + parentTree.plus = merged; + } else { + parentTree.minus = merged; + } + } + + // merging phase + plus.merge(merged.plus, leafMerger, merged, true); + minus.merge(merged.minus, leafMerger, merged, false); + merged.condense(); + if (merged.cut != null) { + merged.cut = merged.fitToCell(merged.cut.getHyperplane().wholeHyperplane()); + } + + return merged; + + } + } + + /** This interface gather the merging operations between a BSP tree + * leaf and another BSP tree. + * <p>As explained in Bruce Naylor, John Amanatides and William + * Thibault paper <a + * href="http://www.cs.yorku.ca/~amana/research/bsptSetOp.pdf">Merging + * BSP Trees Yields Polyhedral Set Operations</a>, + * the operations on {@link BSPTree BSP trees} can be expressed as a + * generic recursive merging operation where only the final part, + * when one of the operand is a leaf, is specific to the real + * operation semantics. For example, a tree representing a region + * using a boolean attribute to identify inside cells and outside + * cells would use four different objects to implement the final + * merging phase of the four set operations union, intersection, + * difference and symmetric difference (exclusive or).</p> + * @param <S> Type of the space. + */ + public interface LeafMerger<S extends Space> { + + /** Merge a leaf node and a tree node. + * <p>This method is called at the end of a recursive merging + * resulting from a {@code tree1.merge(tree2, leafMerger)} + * call, when one of the sub-trees involved is a leaf (i.e. when + * its cut-hyperplane is null). This is the only place where the + * precise semantics of the operation are required. For all upper + * level nodes in the tree, the merging operation is only a + * generic partitioning algorithm.</p> + * <p>Since the final operation may be non-commutative, it is + * important to know if the leaf node comes from the instance tree + * ({@code tree1}) or the argument tree + * ({@code tree2}). The third argument of the method is + * devoted to this. It can be ignored for commutative + * operations.</p> + * <p>The {@link BSPTree#insertInTree BSPTree.insertInTree} method + * may be useful to implement this method.</p> + * @param leaf leaf node (its cut hyperplane is guaranteed to be + * null) + * @param tree tree node (its cut hyperplane may be null or not) + * @param parentTree parent tree to connect to (may be null) + * @param isPlusChild if true and if parentTree is not null, the + * resulting tree should be the plus child of its parent, ignored if + * parentTree is null + * @param leafFromInstance if true, the leaf node comes from the + * instance tree ({@code tree1}) and the tree node comes from + * the argument tree ({@code tree2}) + * @return the BSP tree resulting from the merging (may be one of + * the arguments) + */ + BSPTree<S> merge(BSPTree<S> leaf, BSPTree<S> tree, BSPTree<S> parentTree, + boolean isPlusChild, boolean leafFromInstance); + + } + + /** This interface handles the corner cases when an internal node cut sub-hyperplane vanishes. + * <p> + * Such cases happens for example when a cut sub-hyperplane is inserted into + * another tree (during a merge operation), and is split in several parts, + * some of which becomes smaller than the tolerance. The corresponding node + * as then no cut sub-hyperplane anymore, but does have children. This interface + * specifies how to handle this situation. + * setting + * </p> + * @param <S> Type of the space. + * @since 3.4 + */ + public interface VanishingCutHandler<S extends Space> { + + /** Fix a node with both vanished cut and children. + * @param node node to fix + * @return fixed node + */ + BSPTree<S> fixNode(BSPTree<S> node); + + } + + /** Split a BSP tree by an external sub-hyperplane. + * <p>Split a tree in two halves, on each side of the + * sub-hyperplane. The instance is not modified.</p> + * <p>The tree returned is not upward-consistent: despite all of its + * sub-trees cut sub-hyperplanes (including its own cut + * sub-hyperplane) are bounded to the current cell, it is <em>not</em> + * attached to any parent tree yet. This tree is intended to be + * later inserted into an higher level tree.</p> + * <p>The algorithm used here is the one given in Naylor, Amanatides + * and Thibault paper (section III, Binary Partitioning of a BSP + * Tree).</p> + * @param sub partitioning sub-hyperplane, must be already clipped + * to the convex region represented by the instance, will be used as + * the cut sub-hyperplane of the returned tree + * @return a tree having the specified sub-hyperplane as its cut + * sub-hyperplane, the two parts of the split instance as its two + * sub-trees and a null parent + */ + public BSPTree<S> split(final SubHyperplane<S> sub) { + + if (cut == null) { + return new BSPTree<S>(sub, copySelf(), new BSPTree<S>(attribute), null); + } + + final Hyperplane<S> cHyperplane = cut.getHyperplane(); + final Hyperplane<S> sHyperplane = sub.getHyperplane(); + final SubHyperplane.SplitSubHyperplane<S> subParts = sub.split(cHyperplane); + switch (subParts.getSide()) { + case PLUS : + { // the partitioning sub-hyperplane is entirely in the plus sub-tree + final BSPTree<S> split = plus.split(sub); + if (cut.split(sHyperplane).getSide() == Side.PLUS) { + split.plus = + new BSPTree<S>(cut.copySelf(), split.plus, minus.copySelf(), attribute); + split.plus.condense(); + split.plus.parent = split; + } else { + split.minus = + new BSPTree<S>(cut.copySelf(), split.minus, minus.copySelf(), attribute); + split.minus.condense(); + split.minus.parent = split; + } + return split; + } + case MINUS : + { // the partitioning sub-hyperplane is entirely in the minus sub-tree + final BSPTree<S> split = minus.split(sub); + if (cut.split(sHyperplane).getSide() == Side.PLUS) { + split.plus = + new BSPTree<S>(cut.copySelf(), plus.copySelf(), split.plus, attribute); + split.plus.condense(); + split.plus.parent = split; + } else { + split.minus = + new BSPTree<S>(cut.copySelf(), plus.copySelf(), split.minus, attribute); + split.minus.condense(); + split.minus.parent = split; + } + return split; + } + case BOTH : + { + final SubHyperplane.SplitSubHyperplane<S> cutParts = cut.split(sHyperplane); + final BSPTree<S> split = + new BSPTree<S>(sub, plus.split(subParts.getPlus()), minus.split(subParts.getMinus()), + null); + split.plus.cut = cutParts.getPlus(); + split.minus.cut = cutParts.getMinus(); + final BSPTree<S> tmp = split.plus.minus; + split.plus.minus = split.minus.plus; + split.plus.minus.parent = split.plus; + split.minus.plus = tmp; + split.minus.plus.parent = split.minus; + split.plus.condense(); + split.minus.condense(); + return split; + } + default : + return cHyperplane.sameOrientationAs(sHyperplane) ? + new BSPTree<S>(sub, plus.copySelf(), minus.copySelf(), attribute) : + new BSPTree<S>(sub, minus.copySelf(), plus.copySelf(), attribute); + } + + } + + /** Insert the instance into another tree. + * <p>The instance itself is modified so its former parent should + * not be used anymore.</p> + * @param parentTree parent tree to connect to (may be null) + * @param isPlusChild if true and if parentTree is not null, the + * resulting tree should be the plus child of its parent, ignored if + * parentTree is null + * @see LeafMerger + * @deprecated as of 3.4, replaced with {@link #insertInTree(BSPTree, boolean, VanishingCutHandler)} + */ + @Deprecated + public void insertInTree(final BSPTree<S> parentTree, final boolean isPlusChild) { + insertInTree(parentTree, isPlusChild, new VanishingCutHandler<S>() { + /** {@inheritDoc} */ + public BSPTree<S> fixNode(BSPTree<S> node) { + // the cut should not be null + throw new MathIllegalStateException(LocalizedFormats.NULL_NOT_ALLOWED); + } + }); + } + + /** Insert the instance into another tree. + * <p>The instance itself is modified so its former parent should + * not be used anymore.</p> + * @param parentTree parent tree to connect to (may be null) + * @param isPlusChild if true and if parentTree is not null, the + * resulting tree should be the plus child of its parent, ignored if + * parentTree is null + * @param vanishingHandler handler to use for handling very rare corner + * cases of vanishing cut sub-hyperplanes in internal nodes during merging + * @see LeafMerger + * @since 3.4 + */ + public void insertInTree(final BSPTree<S> parentTree, final boolean isPlusChild, + final VanishingCutHandler<S> vanishingHandler) { + + // set up parent/child links + parent = parentTree; + if (parentTree != null) { + if (isPlusChild) { + parentTree.plus = this; + } else { + parentTree.minus = this; + } + } + + // make sure the inserted tree lies in the cell defined by its parent nodes + if (cut != null) { + + // explore the parent nodes from here towards tree root + for (BSPTree<S> tree = this; tree.parent != null; tree = tree.parent) { + + // this is an hyperplane of some parent node + final Hyperplane<S> hyperplane = tree.parent.cut.getHyperplane(); + + // chop off the parts of the inserted tree that extend + // on the wrong side of this parent hyperplane + if (tree == tree.parent.plus) { + cut = cut.split(hyperplane).getPlus(); + plus.chopOffMinus(hyperplane, vanishingHandler); + minus.chopOffMinus(hyperplane, vanishingHandler); + } else { + cut = cut.split(hyperplane).getMinus(); + plus.chopOffPlus(hyperplane, vanishingHandler); + minus.chopOffPlus(hyperplane, vanishingHandler); + } + + if (cut == null) { + // the cut sub-hyperplane has vanished + final BSPTree<S> fixed = vanishingHandler.fixNode(this); + cut = fixed.cut; + plus = fixed.plus; + minus = fixed.minus; + attribute = fixed.attribute; + if (cut == null) { + break; + } + } + + } + + // since we may have drop some parts of the inserted tree, + // perform a condensation pass to keep the tree structure simple + condense(); + + } + + } + + /** Prune a tree around a cell. + * <p> + * This method can be used to extract a convex cell from a tree. + * The original cell may either be a leaf node or an internal node. + * If it is an internal node, it's subtree will be ignored (i.e. the + * extracted cell will be a leaf node in all cases). The original + * tree to which the original cell belongs is not touched at all, + * a new independent tree will be built. + * </p> + * @param cellAttribute attribute to set for the leaf node + * corresponding to the initial instance cell + * @param otherLeafsAttributes attribute to set for the other leaf + * nodes + * @param internalAttributes attribute to set for the internal nodes + * @return a new tree (the original tree is left untouched) containing + * a single branch with the cell as a leaf node, and other leaf nodes + * as the remnants of the pruned branches + * @since 3.3 + */ + public BSPTree<S> pruneAroundConvexCell(final Object cellAttribute, + final Object otherLeafsAttributes, + final Object internalAttributes) { + + // build the current cell leaf + BSPTree<S> tree = new BSPTree<S>(cellAttribute); + + // build the pruned tree bottom-up + for (BSPTree<S> current = this; current.parent != null; current = current.parent) { + final SubHyperplane<S> parentCut = current.parent.cut.copySelf(); + final BSPTree<S> sibling = new BSPTree<S>(otherLeafsAttributes); + if (current == current.parent.plus) { + tree = new BSPTree<S>(parentCut, tree, sibling, internalAttributes); + } else { + tree = new BSPTree<S>(parentCut, sibling, tree, internalAttributes); + } + } + + return tree; + + } + + /** Chop off parts of the tree. + * <p>The instance is modified in place, all the parts that are on + * the minus side of the chopping hyperplane are discarded, only the + * parts on the plus side remain.</p> + * @param hyperplane chopping hyperplane + * @param vanishingHandler handler to use for handling very rare corner + * cases of vanishing cut sub-hyperplanes in internal nodes during merging + */ + private void chopOffMinus(final Hyperplane<S> hyperplane, final VanishingCutHandler<S> vanishingHandler) { + if (cut != null) { + + cut = cut.split(hyperplane).getPlus(); + plus.chopOffMinus(hyperplane, vanishingHandler); + minus.chopOffMinus(hyperplane, vanishingHandler); + + if (cut == null) { + // the cut sub-hyperplane has vanished + final BSPTree<S> fixed = vanishingHandler.fixNode(this); + cut = fixed.cut; + plus = fixed.plus; + minus = fixed.minus; + attribute = fixed.attribute; + } + + } + } + + /** Chop off parts of the tree. + * <p>The instance is modified in place, all the parts that are on + * the plus side of the chopping hyperplane are discarded, only the + * parts on the minus side remain.</p> + * @param hyperplane chopping hyperplane + * @param vanishingHandler handler to use for handling very rare corner + * cases of vanishing cut sub-hyperplanes in internal nodes during merging + */ + private void chopOffPlus(final Hyperplane<S> hyperplane, final VanishingCutHandler<S> vanishingHandler) { + if (cut != null) { + + cut = cut.split(hyperplane).getMinus(); + plus.chopOffPlus(hyperplane, vanishingHandler); + minus.chopOffPlus(hyperplane, vanishingHandler); + + if (cut == null) { + // the cut sub-hyperplane has vanished + final BSPTree<S> fixed = vanishingHandler.fixNode(this); + cut = fixed.cut; + plus = fixed.plus; + minus = fixed.minus; + attribute = fixed.attribute; + } + + } + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTreeVisitor.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTreeVisitor.java new file mode 100644 index 0000000..3d09939 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BSPTreeVisitor.java @@ -0,0 +1,114 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** This interface is used to visit {@link BSPTree BSP tree} nodes. + + * <p>Navigation through {@link BSPTree BSP trees} can be done using + * two different point of views:</p> + * <ul> + * <li> + * the first one is in a node-oriented way using the {@link + * BSPTree#getPlus}, {@link BSPTree#getMinus} and {@link + * BSPTree#getParent} methods. Terminal nodes without associated + * {@link SubHyperplane sub-hyperplanes} can be visited this way, + * there is no constraint in the visit order, and it is possible + * to visit either all nodes or only a subset of the nodes + * </li> + * <li> + * the second one is in a sub-hyperplane-oriented way using + * classes implementing this interface which obeys the visitor + * design pattern. The visit order is provided by the visitor as + * each node is first encountered. Each node is visited exactly + * once. + * </li> + * </ul> + + * @param <S> Type of the space. + + * @see BSPTree + * @see SubHyperplane + + * @since 3.0 + */ +public interface BSPTreeVisitor<S extends Space> { + + /** Enumerate for visit order with respect to plus sub-tree, minus sub-tree and cut sub-hyperplane. */ + enum Order { + /** Indicator for visit order plus sub-tree, then minus sub-tree, + * and last cut sub-hyperplane. + */ + PLUS_MINUS_SUB, + + /** Indicator for visit order plus sub-tree, then cut sub-hyperplane, + * and last minus sub-tree. + */ + PLUS_SUB_MINUS, + + /** Indicator for visit order minus sub-tree, then plus sub-tree, + * and last cut sub-hyperplane. + */ + MINUS_PLUS_SUB, + + /** Indicator for visit order minus sub-tree, then cut sub-hyperplane, + * and last plus sub-tree. + */ + MINUS_SUB_PLUS, + + /** Indicator for visit order cut sub-hyperplane, then plus sub-tree, + * and last minus sub-tree. + */ + SUB_PLUS_MINUS, + + /** Indicator for visit order cut sub-hyperplane, then minus sub-tree, + * and last plus sub-tree. + */ + SUB_MINUS_PLUS; + } + + /** Determine the visit order for this node. + * <p>Before attempting to visit an internal node, this method is + * called to determine the desired ordering of the visit. It is + * guaranteed that this method will be called before {@link + * #visitInternalNode visitInternalNode} for a given node, it will be + * called exactly once for each internal node.</p> + * @param node BSP node guaranteed to have a non null cut sub-hyperplane + * @return desired visit order, must be one of + * {@link Order#PLUS_MINUS_SUB}, {@link Order#PLUS_SUB_MINUS}, + * {@link Order#MINUS_PLUS_SUB}, {@link Order#MINUS_SUB_PLUS}, + * {@link Order#SUB_PLUS_MINUS}, {@link Order#SUB_MINUS_PLUS} + */ + Order visitOrder(BSPTree<S> node); + + /** Visit a BSP tree node node having a non-null sub-hyperplane. + * <p>It is guaranteed that this method will be called after {@link + * #visitOrder visitOrder} has been called for a given node, + * it wil be called exactly once for each internal node.</p> + * @param node BSP node guaranteed to have a non null cut sub-hyperplane + * @see #visitLeafNode + */ + void visitInternalNode(BSPTree<S> node); + + /** Visit a leaf BSP tree node node having a null sub-hyperplane. + * @param node leaf BSP node having a null sub-hyperplane + * @see #visitInternalNode + */ + void visitLeafNode(BSPTree<S> node); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryAttribute.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryAttribute.java new file mode 100644 index 0000000..dad884c --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryAttribute.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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** Class holding boundary attributes. + * <p>This class is used for the attributes associated with the + * nodes of region boundary shell trees returned by the {@link + * Region#getTree(boolean) Region.getTree(includeBoundaryAttributes)} + * when the boolean {@code includeBoundaryAttributes} parameter is + * set to {@code true}. It contains the parts of the node cut + * sub-hyperplane that belong to the boundary.</p> + * <p>This class is a simple placeholder, it does not provide any + * processing methods.</p> + * @param <S> Type of the space. + * @see Region#getTree + * @since 3.0 + */ +public class BoundaryAttribute<S extends Space> { + + /** Part of the node cut sub-hyperplane that belongs to the + * boundary and has the outside of the region on the plus side of + * its underlying hyperplane (may be null). + */ + private final SubHyperplane<S> plusOutside; + + /** Part of the node cut sub-hyperplane that belongs to the + * boundary and has the inside of the region on the plus side of + * its underlying hyperplane (may be null). + */ + private final SubHyperplane<S> plusInside; + + /** Sub-hyperplanes that were used to split the boundary part. */ + private final NodesSet<S> splitters; + + /** Simple constructor. + * @param plusOutside part of the node cut sub-hyperplane that + * belongs to the boundary and has the outside of the region on + * the plus side of its underlying hyperplane (may be null) + * @param plusInside part of the node cut sub-hyperplane that + * belongs to the boundary and has the inside of the region on the + * plus side of its underlying hyperplane (may be null) + * @deprecated as of 3.4, the constructor has been replaced by a new one + * which is not public anymore, as it is intended to be used only by + * {@link BoundaryBuilder} + */ + @Deprecated + public BoundaryAttribute(final SubHyperplane<S> plusOutside, + final SubHyperplane<S> plusInside) { + this(plusOutside, plusInside, null); + } + + /** Simple constructor. + * @param plusOutside part of the node cut sub-hyperplane that + * belongs to the boundary and has the outside of the region on + * the plus side of its underlying hyperplane (may be null) + * @param plusInside part of the node cut sub-hyperplane that + * belongs to the boundary and has the inside of the region on the + * plus side of its underlying hyperplane (may be null) + * @param splitters sub-hyperplanes that were used to + * split the boundary part (may be null) + * @since 3.4 + */ + BoundaryAttribute(final SubHyperplane<S> plusOutside, + final SubHyperplane<S> plusInside, + final NodesSet<S> splitters) { + this.plusOutside = plusOutside; + this.plusInside = plusInside; + this.splitters = splitters; + } + + /** Get the part of the node cut sub-hyperplane that belongs to the + * boundary and has the outside of the region on the plus side of + * its underlying hyperplane. + * @return part of the node cut sub-hyperplane that belongs to the + * boundary and has the outside of the region on the plus side of + * its underlying hyperplane + */ + public SubHyperplane<S> getPlusOutside() { + return plusOutside; + } + + /** Get the part of the node cut sub-hyperplane that belongs to the + * boundary and has the inside of the region on the plus side of + * its underlying hyperplane. + * @return part of the node cut sub-hyperplane that belongs to the + * boundary and has the inside of the region on the plus side of + * its underlying hyperplane + */ + public SubHyperplane<S> getPlusInside() { + return plusInside; + } + + /** Get the sub-hyperplanes that were used to split the boundary part. + * @return sub-hyperplanes that were used to split the boundary part + */ + public NodesSet<S> getSplitters() { + return splitters; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryBuilder.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryBuilder.java new file mode 100644 index 0000000..cea4de3 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryBuilder.java @@ -0,0 +1,95 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** Visitor building boundary shell tree. + * <p> + * The boundary shell is represented as {@link BoundaryAttribute boundary attributes} + * at each internal node. + * </p> + * @param <S> Type of the space. + * @since 3.4 + */ +class BoundaryBuilder<S extends Space> implements BSPTreeVisitor<S> { + + /** {@inheritDoc} */ + public Order visitOrder(BSPTree<S> node) { + return Order.PLUS_MINUS_SUB; + } + + /** {@inheritDoc} */ + public void visitInternalNode(BSPTree<S> node) { + + SubHyperplane<S> plusOutside = null; + SubHyperplane<S> plusInside = null; + NodesSet<S> splitters = null; + + // characterize the cut sub-hyperplane, + // first with respect to the plus sub-tree + final Characterization<S> plusChar = new Characterization<S>(node.getPlus(), node.getCut().copySelf()); + + if (plusChar.touchOutside()) { + // plusChar.outsideTouching() corresponds to a subset of the cut sub-hyperplane + // known to have outside cells on its plus side, we want to check if parts + // of this subset do have inside cells on their minus side + final Characterization<S> minusChar = new Characterization<S>(node.getMinus(), plusChar.outsideTouching()); + if (minusChar.touchInside()) { + // this part belongs to the boundary, + // it has the outside on its plus side and the inside on its minus side + plusOutside = minusChar.insideTouching(); + splitters = new NodesSet<S>(); + splitters.addAll(minusChar.getInsideSplitters()); + splitters.addAll(plusChar.getOutsideSplitters()); + } + } + + if (plusChar.touchInside()) { + // plusChar.insideTouching() corresponds to a subset of the cut sub-hyperplane + // known to have inside cells on its plus side, we want to check if parts + // of this subset do have outside cells on their minus side + final Characterization<S> minusChar = new Characterization<S>(node.getMinus(), plusChar.insideTouching()); + if (minusChar.touchOutside()) { + // this part belongs to the boundary, + // it has the inside on its plus side and the outside on its minus side + plusInside = minusChar.outsideTouching(); + if (splitters == null) { + splitters = new NodesSet<S>(); + } + splitters.addAll(minusChar.getOutsideSplitters()); + splitters.addAll(plusChar.getInsideSplitters()); + } + } + + if (splitters != null) { + // the parent nodes are natural splitters for boundary sub-hyperplanes + for (BSPTree<S> up = node.getParent(); up != null; up = up.getParent()) { + splitters.add(up); + } + } + + // set the boundary attribute at non-leaf nodes + node.setAttribute(new BoundaryAttribute<S>(plusOutside, plusInside, splitters)); + + } + + /** {@inheritDoc} */ + public void visitLeafNode(BSPTree<S> node) { + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjection.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjection.java new file mode 100644 index 0000000..03526e4 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjection.java @@ -0,0 +1,83 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** Class holding the result of point projection on region boundary. + * <p>This class is a simple placeholder, it does not provide any + * processing methods.</p> + * <p>Instances of this class are guaranteed to be immutable</p> + * @param <S> Type of the space. + * @since 3.3 + * @see AbstractRegion#projectToBoundary(Point) + */ +public class BoundaryProjection<S extends Space> { + + /** Original point. */ + private final Point<S> original; + + /** Projected point. */ + private final Point<S> projected; + + /** Offset of the point with respect to the boundary it is projected on. */ + private final double offset; + + /** Constructor from raw elements. + * @param original original point + * @param projected projected point + * @param offset offset of the point with respect to the boundary it is projected on + */ + public BoundaryProjection(final Point<S> original, final Point<S> projected, final double offset) { + this.original = original; + this.projected = projected; + this.offset = offset; + } + + /** Get the original point. + * @return original point + */ + public Point<S> getOriginal() { + return original; + } + + /** Projected point. + * @return projected point, or null if there are no boundary + */ + public Point<S> getProjected() { + return projected; + } + + /** Offset of the point with respect to the boundary it is projected on. + * <p> + * The offset with respect to the boundary is negative if the {@link + * #getOriginal() original point} is inside the region, and positive otherwise. + * </p> + * <p> + * If there are no boundary, the value is set to either {@code + * Double.POSITIVE_INFINITY} if the region is empty (i.e. all points are + * outside of the region) or {@code Double.NEGATIVE_INFINITY} if the region + * covers the whole space (i.e. all points are inside of the region). + * </p> + * @return offset of the point with respect to the boundary it is projected on + */ + public double getOffset() { + return offset; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjector.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjector.java new file mode 100644 index 0000000..486bbf1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundaryProjector.java @@ -0,0 +1,200 @@ +/* + * 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.partitioning; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.partitioning.Region.Location; +import org.apache.commons.math3.util.FastMath; + +/** Local tree visitor to compute projection on boundary. + * @param <S> Type of the space. + * @param <T> Type of the sub-space. + * @since 3.3 + */ +class BoundaryProjector<S extends Space, T extends Space> implements BSPTreeVisitor<S> { + + /** Original point. */ + private final Point<S> original; + + /** Current best projected point. */ + private Point<S> projected; + + /** Leaf node closest to the test point. */ + private BSPTree<S> leaf; + + /** Current offset. */ + private double offset; + + /** Simple constructor. + * @param original original point + */ + BoundaryProjector(final Point<S> original) { + this.original = original; + this.projected = null; + this.leaf = null; + this.offset = Double.POSITIVE_INFINITY; + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<S> node) { + // we want to visit the tree so that the first encountered + // leaf is the one closest to the test point + if (node.getCut().getHyperplane().getOffset(original) <= 0) { + return Order.MINUS_SUB_PLUS; + } else { + return Order.PLUS_SUB_MINUS; + } + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<S> node) { + + // project the point on the cut sub-hyperplane + final Hyperplane<S> hyperplane = node.getCut().getHyperplane(); + final double signedOffset = hyperplane.getOffset(original); + if (FastMath.abs(signedOffset) < offset) { + + // project point + final Point<S> regular = hyperplane.project(original); + + // get boundary parts + final List<Region<T>> boundaryParts = boundaryRegions(node); + + // check if regular projection really belongs to the boundary + boolean regularFound = false; + for (final Region<T> part : boundaryParts) { + if (!regularFound && belongsToPart(regular, hyperplane, part)) { + // the projected point lies in the boundary + projected = regular; + offset = FastMath.abs(signedOffset); + regularFound = true; + } + } + + if (!regularFound) { + // the regular projected point is not on boundary, + // so we have to check further if a singular point + // (i.e. a vertex in 2D case) is a possible projection + for (final Region<T> part : boundaryParts) { + final Point<S> spI = singularProjection(regular, hyperplane, part); + if (spI != null) { + final double distance = original.distance(spI); + if (distance < offset) { + projected = spI; + offset = distance; + } + } + } + + } + + } + + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<S> node) { + if (leaf == null) { + // this is the first leaf we visit, + // it is the closest one to the original point + leaf = node; + } + } + + /** Get the projection. + * @return projection + */ + public BoundaryProjection<S> getProjection() { + + // fix offset sign + offset = FastMath.copySign(offset, (Boolean) leaf.getAttribute() ? -1 : +1); + + return new BoundaryProjection<S>(original, projected, offset); + + } + + /** Extract the regions of the boundary on an internal node. + * @param node internal node + * @return regions in the node sub-hyperplane + */ + private List<Region<T>> boundaryRegions(final BSPTree<S> node) { + + final List<Region<T>> regions = new ArrayList<Region<T>>(2); + + @SuppressWarnings("unchecked") + final BoundaryAttribute<S> ba = (BoundaryAttribute<S>) node.getAttribute(); + addRegion(ba.getPlusInside(), regions); + addRegion(ba.getPlusOutside(), regions); + + return regions; + + } + + /** Add a boundary region to a list. + * @param sub sub-hyperplane defining the region + * @param list to fill up + */ + private void addRegion(final SubHyperplane<S> sub, final List<Region<T>> list) { + if (sub != null) { + @SuppressWarnings("unchecked") + final Region<T> region = ((AbstractSubHyperplane<S, T>) sub).getRemainingRegion(); + if (region != null) { + list.add(region); + } + } + } + + /** Check if a projected point lies on a boundary part. + * @param point projected point to check + * @param hyperplane hyperplane into which the point was projected + * @param part boundary part + * @return true if point lies on the boundary part + */ + private boolean belongsToPart(final Point<S> point, final Hyperplane<S> hyperplane, + final Region<T> part) { + + // there is a non-null sub-space, we can dive into smaller dimensions + @SuppressWarnings("unchecked") + final Embedding<S, T> embedding = (Embedding<S, T>) hyperplane; + return part.checkPoint(embedding.toSubSpace(point)) != Location.OUTSIDE; + + } + + /** Get the projection to the closest boundary singular point. + * @param point projected point to check + * @param hyperplane hyperplane into which the point was projected + * @param part boundary part + * @return projection to a singular point of boundary part (may be null) + */ + private Point<S> singularProjection(final Point<S> point, final Hyperplane<S> hyperplane, + final Region<T> part) { + + // there is a non-null sub-space, we can dive into smaller dimensions + @SuppressWarnings("unchecked") + final Embedding<S, T> embedding = (Embedding<S, T>) hyperplane; + final BoundaryProjection<T> bp = part.projectToBoundary(embedding.toSubSpace(point)); + + // back to initial dimension + return (bp.getProjected() == null) ? null : embedding.toSpace(bp.getProjected()); + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundarySizeVisitor.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundarySizeVisitor.java new file mode 100644 index 0000000..054838b --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/BoundarySizeVisitor.java @@ -0,0 +1,65 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** Visitor computing the boundary size. + * @param <S> Type of the space. + * @since 3.0 + */ +class BoundarySizeVisitor<S extends Space> implements BSPTreeVisitor<S> { + + /** Size of the boundary. */ + private double boundarySize; + + /** Simple constructor. + */ + BoundarySizeVisitor() { + boundarySize = 0; + } + + /** {@inheritDoc}*/ + public Order visitOrder(final BSPTree<S> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc}*/ + public void visitInternalNode(final BSPTree<S> node) { + @SuppressWarnings("unchecked") + final BoundaryAttribute<S> attribute = + (BoundaryAttribute<S>) node.getAttribute(); + if (attribute.getPlusOutside() != null) { + boundarySize += attribute.getPlusOutside().getSize(); + } + if (attribute.getPlusInside() != null) { + boundarySize += attribute.getPlusInside().getSize(); + } + } + + /** {@inheritDoc}*/ + public void visitLeafNode(final BSPTree<S> node) { + } + + /** Get the size of the boundary. + * @return size of the boundary + */ + public double getSize() { + return boundarySize; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Characterization.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Characterization.java new file mode 100644 index 0000000..f8ec2f9 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Characterization.java @@ -0,0 +1,190 @@ +/* + * 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.partitioning; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.exception.MathInternalError; +import org.apache.commons.math3.geometry.Space; + +/** Cut sub-hyperplanes characterization with respect to inside/outside cells. + * @see BoundaryBuilder + * @param <S> Type of the space. + * @since 3.4 + */ +class Characterization<S extends Space> { + + /** Part of the cut sub-hyperplane that touch outside cells. */ + private SubHyperplane<S> outsideTouching; + + /** Part of the cut sub-hyperplane that touch inside cells. */ + private SubHyperplane<S> insideTouching; + + /** Nodes that were used to split the outside touching part. */ + private final NodesSet<S> outsideSplitters; + + /** Nodes that were used to split the outside touching part. */ + private final NodesSet<S> insideSplitters; + + /** Simple constructor. + * <p>Characterization consists in splitting the specified + * sub-hyperplane into several parts lying in inside and outside + * cells of the tree. The principle is to compute characterization + * twice for each cut sub-hyperplane in the tree, once on the plus + * node and once on the minus node. The parts that have the same flag + * (inside/inside or outside/outside) do not belong to the boundary + * while parts that have different flags (inside/outside or + * outside/inside) do belong to the boundary.</p> + * @param node current BSP tree node + * @param sub sub-hyperplane to characterize + */ + Characterization(final BSPTree<S> node, final SubHyperplane<S> sub) { + outsideTouching = null; + insideTouching = null; + outsideSplitters = new NodesSet<S>(); + insideSplitters = new NodesSet<S>(); + characterize(node, sub, new ArrayList<BSPTree<S>>()); + } + + /** Filter the parts of an hyperplane belonging to the boundary. + * <p>The filtering consist in splitting the specified + * sub-hyperplane into several parts lying in inside and outside + * cells of the tree. The principle is to call this method twice for + * each cut sub-hyperplane in the tree, once on the plus node and + * once on the minus node. The parts that have the same flag + * (inside/inside or outside/outside) do not belong to the boundary + * while parts that have different flags (inside/outside or + * outside/inside) do belong to the boundary.</p> + * @param node current BSP tree node + * @param sub sub-hyperplane to characterize + * @param splitters nodes that did split the current one + */ + private void characterize(final BSPTree<S> node, final SubHyperplane<S> sub, + final List<BSPTree<S>> splitters) { + if (node.getCut() == null) { + // we have reached a leaf node + final boolean inside = (Boolean) node.getAttribute(); + if (inside) { + addInsideTouching(sub, splitters); + } else { + addOutsideTouching(sub, splitters); + } + } else { + final Hyperplane<S> hyperplane = node.getCut().getHyperplane(); + final SubHyperplane.SplitSubHyperplane<S> split = sub.split(hyperplane); + switch (split.getSide()) { + case PLUS: + characterize(node.getPlus(), sub, splitters); + break; + case MINUS: + characterize(node.getMinus(), sub, splitters); + break; + case BOTH: + splitters.add(node); + characterize(node.getPlus(), split.getPlus(), splitters); + characterize(node.getMinus(), split.getMinus(), splitters); + splitters.remove(splitters.size() - 1); + break; + default: + // this should not happen + throw new MathInternalError(); + } + } + } + + /** Add a part of the cut sub-hyperplane known to touch an outside cell. + * @param sub part of the cut sub-hyperplane known to touch an outside cell + * @param splitters sub-hyperplanes that did split the current one + */ + private void addOutsideTouching(final SubHyperplane<S> sub, + final List<BSPTree<S>> splitters) { + if (outsideTouching == null) { + outsideTouching = sub; + } else { + outsideTouching = outsideTouching.reunite(sub); + } + outsideSplitters.addAll(splitters); + } + + /** Add a part of the cut sub-hyperplane known to touch an inside cell. + * @param sub part of the cut sub-hyperplane known to touch an inside cell + * @param splitters sub-hyperplanes that did split the current one + */ + private void addInsideTouching(final SubHyperplane<S> sub, + final List<BSPTree<S>> splitters) { + if (insideTouching == null) { + insideTouching = sub; + } else { + insideTouching = insideTouching.reunite(sub); + } + insideSplitters.addAll(splitters); + } + + /** Check if the cut sub-hyperplane touches outside cells. + * @return true if the cut sub-hyperplane touches outside cells + */ + public boolean touchOutside() { + return outsideTouching != null && !outsideTouching.isEmpty(); + } + + /** Get all the parts of the cut sub-hyperplane known to touch outside cells. + * @return parts of the cut sub-hyperplane known to touch outside cells + * (may be null or empty) + */ + public SubHyperplane<S> outsideTouching() { + return outsideTouching; + } + + /** Get the nodes that were used to split the outside touching part. + * <p> + * Splitting nodes are internal nodes (i.e. they have a non-null + * cut sub-hyperplane). + * </p> + * @return nodes that were used to split the outside touching part + */ + public NodesSet<S> getOutsideSplitters() { + return outsideSplitters; + } + + /** Check if the cut sub-hyperplane touches inside cells. + * @return true if the cut sub-hyperplane touches inside cells + */ + public boolean touchInside() { + return insideTouching != null && !insideTouching.isEmpty(); + } + + /** Get all the parts of the cut sub-hyperplane known to touch inside cells. + * @return parts of the cut sub-hyperplane known to touch inside cells + * (may be null or empty) + */ + public SubHyperplane<S> insideTouching() { + return insideTouching; + } + + /** Get the nodes that were used to split the inside touching part. + * <p> + * Splitting nodes are internal nodes (i.e. they have a non-null + * cut sub-hyperplane). + * </p> + * @return nodes that were used to split the inside touching part + */ + public NodesSet<S> getInsideSplitters() { + return insideSplitters; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Embedding.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Embedding.java new file mode 100644 index 0000000..74e2c00 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Embedding.java @@ -0,0 +1,68 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** This interface defines mappers between a space and one of its sub-spaces. + + * <p>Sub-spaces are the lower dimensions subsets of a n-dimensions + * space. The (n-1)-dimension sub-spaces are specific sub-spaces known + * as {@link Hyperplane hyperplanes}. This interface can be used regardless + * of the dimensions differences. As an example, {@link + * org.apache.commons.math3.geometry.euclidean.threed.Line Line} in 3D + * implements Embedding<{@link + * org.apache.commons.math3.geometry.euclidean.threed.Vector3D Vector3D}, {link + * org.apache.commons.math3.geometry.euclidean.oned.Vector1D Vector1D>, i.e. it + * maps directly dimensions 3 and 1.</p> + + * <p>In the 3D euclidean space, hyperplanes are 2D planes, and the 1D + * sub-spaces are lines.</p> + + * <p> + * Note that this interface is <em>not</em> intended to be implemented + * by Apache Commons Math users, it is only intended to be implemented + * within the library itself. New methods may be added even for minor + * versions, which breaks compatibility for external implementations. + * </p> + + * @param <S> Type of the embedding space. + * @param <T> Type of the embedded sub-space. + + * @see Hyperplane + * @since 3.0 + */ +public interface Embedding<S extends Space, T extends Space> { + + /** Transform a space point into a sub-space point. + * @param point n-dimension point of the space + * @return (n-1)-dimension point of the sub-space corresponding to + * the specified space point + * @see #toSpace + */ + Point<T> toSubSpace(Point<S> point); + + /** Transform a sub-space point into a space point. + * @param point (n-1)-dimension point of the sub-space + * @return n-dimension point of the space corresponding to the + * specified sub-space point + * @see #toSubSpace + */ + Point<S> toSpace(Point<T> point); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Hyperplane.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Hyperplane.java new file mode 100644 index 0000000..f90c3bc --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Hyperplane.java @@ -0,0 +1,98 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + +/** This interface represents an hyperplane of a space. + + * <p>The most prominent place where hyperplane appears in space + * partitioning is as cutters. Each partitioning node in a {@link + * BSPTree BSP tree} has a cut {@link SubHyperplane sub-hyperplane} + * which is either an hyperplane or a part of an hyperplane. In an + * n-dimensions euclidean space, an hyperplane is an (n-1)-dimensions + * hyperplane (for example a traditional plane in the 3D euclidean + * space). They can be more exotic objects in specific fields, for + * example a circle on the surface of the unit sphere.</p> + + * <p> + * Note that this interface is <em>not</em> intended to be implemented + * by Apache Commons Math users, it is only intended to be implemented + * within the library itself. New methods may be added even for minor + * versions, which breaks compatibility for external implementations. + * </p> + + * @param <S> Type of the space. + + * @since 3.0 + */ +public interface Hyperplane<S extends Space> { + + /** 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 + */ + Hyperplane<S> copySelf(); + + /** 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 + */ + double getOffset(Point<S> point); + + /** Project a point to the hyperplane. + * @param point point to project + * @return projected point + * @since 3.3 + */ + Point<S> project(Point<S> point); + + /** Get the tolerance below which points are considered to belong to the hyperplane. + * @return tolerance below which points are considered to belong to the hyperplane + * @since 3.3 + */ + double getTolerance(); + + /** Check if the instance has the same orientation as another hyperplane. + * <p>This method is expected to be called on parallel hyperplanes. The + * method should <em>not</em> re-check for parallelism, only for + * orientation, typically by testing something like the sign of the + * dot-products of normals.</p> + * @param other other hyperplane to check against the instance + * @return true if the instance and the other hyperplane have + * the same orientation + */ + boolean sameOrientationAs(Hyperplane<S> other); + + /** Build a sub-hyperplane covering the whole hyperplane. + * @return a sub-hyperplane covering the whole hyperplane + */ + SubHyperplane<S> wholeHyperplane(); + + /** Build a region covering the whole space. + * @return a region containing the instance + */ + Region<S> wholeSpace(); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/InsideFinder.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/InsideFinder.java new file mode 100644 index 0000000..b1db90a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/InsideFinder.java @@ -0,0 +1,150 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** Utility class checking if inside nodes can be found + * on the plus and minus sides of an hyperplane. + * @param <S> Type of the space. + * @since 3.4 + */ +class InsideFinder<S extends Space> { + + /** Region on which to operate. */ + private final Region<S> region; + + /** Indicator of inside leaf nodes found on the plus side. */ + private boolean plusFound; + + /** Indicator of inside leaf nodes found on the plus side. */ + private boolean minusFound; + + /** Simple constructor. + * @param region region on which to operate + */ + InsideFinder(final Region<S> region) { + this.region = region; + plusFound = false; + minusFound = false; + } + + /** Search recursively for inside leaf nodes on each side of the given hyperplane. + + * <p>The algorithm used here is directly derived from the one + * described in section III (<i>Binary Partitioning of a BSP + * Tree</i>) of the Bruce Naylor, John Amanatides and William + * Thibault paper <a + * href="http://www.cs.yorku.ca/~amana/research/bsptSetOp.pdf">Merging + * BSP Trees Yields Polyhedral Set Operations</a> Proc. Siggraph + * '90, Computer Graphics 24(4), August 1990, pp 115-124, published + * by the Association for Computing Machinery (ACM)..</p> + + * @param node current BSP tree node + * @param sub sub-hyperplane + */ + public void recurseSides(final BSPTree<S> node, final SubHyperplane<S> sub) { + + if (node.getCut() == null) { + if ((Boolean) node.getAttribute()) { + // this is an inside cell expanding across the hyperplane + plusFound = true; + minusFound = true; + } + return; + } + + final Hyperplane<S> hyperplane = node.getCut().getHyperplane(); + final SubHyperplane.SplitSubHyperplane<S> split = sub.split(hyperplane); + switch (split.getSide()) { + case PLUS : + // the sub-hyperplane is entirely in the plus sub-tree + if (node.getCut().split(sub.getHyperplane()).getSide() == Side.PLUS) { + if (!region.isEmpty(node.getMinus())) { + plusFound = true; + } + } else { + if (!region.isEmpty(node.getMinus())) { + minusFound = true; + } + } + if (!(plusFound && minusFound)) { + recurseSides(node.getPlus(), sub); + } + break; + case MINUS : + // the sub-hyperplane is entirely in the minus sub-tree + if (node.getCut().split(sub.getHyperplane()).getSide() == Side.PLUS) { + if (!region.isEmpty(node.getPlus())) { + plusFound = true; + } + } else { + if (!region.isEmpty(node.getPlus())) { + minusFound = true; + } + } + if (!(plusFound && minusFound)) { + recurseSides(node.getMinus(), sub); + } + break; + case BOTH : + // the sub-hyperplane extends in both sub-trees + + // explore first the plus sub-tree + recurseSides(node.getPlus(), split.getPlus()); + + // if needed, explore the minus sub-tree + if (!(plusFound && minusFound)) { + recurseSides(node.getMinus(), split.getMinus()); + } + break; + default : + // the sub-hyperplane and the cut sub-hyperplane share the same hyperplane + if (node.getCut().getHyperplane().sameOrientationAs(sub.getHyperplane())) { + if ((node.getPlus().getCut() != null) || ((Boolean) node.getPlus().getAttribute())) { + plusFound = true; + } + if ((node.getMinus().getCut() != null) || ((Boolean) node.getMinus().getAttribute())) { + minusFound = true; + } + } else { + if ((node.getPlus().getCut() != null) || ((Boolean) node.getPlus().getAttribute())) { + minusFound = true; + } + if ((node.getMinus().getCut() != null) || ((Boolean) node.getMinus().getAttribute())) { + plusFound = true; + } + } + } + + } + + /** Check if inside leaf nodes have been found on the plus side. + * @return true if inside leaf nodes have been found on the plus side + */ + public boolean plusFound() { + return plusFound; + } + + /** Check if inside leaf nodes have been found on the minus side. + * @return true if inside leaf nodes have been found on the minus side + */ + public boolean minusFound() { + return minusFound; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/NodesSet.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/NodesSet.java new file mode 100644 index 0000000..688279a --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/NodesSet.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.partitioning; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; + +import org.apache.commons.math3.geometry.Space; + +/** Set of {@link BSPTree BSP tree} nodes. + * @see BoundaryAttribute + * @param <S> Type of the space. + * @since 3.4 + */ +public class NodesSet<S extends Space> implements Iterable<BSPTree<S>> { + + /** List of sub-hyperplanes. */ + private List<BSPTree<S>> list; + + /** Simple constructor. + */ + public NodesSet() { + list = new ArrayList<BSPTree<S>>(); + } + + /** Add a node if not already known. + * @param node node to add + */ + public void add(final BSPTree<S> node) { + + for (final BSPTree<S> existing : list) { + if (node == existing) { + // the node is already known, don't add it + return; + } + } + + // the node was not known, add it + list.add(node); + + } + + /** Add nodes if they are not already known. + * @param iterator nodes iterator + */ + public void addAll(final Iterable<BSPTree<S>> iterator) { + for (final BSPTree<S> node : iterator) { + add(node); + } + } + + /** {@inheritDoc} */ + public Iterator<BSPTree<S>> iterator() { + return list.iterator(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Region.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Region.java new file mode 100644 index 0000000..9ff3946 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Region.java @@ -0,0 +1,221 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.Point; + +/** This interface represents a region of a space as a partition. + + * <p>Region are subsets of a space, they can be infinite (whole + * space, half space, infinite stripe ...) or finite (polygons in 2D, + * polyhedrons in 3D ...). Their main characteristic is to separate + * points that are considered to be <em>inside</em> the region from + * points considered to be <em>outside</em> of it. In between, there + * may be points on the <em>boundary</em> of the region.</p> + + * <p>This implementation is limited to regions for which the boundary + * is composed of several {@link SubHyperplane sub-hyperplanes}, + * including regions with no boundary at all: the whole space and the + * empty region. They are not necessarily finite and not necessarily + * path-connected. They can contain holes.</p> + + * <p>Regions can be combined using the traditional sets operations : + * union, intersection, difference and symetric difference (exclusive + * or) for the binary operations, complement for the unary + * operation.</p> + + * <p> + * Note that this interface is <em>not</em> intended to be implemented + * by Apache Commons Math users, it is only intended to be implemented + * within the library itself. New methods may be added even for minor + * versions, which breaks compatibility for external implementations. + * </p> + + * @param <S> Type of the space. + + * @since 3.0 + */ +public interface Region<S extends Space> { + + /** Enumerate for the location of a point with respect to the region. */ + enum Location { + /** Code for points inside the partition. */ + INSIDE, + + /** Code for points outside of the partition. */ + OUTSIDE, + + /** Code for points on the partition boundary. */ + BOUNDARY; + } + + /** Build a region using the instance as a prototype. + * <p>This method allow to create new instances without knowing + * exactly the type of the region. It is an application of the + * prototype design pattern.</p> + * <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}. The + * tree also <em>must</em> have either null internal nodes or + * internal nodes representing the boundary as specified in the + * {@link #getTree getTree} method).</p> + * @param newTree inside/outside BSP tree representing the new region + * @return the built region + */ + Region<S> buildNew(BSPTree<S> newTree); + + /** 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 the underlying tree {@code Boolean} + * attributes and immutable objects).</p> + * @return a new region, copy of the instance + */ + Region<S> copySelf(); + + /** Check if the instance is empty. + * @return true if the instance is empty + */ + boolean isEmpty(); + + /** Check if the sub-tree starting at a given node is empty. + * @param node root node of the sub-tree (<em>must</em> have {@link + * Region Region} tree semantics, i.e. the leaf nodes must have + * {@code Boolean} attributes representing an inside/outside + * property) + * @return true if the sub-tree starting at the given node is empty + */ + boolean isEmpty(final BSPTree<S> node); + + /** Check if the instance covers the full space. + * @return true if the instance covers the full space + */ + boolean isFull(); + + /** Check if the sub-tree starting at a given node covers the full space. + * @param node root node of the sub-tree (<em>must</em> have {@link + * Region Region} tree semantics, i.e. the leaf nodes must have + * {@code Boolean} attributes representing an inside/outside + * property) + * @return true if the sub-tree starting at the given node covers the full space + */ + boolean isFull(final BSPTree<S> node); + + /** Check if the instance entirely contains another region. + * @param region region to check against the instance + * @return true if the instance contains the specified tree + */ + boolean contains(final Region<S> region); + + /** Check a point with respect to the region. + * @param point point to check + * @return a code representing the point status: either {@link + * Location#INSIDE}, {@link Location#OUTSIDE} or {@link Location#BOUNDARY} + */ + Location checkPoint(final Point<S> point); + + /** Project a point on the boundary of the region. + * @param point point to check + * @return projection of the point on the boundary + * @since 3.3 + */ + BoundaryProjection<S> projectToBoundary(final Point<S> point); + + /** Get the underlying BSP tree. + + * <p>Regions are represented by an underlying inside/outside BSP + * tree whose leaf attributes are {@code Boolean} instances + * representing inside leaf cells if the attribute value is + * {@code true} and outside leaf cells if the attribute is + * {@code false}. These leaf attributes are always present and + * guaranteed to be non null.</p> + + * <p>In addition to the leaf attributes, the internal nodes which + * correspond to cells split by cut sub-hyperplanes may contain + * {@link BoundaryAttribute BoundaryAttribute} objects representing + * the parts of the corresponding cut sub-hyperplane that belong to + * the boundary. When the boundary attributes have been computed, + * all internal nodes are guaranteed to have non-null + * attributes, however some {@link BoundaryAttribute + * BoundaryAttribute} instances may have their {@link + * BoundaryAttribute#getPlusInside() getPlusInside} and {@link + * BoundaryAttribute#getPlusOutside() getPlusOutside} methods both + * returning null if the corresponding cut sub-hyperplane does not + * have any parts belonging to the boundary.</p> + + * <p>Since computing the boundary is not always required and can be + * time-consuming for large trees, these internal nodes attributes + * are computed using lazy evaluation only when required by setting + * the {@code includeBoundaryAttributes} argument to + * {@code true}. Once computed, these attributes remain in the + * tree, which implies that in this case, further calls to the + * method for the same region will always include these attributes + * regardless of the value of the + * {@code includeBoundaryAttributes} argument.</p> + + * @param includeBoundaryAttributes if true, the boundary attributes + * at internal nodes are guaranteed to be included (they may be + * included even if the argument is false, if they have already been + * computed due to a previous call) + * @return underlying BSP tree + * @see BoundaryAttribute + */ + BSPTree<S> getTree(final boolean includeBoundaryAttributes); + + /** Get the size of the boundary. + * @return the size of the boundary (this is 0 in 1D, a length in + * 2D, an area in 3D ...) + */ + double getBoundarySize(); + + /** Get the size of the instance. + * @return the size of the instance (this is a length in 1D, an area + * in 2D, a volume in 3D ...) + */ + double getSize(); + + /** Get the barycenter of the instance. + * @return an object representing the barycenter + */ + Point<S> getBarycenter(); + + /** Compute the relative position of the instance with respect to an + * hyperplane. + * @param hyperplane reference hyperplane + * @return one of {@link Side#PLUS Side.PLUS}, {@link Side#MINUS + * Side.MINUS}, {@link Side#BOTH Side.BOTH} or {@link Side#HYPER + * Side.HYPER} (the latter result can occur only if the tree + * contains only one cut hyperplane) + * @deprecated as of 3.6, this method which was only intended for + * internal use is not used anymore + */ + @Deprecated + Side side(final Hyperplane<S> hyperplane); + + /** Get the parts of a sub-hyperplane that are contained in the region. + * <p>The parts of the sub-hyperplane that belong to the boundary are + * <em>not</em> included in the resulting parts.</p> + * @param sub sub-hyperplane traversing the region + * @return filtered sub-hyperplane + */ + SubHyperplane<S> intersection(final SubHyperplane<S> sub); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/RegionFactory.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/RegionFactory.java new file mode 100644 index 0000000..688ffde --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/RegionFactory.java @@ -0,0 +1,378 @@ +/* + * 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.partitioning; + +import java.util.HashMap; +import java.util.Map; + +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.Space; +import org.apache.commons.math3.geometry.partitioning.BSPTree.VanishingCutHandler; +import org.apache.commons.math3.geometry.partitioning.Region.Location; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane.SplitSubHyperplane; + +/** This class is a factory for {@link Region}. + + * @param <S> Type of the space. + + * @since 3.0 + */ +public class RegionFactory<S extends Space> { + + /** Visitor removing internal nodes attributes. */ + private final NodesCleaner nodeCleaner; + + /** Simple constructor. + */ + public RegionFactory() { + nodeCleaner = new NodesCleaner(); + } + + /** Build a convex region from a collection of bounding hyperplanes. + * @param hyperplanes collection of bounding hyperplanes + * @return a new convex region, or null if the collection is empty + */ + public Region<S> buildConvex(final Hyperplane<S> ... hyperplanes) { + if ((hyperplanes == null) || (hyperplanes.length == 0)) { + return null; + } + + // use the first hyperplane to build the right class + final Region<S> region = hyperplanes[0].wholeSpace(); + + // chop off parts of the space + BSPTree<S> node = region.getTree(false); + node.setAttribute(Boolean.TRUE); + for (final Hyperplane<S> hyperplane : hyperplanes) { + if (node.insertCut(hyperplane)) { + node.setAttribute(null); + node.getPlus().setAttribute(Boolean.FALSE); + node = node.getMinus(); + node.setAttribute(Boolean.TRUE); + } else { + // the hyperplane could not be inserted in the current leaf node + // either it is completely outside (which means the input hyperplanes + // are wrong), or it is parallel to a previous hyperplane + SubHyperplane<S> s = hyperplane.wholeHyperplane(); + for (BSPTree<S> tree = node; tree.getParent() != null && s != null; tree = tree.getParent()) { + final Hyperplane<S> other = tree.getParent().getCut().getHyperplane(); + final SplitSubHyperplane<S> split = s.split(other); + switch (split.getSide()) { + case HYPER : + // the hyperplane is parallel to a previous hyperplane + if (!hyperplane.sameOrientationAs(other)) { + // this hyperplane is opposite to the other one, + // the region is thinner than the tolerance, we consider it empty + return getComplement(hyperplanes[0].wholeSpace()); + } + // the hyperplane is an extension of an already known hyperplane, we just ignore it + break; + case PLUS : + // the hyperplane is outside of the current convex zone, + // the input hyperplanes are inconsistent + throw new MathIllegalArgumentException(LocalizedFormats.NOT_CONVEX_HYPERPLANES); + default : + s = split.getMinus(); + } + } + } + } + + return region; + + } + + /** Compute the union of two regions. + * @param region1 first region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @param region2 second region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @return a new region, result of {@code region1 union region2} + */ + public Region<S> union(final Region<S> region1, final Region<S> region2) { + final BSPTree<S> tree = + region1.getTree(false).merge(region2.getTree(false), new UnionMerger()); + tree.visit(nodeCleaner); + return region1.buildNew(tree); + } + + /** Compute the intersection of two regions. + * @param region1 first region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @param region2 second region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @return a new region, result of {@code region1 intersection region2} + */ + public Region<S> intersection(final Region<S> region1, final Region<S> region2) { + final BSPTree<S> tree = + region1.getTree(false).merge(region2.getTree(false), new IntersectionMerger()); + tree.visit(nodeCleaner); + return region1.buildNew(tree); + } + + /** Compute the symmetric difference (exclusive or) of two regions. + * @param region1 first region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @param region2 second region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @return a new region, result of {@code region1 xor region2} + */ + public Region<S> xor(final Region<S> region1, final Region<S> region2) { + final BSPTree<S> tree = + region1.getTree(false).merge(region2.getTree(false), new XorMerger()); + tree.visit(nodeCleaner); + return region1.buildNew(tree); + } + + /** Compute the difference of two regions. + * @param region1 first region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @param region2 second region (will be unusable after the operation as + * parts of it will be reused in the new region) + * @return a new region, result of {@code region1 minus region2} + */ + public Region<S> difference(final Region<S> region1, final Region<S> region2) { + final BSPTree<S> tree = + region1.getTree(false).merge(region2.getTree(false), new DifferenceMerger(region1, region2)); + tree.visit(nodeCleaner); + return region1.buildNew(tree); + } + + /** Get the complement of the region (exchanged interior/exterior). + * @param region region to complement, it will not modified, a new + * region independent region will be built + * @return a new region, complement of the specified one + */ + /** Get the complement of the region (exchanged interior/exterior). + * @param region region to complement, it will not modified, a new + * region independent region will be built + * @return a new region, complement of the specified one + */ + public Region<S> getComplement(final Region<S> region) { + return region.buildNew(recurseComplement(region.getTree(false))); + } + + /** Recursively build the complement of a BSP tree. + * @param node current node of the original tree + * @return new tree, complement of the node + */ + private BSPTree<S> recurseComplement(final BSPTree<S> node) { + + // transform the tree, except for boundary attribute splitters + final Map<BSPTree<S>, BSPTree<S>> map = new HashMap<BSPTree<S>, BSPTree<S>>(); + final BSPTree<S> transformedTree = recurseComplement(node, map); + + // set up the boundary attributes splitters + for (final Map.Entry<BSPTree<S>, BSPTree<S>> entry : map.entrySet()) { + if (entry.getKey().getCut() != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<S> original = (BoundaryAttribute<S>) entry.getKey().getAttribute(); + if (original != null) { + @SuppressWarnings("unchecked") + BoundaryAttribute<S> transformed = (BoundaryAttribute<S>) entry.getValue().getAttribute(); + for (final BSPTree<S> splitter : original.getSplitters()) { + transformed.getSplitters().add(map.get(splitter)); + } + } + } + } + + return transformedTree; + + } + + /** Recursively build the complement of a BSP tree. + * @param node current node of the original tree + * @param map transformed nodes map + * @return new tree, complement of the node + */ + private BSPTree<S> recurseComplement(final BSPTree<S> node, + final Map<BSPTree<S>, BSPTree<S>> map) { + + final BSPTree<S> transformedNode; + if (node.getCut() == null) { + transformedNode = new BSPTree<S>(((Boolean) node.getAttribute()) ? Boolean.FALSE : Boolean.TRUE); + } else { + + @SuppressWarnings("unchecked") + BoundaryAttribute<S> attribute = (BoundaryAttribute<S>) node.getAttribute(); + if (attribute != null) { + final SubHyperplane<S> plusOutside = + (attribute.getPlusInside() == null) ? null : attribute.getPlusInside().copySelf(); + final SubHyperplane<S> plusInside = + (attribute.getPlusOutside() == null) ? null : attribute.getPlusOutside().copySelf(); + // we start with an empty list of splitters, it will be filled in out of recursion + attribute = new BoundaryAttribute<S>(plusOutside, plusInside, new NodesSet<S>()); + } + + transformedNode = new BSPTree<S>(node.getCut().copySelf(), + recurseComplement(node.getPlus(), map), + recurseComplement(node.getMinus(), map), + attribute); + } + + map.put(node, transformedNode); + return transformedNode; + + } + + /** BSP tree leaf merger computing union of two regions. */ + private class UnionMerger implements BSPTree.LeafMerger<S> { + /** {@inheritDoc} */ + public BSPTree<S> merge(final BSPTree<S> leaf, final BSPTree<S> tree, + final BSPTree<S> parentTree, + final boolean isPlusChild, final boolean leafFromInstance) { + if ((Boolean) leaf.getAttribute()) { + // the leaf node represents an inside cell + leaf.insertInTree(parentTree, isPlusChild, new VanishingToLeaf(true)); + return leaf; + } + // the leaf node represents an outside cell + tree.insertInTree(parentTree, isPlusChild, new VanishingToLeaf(false)); + return tree; + } + } + + /** BSP tree leaf merger computing intersection of two regions. */ + private class IntersectionMerger implements BSPTree.LeafMerger<S> { + /** {@inheritDoc} */ + public BSPTree<S> merge(final BSPTree<S> leaf, final BSPTree<S> tree, + final BSPTree<S> parentTree, + final boolean isPlusChild, final boolean leafFromInstance) { + if ((Boolean) leaf.getAttribute()) { + // the leaf node represents an inside cell + tree.insertInTree(parentTree, isPlusChild, new VanishingToLeaf(true)); + return tree; + } + // the leaf node represents an outside cell + leaf.insertInTree(parentTree, isPlusChild, new VanishingToLeaf(false)); + return leaf; + } + } + + /** BSP tree leaf merger computing symmetric difference (exclusive or) of two regions. */ + private class XorMerger implements BSPTree.LeafMerger<S> { + /** {@inheritDoc} */ + public BSPTree<S> merge(final BSPTree<S> leaf, final BSPTree<S> tree, + final BSPTree<S> parentTree, final boolean isPlusChild, + final boolean leafFromInstance) { + BSPTree<S> t = tree; + if ((Boolean) leaf.getAttribute()) { + // the leaf node represents an inside cell + t = recurseComplement(t); + } + t.insertInTree(parentTree, isPlusChild, new VanishingToLeaf(true)); + return t; + } + } + + /** BSP tree leaf merger computing difference of two regions. */ + private class DifferenceMerger implements BSPTree.LeafMerger<S>, VanishingCutHandler<S> { + + /** Region to subtract from. */ + private final Region<S> region1; + + /** Region to subtract. */ + private final Region<S> region2; + + /** Simple constructor. + * @param region1 region to subtract from + * @param region2 region to subtract + */ + DifferenceMerger(final Region<S> region1, final Region<S> region2) { + this.region1 = region1.copySelf(); + this.region2 = region2.copySelf(); + } + + /** {@inheritDoc} */ + public BSPTree<S> merge(final BSPTree<S> leaf, final BSPTree<S> tree, + final BSPTree<S> parentTree, final boolean isPlusChild, + final boolean leafFromInstance) { + if ((Boolean) leaf.getAttribute()) { + // the leaf node represents an inside cell + final BSPTree<S> argTree = + recurseComplement(leafFromInstance ? tree : leaf); + argTree.insertInTree(parentTree, isPlusChild, this); + return argTree; + } + // the leaf node represents an outside cell + final BSPTree<S> instanceTree = + leafFromInstance ? leaf : tree; + instanceTree.insertInTree(parentTree, isPlusChild, this); + return instanceTree; + } + + /** {@inheritDoc} */ + public BSPTree<S> fixNode(final BSPTree<S> node) { + // get a representative point in the degenerate cell + final BSPTree<S> cell = node.pruneAroundConvexCell(Boolean.TRUE, Boolean.FALSE, null); + final Region<S> r = region1.buildNew(cell); + final Point<S> p = r.getBarycenter(); + return new BSPTree<S>(region1.checkPoint(p) == Location.INSIDE && + region2.checkPoint(p) == Location.OUTSIDE); + } + + } + + /** Visitor removing internal nodes attributes. */ + private class NodesCleaner implements BSPTreeVisitor<S> { + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<S> node) { + return Order.PLUS_SUB_MINUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<S> node) { + node.setAttribute(null); + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<S> node) { + } + + } + + /** Handler replacing nodes with vanishing cuts with leaf nodes. */ + private class VanishingToLeaf implements VanishingCutHandler<S> { + + /** Inside/outside indocator to use for ambiguous nodes. */ + private final boolean inside; + + /** Simple constructor. + * @param inside inside/outside indicator to use for ambiguous nodes + */ + VanishingToLeaf(final boolean inside) { + this.inside = inside; + } + + /** {@inheritDoc} */ + public BSPTree<S> fixNode(final BSPTree<S> node) { + if (node.getPlus().getAttribute().equals(node.getMinus().getAttribute())) { + // no ambiguity + return new BSPTree<S>(node.getPlus().getAttribute()); + } else { + // ambiguous node + return new BSPTree<S>(inside); + } + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Side.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Side.java new file mode 100644 index 0000000..c9a1357 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Side.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.partitioning; + +/** Enumerate representing the location of an element with respect to an + * {@link Hyperplane hyperplane} of a space. + * @since 3.0 + */ +public enum Side { + + /** Code for the plus side of the hyperplane. */ + PLUS, + + /** Code for the minus side of the hyperplane. */ + MINUS, + + /** Code for elements crossing the hyperplane from plus to minus side. */ + BOTH, + + /** Code for the hyperplane itself. */ + HYPER; + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/SubHyperplane.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/SubHyperplane.java new file mode 100644 index 0000000..2069f6f --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/SubHyperplane.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.partitioning; + +import org.apache.commons.math3.geometry.Space; + +/** This interface represents the remaining parts of an hyperplane after + * other parts have been chopped off. + + * <p>sub-hyperplanes are obtained when parts of an {@link + * Hyperplane hyperplane} are chopped off by other hyperplanes that + * intersect it. The remaining part is a convex region. Such objects + * appear in {@link BSPTree BSP trees} as the intersection of a cut + * hyperplane with the convex region which it splits, the chopping + * hyperplanes are the cut hyperplanes closer to the tree root.</p> + + * <p> + * Note that this interface is <em>not</em> intended to be implemented + * by Apache Commons Math users, it is only intended to be implemented + * within the library itself. New methods may be added even for minor + * versions, which breaks compatibility for external implementations. + * </p> + + * @param <S> Type of the embedding space. + + * @since 3.0 + */ +public interface SubHyperplane<S extends Space> { + + /** Copy the instance. + * <p>The instance created is completely independent of the original + * one. A deep copy is used, none of the underlying objects are + * shared (except for the nodes attributes and immutable + * objects).</p> + * @return a new sub-hyperplane, copy of the instance + */ + SubHyperplane<S> copySelf(); + + /** Get the underlying hyperplane. + * @return underlying hyperplane + */ + Hyperplane<S> getHyperplane(); + + /** Check if the instance is empty. + * @return true if the instance is empty + */ + boolean isEmpty(); + + /** Get the size of the instance. + * @return the size of the instance (this is a length in 1D, an area + * in 2D, a volume in 3D ...) + */ + double getSize(); + + /** Compute the relative position of the instance with respect + * to an hyperplane. + * @param hyperplane hyperplane to check instance against + * @return one of {@link Side#PLUS}, {@link Side#MINUS}, {@link Side#BOTH}, + * {@link Side#HYPER} + * @deprecated as of 3.6, replaced with {@link #split(Hyperplane)}.{@link SplitSubHyperplane#getSide()} + */ + @Deprecated + Side side(Hyperplane<S> hyperplane); + + /** 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 hyperplane and the part of the + * instance on the minus side of the hyperplane + */ + SplitSubHyperplane<S> split(Hyperplane<S> hyperplane); + + /** Compute the union of the instance and another sub-hyperplane. + * @param other other sub-hyperplane to union (<em>must</em> be in the + * same hyperplane as the instance) + * @return a new sub-hyperplane, union of the instance and other + */ + SubHyperplane<S> reunite(SubHyperplane<S> other); + + /** Class holding the results of the {@link #split split} method. + * @param <U> Type of the embedding space. + */ + class SplitSubHyperplane<U extends Space> { + + /** Part of the sub-hyperplane on the plus side of the splitting hyperplane. */ + private final SubHyperplane<U> plus; + + /** Part of the sub-hyperplane on the minus side of the splitting hyperplane. */ + private final SubHyperplane<U> minus; + + /** Build a SplitSubHyperplane from its parts. + * @param plus part of the sub-hyperplane on the plus side of the + * splitting hyperplane + * @param minus part of the sub-hyperplane on the minus side of the + * splitting hyperplane + */ + public SplitSubHyperplane(final SubHyperplane<U> plus, + final SubHyperplane<U> minus) { + this.plus = plus; + this.minus = minus; + } + + /** Get the part of the sub-hyperplane on the plus side of the splitting hyperplane. + * @return part of the sub-hyperplane on the plus side of the splitting hyperplane + */ + public SubHyperplane<U> getPlus() { + return plus; + } + + /** Get the part of the sub-hyperplane on the minus side of the splitting hyperplane. + * @return part of the sub-hyperplane on the minus side of the splitting hyperplane + */ + public SubHyperplane<U> getMinus() { + return minus; + } + + /** Get the side of the split sub-hyperplane with respect to its splitter. + * @return {@link Side#PLUS} if only {@link #getPlus()} is neither null nor empty, + * {@link Side#MINUS} if only {@link #getMinus()} is neither null nor empty, + * {@link Side#BOTH} if both {@link #getPlus()} and {@link #getMinus()} + * are neither null nor empty or {@link Side#HYPER} if both {@link #getPlus()} and + * {@link #getMinus()} are either null or empty + * @since 3.6 + */ + public Side getSide() { + if (plus != null && !plus.isEmpty()) { + if (minus != null && !minus.isEmpty()) { + return Side.BOTH; + } else { + return Side.PLUS; + } + } else if (minus != null && !minus.isEmpty()) { + return Side.MINUS; + } else { + return Side.HYPER; + } + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/Transform.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/Transform.java new file mode 100644 index 0000000..ba0c1dd --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/Transform.java @@ -0,0 +1,80 @@ +/* + * 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.partitioning; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; + + +/** This interface represents an inversible affine transform in a space. + * <p>Inversible affine transform include for example scalings, + * translations, rotations.</p> + + * <p>Transforms are dimension-specific. The consistency rules between + * the three {@code apply} methods are the following ones for a + * transformed defined for dimension D:</p> + * <ul> + * <li> + * the transform can be applied to a point in the + * D-dimension space using its {@link #apply(Point)} + * method + * </li> + * <li> + * the transform can be applied to a (D-1)-dimension + * hyperplane in the D-dimension space using its + * {@link #apply(Hyperplane)} method + * </li> + * <li> + * the transform can be applied to a (D-2)-dimension + * sub-hyperplane in a (D-1)-dimension hyperplane using + * its {@link #apply(SubHyperplane, Hyperplane, Hyperplane)} + * method + * </li> + * </ul> + + * @param <S> Type of the embedding space. + * @param <T> Type of the embedded sub-space. + + * @since 3.0 + */ +public interface Transform<S extends Space, T extends Space> { + + /** Transform a point of a space. + * @param point point to transform + * @return a new object representing the transformed point + */ + Point<S> apply(Point<S> point); + + /** Transform an hyperplane of a space. + * @param hyperplane hyperplane to transform + * @return a new object representing the transformed hyperplane + */ + Hyperplane<S> apply(Hyperplane<S> hyperplane); + + /** Transform a sub-hyperplane embedded in an hyperplane. + * @param sub sub-hyperplane to transform + * @param original hyperplane in which the sub-hyperplane is + * defined (this is the original hyperplane, the transform has + * <em>not</em> been applied to it) + * @param transformed hyperplane in which the sub-hyperplane is + * defined (this is the transformed hyperplane, the transform + * <em>has</em> been applied to it) + * @return a new object representing the transformed sub-hyperplane + */ + SubHyperplane<T> apply(SubHyperplane<T> sub, Hyperplane<S> original, Hyperplane<S> transformed); + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/package-info.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/package-info.java new file mode 100644 index 0000000..6e63c73 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/package-info.java @@ -0,0 +1,114 @@ +/* + * 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. + */ +/** + * + * This package provides classes to implement Binary Space Partition trees. + * + * <p> + * {@link org.apache.commons.math3.geometry.partitioning.BSPTree BSP trees} + * are an efficient way to represent parts of space and in particular + * polytopes (line segments in 1D, polygons in 2D and polyhedrons in 3D) + * and to operate on them. The main principle is to recursively subdivide + * the space using simple hyperplanes (points in 1D, lines in 2D, planes + * in 3D). + * </p> + * + * <p> + * We start with a tree composed of a single node without any cut + * hyperplane: it represents the complete space, which is a convex + * part. If we add a cut hyperplane to this node, this represents a + * partition with the hyperplane at the node level and two half spaces at + * each side of the cut hyperplane. These half-spaces are represented by + * two child nodes without any cut hyperplanes associated, the plus child + * which represents the half space on the plus side of the cut hyperplane + * and the minus child on the other side. Continuing the subdivisions, we + * end up with a tree having internal nodes that are associated with a + * cut hyperplane and leaf nodes without any hyperplane which correspond + * to convex parts. + * </p> + * + * <p> + * When BSP trees are used to represent polytopes, the convex parts are + * known to be completely inside or outside the polytope as long as there + * is no facet in the part (which is obviously the case if the cut + * hyperplanes have been chosen as the underlying hyperplanes of the + * facets (this is called an autopartition) and if the subdivision + * process has been continued until all facets have been processed. It is + * important to note that the polytope is <em>not</em> defined by a + * single part, but by several convex ones. This is the property that + * allows BSP-trees to represent non-convex polytopes despites all parts + * are convex. The {@link + * org.apache.commons.math3.geometry.partitioning.Region Region} class is + * devoted to this representation, it is build on top of the {@link + * org.apache.commons.math3.geometry.partitioning.BSPTree BSPTree} class using + * boolean objects as the leaf nodes attributes to represent the + * inside/outside property of each leaf part, and also adds various + * methods dealing with boundaries (i.e. the separation between the + * inside and the outside parts). + * </p> + * + * <p> + * Rather than simply associating the internal nodes with an hyperplane, + * we consider <em>sub-hyperplanes</em> which correspond to the part of + * the hyperplane that is inside the convex part defined by all the + * parent nodes (this implies that the sub-hyperplane at root node is in + * fact a complete hyperplane, because there is no parent to bound + * it). Since the parts are convex, the sub-hyperplanes are convex, in + * 3D the convex parts are convex polyhedrons, and the sub-hyperplanes + * are convex polygons that cut these polyhedrons in two + * sub-polyhedrons. Using this definition, a BSP tree completely + * partitions the space. Each point either belongs to one of the + * sub-hyperplanes in an internal node or belongs to one of the leaf + * convex parts. + * </p> + * + * <p> + * In order to determine where a point is, it is sufficient to check its + * position with respect to the root cut hyperplane, to select the + * corresponding child tree and to repeat the procedure recursively, + * until either the point appears to be exactly on one of the hyperplanes + * in the middle of the tree or to be in one of the leaf parts. For + * this operation, it is sufficient to consider the complete hyperplanes, + * there is no need to check the points with the boundary of the + * sub-hyperplanes, because this check has in fact already been realized + * by the recursive descent in the tree. This is very easy to do and very + * efficient, especially if the tree is well balanced (the cost is + * <code>O(log(n))</code> where <code>n</code> is the number of facets) + * or if the first tree levels close to the root discriminate large parts + * of the total space. + * </p> + * + * <p> + * One of the main sources for the development of this package was Bruce + * Naylor, John Amanatides and William Thibault paper <a + * href="http://www.cs.yorku.ca/~amana/research/bsptSetOp.pdf">Merging + * BSP Trees Yields Polyhedral Set Operations</a> Proc. Siggraph '90, + * Computer Graphics 24(4), August 1990, pp 115-124, published by the + * Association for Computing Machinery (ACM). The same paper can also be + * found <a + * href="http://www.cs.utexas.edu/users/fussell/courses/cs384g/bsp_treemerge.pdf">here</a>. + * </p> + * + * <p> + * Note that the interfaces defined in this package are <em>not</em> intended to + * be implemented by Apache Commons Math users, they are only intended to be + * implemented within the library itself. New methods may be added even for + * minor versions, which breaks compatibility for external implementations. + * </p> + * + */ +package org.apache.commons.math3.geometry.partitioning; diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/AVLTree.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/AVLTree.java new file mode 100644 index 0000000..00c9d3e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/AVLTree.java @@ -0,0 +1,634 @@ +/* + * 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.partitioning.utilities; + +/** This class implements AVL trees. + * + * <p>The purpose of this class is to sort elements while allowing + * duplicate elements (i.e. such that {@code a.equals(b)} is + * true). The {@code SortedSet} interface does not allow this, so + * a specific class is needed. Null elements are not allowed.</p> + * + * <p>Since the {@code equals} method is not sufficient to + * differentiate elements, the {@link #delete delete} method is + * implemented using the equality operator.</p> + * + * <p>In order to clearly mark the methods provided here do not have + * the same semantics as the ones specified in the + * {@code SortedSet} interface, different names are used + * ({@code add} has been replaced by {@link #insert insert} and + * {@code remove} has been replaced by {@link #delete + * delete}).</p> + * + * <p>This class is based on the C implementation Georg Kraml has put + * in the public domain. Unfortunately, his <a + * href="www.purists.org/georg/avltree/index.html">page</a> seems not + * to exist any more.</p> + * + * @param <T> the type of the elements + * + * @since 3.0 + * @deprecated as of 3.4, this class is not used anymore and considered + * to be out of scope of Apache Commons Math + */ +@Deprecated +public class AVLTree<T extends Comparable<T>> { + + /** Top level node. */ + private Node top; + + /** Build an empty tree. + */ + public AVLTree() { + top = null; + } + + /** Insert an element in the tree. + * @param element element to insert (silently ignored if null) + */ + public void insert(final T element) { + if (element != null) { + if (top == null) { + top = new Node(element, null); + } else { + top.insert(element); + } + } + } + + /** Delete an element from the tree. + * <p>The element is deleted only if there is a node {@code n} + * containing exactly the element instance specified, i.e. for which + * {@code n.getElement() == element}. This is purposely + * <em>different</em> from the specification of the + * {@code java.util.Set} {@code remove} method (in fact, + * this is the reason why a specific class has been developed).</p> + * @param element element to delete (silently ignored if null) + * @return true if the element was deleted from the tree + */ + public boolean delete(final T element) { + if (element != null) { + for (Node node = getNotSmaller(element); node != null; node = node.getNext()) { + // loop over all elements neither smaller nor larger + // than the specified one + if (node.element == element) { + node.delete(); + return true; + } else if (node.element.compareTo(element) > 0) { + // all the remaining elements are known to be larger, + // the element is not in the tree + return false; + } + } + } + return false; + } + + /** Check if the tree is empty. + * @return true if the tree is empty + */ + public boolean isEmpty() { + return top == null; + } + + + /** Get the number of elements of the tree. + * @return number of elements contained in the tree + */ + public int size() { + return (top == null) ? 0 : top.size(); + } + + /** Get the node whose element is the smallest one in the tree. + * @return the tree node containing the smallest element in the tree + * or null if the tree is empty + * @see #getLargest + * @see #getNotSmaller + * @see #getNotLarger + * @see Node#getPrevious + * @see Node#getNext + */ + public Node getSmallest() { + return (top == null) ? null : top.getSmallest(); + } + + /** Get the node whose element is the largest one in the tree. + * @return the tree node containing the largest element in the tree + * or null if the tree is empty + * @see #getSmallest + * @see #getNotSmaller + * @see #getNotLarger + * @see Node#getPrevious + * @see Node#getNext + */ + public Node getLargest() { + return (top == null) ? null : top.getLargest(); + } + + /** Get the node whose element is not smaller than the reference object. + * @param reference reference object (may not be in the tree) + * @return the tree node containing the smallest element not smaller + * than the reference object or null if either the tree is empty or + * all its elements are smaller than the reference object + * @see #getSmallest + * @see #getLargest + * @see #getNotLarger + * @see Node#getPrevious + * @see Node#getNext + */ + public Node getNotSmaller(final T reference) { + Node candidate = null; + for (Node node = top; node != null;) { + if (node.element.compareTo(reference) < 0) { + if (node.right == null) { + return candidate; + } + node = node.right; + } else { + candidate = node; + if (node.left == null) { + return candidate; + } + node = node.left; + } + } + return null; + } + + /** Get the node whose element is not larger than the reference object. + * @param reference reference object (may not be in the tree) + * @return the tree node containing the largest element not larger + * than the reference object (in which case the node is guaranteed + * not to be empty) or null if either the tree is empty or all its + * elements are larger than the reference object + * @see #getSmallest + * @see #getLargest + * @see #getNotSmaller + * @see Node#getPrevious + * @see Node#getNext + */ + public Node getNotLarger(final T reference) { + Node candidate = null; + for (Node node = top; node != null;) { + if (node.element.compareTo(reference) > 0) { + if (node.left == null) { + return candidate; + } + node = node.left; + } else { + candidate = node; + if (node.right == null) { + return candidate; + } + node = node.right; + } + } + return null; + } + + /** Enum for tree skew factor. */ + private enum Skew { + /** Code for left high trees. */ + LEFT_HIGH, + + /** Code for right high trees. */ + RIGHT_HIGH, + + /** Code for Skew.BALANCED trees. */ + BALANCED; + } + + /** This class implements AVL trees nodes. + * <p>AVL tree nodes implement all the logical structure of the + * tree. Nodes are created by the {@link AVLTree AVLTree} class.</p> + * <p>The nodes are not independant from each other but must obey + * specific balancing constraints and the tree structure is + * rearranged as elements are inserted or deleted from the tree. The + * creation, modification and tree-related navigation methods have + * therefore restricted access. Only the order-related navigation, + * reading and delete methods are public.</p> + * @see AVLTree + */ + public class Node { + + /** Element contained in the current node. */ + private T element; + + /** Left sub-tree. */ + private Node left; + + /** Right sub-tree. */ + private Node right; + + /** Parent tree. */ + private Node parent; + + /** Skew factor. */ + private Skew skew; + + /** Build a node for a specified element. + * @param element element + * @param parent parent node + */ + Node(final T element, final Node parent) { + this.element = element; + left = null; + right = null; + this.parent = parent; + skew = Skew.BALANCED; + } + + /** Get the contained element. + * @return element contained in the node + */ + public T getElement() { + return element; + } + + /** Get the number of elements of the tree rooted at this node. + * @return number of elements contained in the tree rooted at this node + */ + int size() { + return 1 + ((left == null) ? 0 : left.size()) + ((right == null) ? 0 : right.size()); + } + + /** Get the node whose element is the smallest one in the tree + * rooted at this node. + * @return the tree node containing the smallest element in the + * tree rooted at this node or null if the tree is empty + * @see #getLargest + */ + Node getSmallest() { + Node node = this; + while (node.left != null) { + node = node.left; + } + return node; + } + + /** Get the node whose element is the largest one in the tree + * rooted at this node. + * @return the tree node containing the largest element in the + * tree rooted at this node or null if the tree is empty + * @see #getSmallest + */ + Node getLargest() { + Node node = this; + while (node.right != null) { + node = node.right; + } + return node; + } + + /** Get the node containing the next smaller or equal element. + * @return node containing the next smaller or equal element or + * null if there is no smaller or equal element in the tree + * @see #getNext + */ + public Node getPrevious() { + + if (left != null) { + final Node node = left.getLargest(); + if (node != null) { + return node; + } + } + + for (Node node = this; node.parent != null; node = node.parent) { + if (node != node.parent.left) { + return node.parent; + } + } + + return null; + + } + + /** Get the node containing the next larger or equal element. + * @return node containing the next larger or equal element (in + * which case the node is guaranteed not to be empty) or null if + * there is no larger or equal element in the tree + * @see #getPrevious + */ + public Node getNext() { + + if (right != null) { + final Node node = right.getSmallest(); + if (node != null) { + return node; + } + } + + for (Node node = this; node.parent != null; node = node.parent) { + if (node != node.parent.right) { + return node.parent; + } + } + + return null; + + } + + /** Insert an element in a sub-tree. + * @param newElement element to insert + * @return true if the parent tree should be re-Skew.BALANCED + */ + boolean insert(final T newElement) { + if (newElement.compareTo(this.element) < 0) { + // the inserted element is smaller than the node + if (left == null) { + left = new Node(newElement, this); + return rebalanceLeftGrown(); + } + return left.insert(newElement) ? rebalanceLeftGrown() : false; + } + + // the inserted element is equal to or greater than the node + if (right == null) { + right = new Node(newElement, this); + return rebalanceRightGrown(); + } + return right.insert(newElement) ? rebalanceRightGrown() : false; + + } + + /** Delete the node from the tree. + */ + public void delete() { + if ((parent == null) && (left == null) && (right == null)) { + // this was the last node, the tree is now empty + element = null; + top = null; + } else { + + Node node; + Node child; + boolean leftShrunk; + if ((left == null) && (right == null)) { + node = this; + element = null; + leftShrunk = node == node.parent.left; + child = null; + } else { + node = (left != null) ? left.getLargest() : right.getSmallest(); + element = node.element; + leftShrunk = node == node.parent.left; + child = (node.left != null) ? node.left : node.right; + } + + node = node.parent; + if (leftShrunk) { + node.left = child; + } else { + node.right = child; + } + if (child != null) { + child.parent = node; + } + + while (leftShrunk ? node.rebalanceLeftShrunk() : node.rebalanceRightShrunk()) { + if (node.parent == null) { + return; + } + leftShrunk = node == node.parent.left; + node = node.parent; + } + + } + } + + /** Re-balance the instance as left sub-tree has grown. + * @return true if the parent tree should be reSkew.BALANCED too + */ + private boolean rebalanceLeftGrown() { + switch (skew) { + case LEFT_HIGH: + if (left.skew == Skew.LEFT_HIGH) { + rotateCW(); + skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + } else { + final Skew s = left.right.skew; + left.rotateCCW(); + rotateCW(); + switch(s) { + case LEFT_HIGH: + left.skew = Skew.BALANCED; + right.skew = Skew.RIGHT_HIGH; + break; + case RIGHT_HIGH: + left.skew = Skew.LEFT_HIGH; + right.skew = Skew.BALANCED; + break; + default: + left.skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + } + skew = Skew.BALANCED; + } + return false; + case RIGHT_HIGH: + skew = Skew.BALANCED; + return false; + default: + skew = Skew.LEFT_HIGH; + return true; + } + } + + /** Re-balance the instance as right sub-tree has grown. + * @return true if the parent tree should be reSkew.BALANCED too + */ + private boolean rebalanceRightGrown() { + switch (skew) { + case LEFT_HIGH: + skew = Skew.BALANCED; + return false; + case RIGHT_HIGH: + if (right.skew == Skew.RIGHT_HIGH) { + rotateCCW(); + skew = Skew.BALANCED; + left.skew = Skew.BALANCED; + } else { + final Skew s = right.left.skew; + right.rotateCW(); + rotateCCW(); + switch (s) { + case LEFT_HIGH: + left.skew = Skew.BALANCED; + right.skew = Skew.RIGHT_HIGH; + break; + case RIGHT_HIGH: + left.skew = Skew.LEFT_HIGH; + right.skew = Skew.BALANCED; + break; + default: + left.skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + } + skew = Skew.BALANCED; + } + return false; + default: + skew = Skew.RIGHT_HIGH; + return true; + } + } + + /** Re-balance the instance as left sub-tree has shrunk. + * @return true if the parent tree should be reSkew.BALANCED too + */ + private boolean rebalanceLeftShrunk() { + switch (skew) { + case LEFT_HIGH: + skew = Skew.BALANCED; + return true; + case RIGHT_HIGH: + if (right.skew == Skew.RIGHT_HIGH) { + rotateCCW(); + skew = Skew.BALANCED; + left.skew = Skew.BALANCED; + return true; + } else if (right.skew == Skew.BALANCED) { + rotateCCW(); + skew = Skew.LEFT_HIGH; + left.skew = Skew.RIGHT_HIGH; + return false; + } else { + final Skew s = right.left.skew; + right.rotateCW(); + rotateCCW(); + switch (s) { + case LEFT_HIGH: + left.skew = Skew.BALANCED; + right.skew = Skew.RIGHT_HIGH; + break; + case RIGHT_HIGH: + left.skew = Skew.LEFT_HIGH; + right.skew = Skew.BALANCED; + break; + default: + left.skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + } + skew = Skew.BALANCED; + return true; + } + default: + skew = Skew.RIGHT_HIGH; + return false; + } + } + + /** Re-balance the instance as right sub-tree has shrunk. + * @return true if the parent tree should be reSkew.BALANCED too + */ + private boolean rebalanceRightShrunk() { + switch (skew) { + case RIGHT_HIGH: + skew = Skew.BALANCED; + return true; + case LEFT_HIGH: + if (left.skew == Skew.LEFT_HIGH) { + rotateCW(); + skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + return true; + } else if (left.skew == Skew.BALANCED) { + rotateCW(); + skew = Skew.RIGHT_HIGH; + right.skew = Skew.LEFT_HIGH; + return false; + } else { + final Skew s = left.right.skew; + left.rotateCCW(); + rotateCW(); + switch (s) { + case LEFT_HIGH: + left.skew = Skew.BALANCED; + right.skew = Skew.RIGHT_HIGH; + break; + case RIGHT_HIGH: + left.skew = Skew.LEFT_HIGH; + right.skew = Skew.BALANCED; + break; + default: + left.skew = Skew.BALANCED; + right.skew = Skew.BALANCED; + } + skew = Skew.BALANCED; + return true; + } + default: + skew = Skew.LEFT_HIGH; + return false; + } + } + + /** Perform a clockwise rotation rooted at the instance. + * <p>The skew factor are not updated by this method, they + * <em>must</em> be updated by the caller</p> + */ + private void rotateCW() { + + final T tmpElt = element; + element = left.element; + left.element = tmpElt; + + final Node tmpNode = left; + left = tmpNode.left; + tmpNode.left = tmpNode.right; + tmpNode.right = right; + right = tmpNode; + + if (left != null) { + left.parent = this; + } + if (right.right != null) { + right.right.parent = right; + } + + } + + /** Perform a counter-clockwise rotation rooted at the instance. + * <p>The skew factor are not updated by this method, they + * <em>must</em> be updated by the caller</p> + */ + private void rotateCCW() { + + final T tmpElt = element; + element = right.element; + right.element = tmpElt; + + final Node tmpNode = right; + right = tmpNode.right; + tmpNode.right = tmpNode.left; + tmpNode.left = left; + left = tmpNode; + + if (right != null) { + right.parent = this; + } + if (left.left != null) { + left.left.parent = left; + } + + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/OrderedTuple.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/OrderedTuple.java new file mode 100644 index 0000000..2dad2d7 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/OrderedTuple.java @@ -0,0 +1,431 @@ +/* + * 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.partitioning.utilities; + +import java.util.Arrays; + +import org.apache.commons.math3.util.FastMath; + +/** This class implements an ordering operation for T-uples. + * + * <p>Ordering is done by encoding all components of the T-uple into a + * single scalar value and using this value as the sorting + * key. Encoding is performed using the method invented by Georg + * Cantor in 1877 when he proved it was possible to establish a + * bijection between a line and a plane. The binary representations of + * the components of the T-uple are mixed together to form a single + * scalar. This means that the 2<sup>k</sup> bit of component 0 is + * followed by the 2<sup>k</sup> bit of component 1, then by the + * 2<sup>k</sup> bit of component 2 up to the 2<sup>k</sup> bit of + * component {@code t}, which is followed by the 2<sup>k-1</sup> + * bit of component 0, followed by the 2<sup>k-1</sup> bit of + * component 1 ... The binary representations are extended as needed + * to handle numbers with different scales and a suitable + * 2<sup>p</sup> offset is added to the components in order to avoid + * negative numbers (this offset is adjusted as needed during the + * comparison operations).</p> + * + * <p>The more interesting property of the encoding method for our + * purpose is that it allows to select all the points that are in a + * given range. This is depicted in dimension 2 by the following + * picture:</p> + * + * <img src="doc-files/OrderedTuple.png" /> + * + * <p>This picture shows a set of 100000 random 2-D pairs having their + * first component between -50 and +150 and their second component + * between -350 and +50. We wanted to extract all pairs having their + * first component between +30 and +70 and their second component + * between -120 and -30. We built the lower left point at coordinates + * (30, -120) and the upper right point at coordinates (70, -30). All + * points smaller than the lower left point are drawn in red and all + * points larger than the upper right point are drawn in blue. The + * green points are between the two limits. This picture shows that + * all the desired points are selected, along with spurious points. In + * this case, we get 15790 points, 4420 of which really belonging to + * the desired rectangle. It is possible to extract very small + * subsets. As an example extracting from the same 100000 points set + * the points having their first component between +30 and +31 and + * their second component between -91 and -90, we get a subset of 11 + * points, 2 of which really belonging to the desired rectangle.</p> + * + * <p>the previous selection technique can be applied in all + * dimensions, still using two points to define the interval. The + * first point will have all its components set to their lower bounds + * while the second point will have all its components set to their + * upper bounds.</p> + * + * <p>T-uples with negative infinite or positive infinite components + * are sorted logically.</p> + * + * <p>Since the specification of the {@code Comparator} interface + * allows only {@code ClassCastException} errors, some arbitrary + * choices have been made to handle specific cases. The rationale for + * these choices is to keep <em>regular</em> and consistent T-uples + * together.</p> + * <ul> + * <li>instances with different dimensions are sorted according to + * their dimension regardless of their components values</li> + * <li>instances with {@code Double.NaN} components are sorted + * after all other ones (even after instances with positive infinite + * components</li> + * <li>instances with both positive and negative infinite components + * are considered as if they had {@code Double.NaN} + * components</li> + * </ul> + * + * @since 3.0 + * @deprecated as of 3.4, this class is not used anymore and considered + * to be out of scope of Apache Commons Math + */ +@Deprecated +public class OrderedTuple implements Comparable<OrderedTuple> { + + /** Sign bit mask. */ + private static final long SIGN_MASK = 0x8000000000000000L; + + /** Exponent bits mask. */ + private static final long EXPONENT_MASK = 0x7ff0000000000000L; + + /** Mantissa bits mask. */ + private static final long MANTISSA_MASK = 0x000fffffffffffffL; + + /** Implicit MSB for normalized numbers. */ + private static final long IMPLICIT_ONE = 0x0010000000000000L; + + /** Double components of the T-uple. */ + private double[] components; + + /** Offset scale. */ + private int offset; + + /** Least Significant Bit scale. */ + private int lsb; + + /** Ordering encoding of the double components. */ + private long[] encoding; + + /** Positive infinity marker. */ + private boolean posInf; + + /** Negative infinity marker. */ + private boolean negInf; + + /** Not A Number marker. */ + private boolean nan; + + /** Build an ordered T-uple from its components. + * @param components double components of the T-uple + */ + public OrderedTuple(final double ... components) { + this.components = components.clone(); + int msb = Integer.MIN_VALUE; + lsb = Integer.MAX_VALUE; + posInf = false; + negInf = false; + nan = false; + for (int i = 0; i < components.length; ++i) { + if (Double.isInfinite(components[i])) { + if (components[i] < 0) { + negInf = true; + } else { + posInf = true; + } + } else if (Double.isNaN(components[i])) { + nan = true; + } else { + final long b = Double.doubleToLongBits(components[i]); + final long m = mantissa(b); + if (m != 0) { + final int e = exponent(b); + msb = FastMath.max(msb, e + computeMSB(m)); + lsb = FastMath.min(lsb, e + computeLSB(m)); + } + } + } + + if (posInf && negInf) { + // instance cannot be sorted logically + posInf = false; + negInf = false; + nan = true; + } + + if (lsb <= msb) { + // encode the T-upple with the specified offset + encode(msb + 16); + } else { + encoding = new long[] { + 0x0L + }; + } + + } + + /** Encode the T-uple with a given offset. + * @param minOffset minimal scale of the offset to add to all + * components (must be greater than the MSBs of all components) + */ + private void encode(final int minOffset) { + + // choose an offset with some margins + offset = minOffset + 31; + offset -= offset % 32; + + if ((encoding != null) && (encoding.length == 1) && (encoding[0] == 0x0L)) { + // the components are all zeroes + return; + } + + // allocate an integer array to encode the components (we use only + // 63 bits per element because there is no unsigned long in Java) + final int neededBits = offset + 1 - lsb; + final int neededLongs = (neededBits + 62) / 63; + encoding = new long[components.length * neededLongs]; + + // mix the bits from all components + int eIndex = 0; + int shift = 62; + long word = 0x0L; + for (int k = offset; eIndex < encoding.length; --k) { + for (int vIndex = 0; vIndex < components.length; ++vIndex) { + if (getBit(vIndex, k) != 0) { + word |= 0x1L << shift; + } + if (shift-- == 0) { + encoding[eIndex++] = word; + word = 0x0L; + shift = 62; + } + } + } + + } + + /** Compares this ordered T-uple with the specified object. + + * <p>The ordering method is detailed in the general description of + * the class. Its main property is to be consistent with distance: + * geometrically close T-uples stay close to each other when stored + * in a sorted collection using this comparison method.</p> + + * <p>T-uples with negative infinite, positive infinite are sorted + * logically.</p> + + * <p>Some arbitrary choices have been made to handle specific + * cases. The rationale for these choices is to keep + * <em>normal</em> and consistent T-uples together.</p> + * <ul> + * <li>instances with different dimensions are sorted according to + * their dimension regardless of their components values</li> + * <li>instances with {@code Double.NaN} components are sorted + * after all other ones (evan after instances with positive infinite + * components</li> + * <li>instances with both positive and negative infinite components + * are considered as if they had {@code Double.NaN} + * components</li> + * </ul> + + * @param ot T-uple to compare instance with + * @return a negative integer if the instance is less than the + * object, zero if they are equal, or a positive integer if the + * instance is greater than the object + + */ + public int compareTo(final OrderedTuple ot) { + if (components.length == ot.components.length) { + if (nan) { + return +1; + } else if (ot.nan) { + return -1; + } else if (negInf || ot.posInf) { + return -1; + } else if (posInf || ot.negInf) { + return +1; + } else { + + if (offset < ot.offset) { + encode(ot.offset); + } else if (offset > ot.offset) { + ot.encode(offset); + } + + final int limit = FastMath.min(encoding.length, ot.encoding.length); + for (int i = 0; i < limit; ++i) { + if (encoding[i] < ot.encoding[i]) { + return -1; + } else if (encoding[i] > ot.encoding[i]) { + return +1; + } + } + + if (encoding.length < ot.encoding.length) { + return -1; + } else if (encoding.length > ot.encoding.length) { + return +1; + } else { + return 0; + } + + } + } + + return components.length - ot.components.length; + + } + + /** {@inheritDoc} */ + @Override + public boolean equals(final Object other) { + if (this == other) { + return true; + } else if (other instanceof OrderedTuple) { + return compareTo((OrderedTuple) other) == 0; + } else { + return false; + } + } + + /** {@inheritDoc} */ + @Override + public int hashCode() { + // the following constants are arbitrary small primes + final int multiplier = 37; + final int trueHash = 97; + final int falseHash = 71; + + // hash fields and combine them + // (we rely on the multiplier to have different combined weights + // for all int fields and all boolean fields) + int hash = Arrays.hashCode(components); + hash = hash * multiplier + offset; + hash = hash * multiplier + lsb; + hash = hash * multiplier + (posInf ? trueHash : falseHash); + hash = hash * multiplier + (negInf ? trueHash : falseHash); + hash = hash * multiplier + (nan ? trueHash : falseHash); + + return hash; + + } + + /** Get the components array. + * @return array containing the T-uple components + */ + public double[] getComponents() { + return components.clone(); + } + + /** Extract the sign from the bits of a double. + * @param bits binary representation of the double + * @return sign bit (zero if positive, non zero if negative) + */ + private static long sign(final long bits) { + return bits & SIGN_MASK; + } + + /** Extract the exponent from the bits of a double. + * @param bits binary representation of the double + * @return exponent + */ + private static int exponent(final long bits) { + return ((int) ((bits & EXPONENT_MASK) >> 52)) - 1075; + } + + /** Extract the mantissa from the bits of a double. + * @param bits binary representation of the double + * @return mantissa + */ + private static long mantissa(final long bits) { + return ((bits & EXPONENT_MASK) == 0) ? + ((bits & MANTISSA_MASK) << 1) : // subnormal number + (IMPLICIT_ONE | (bits & MANTISSA_MASK)); // normal number + } + + /** Compute the most significant bit of a long. + * @param l long from which the most significant bit is requested + * @return scale of the most significant bit of {@code l}, + * or 0 if {@code l} is zero + * @see #computeLSB + */ + private static int computeMSB(final long l) { + + long ll = l; + long mask = 0xffffffffL; + int scale = 32; + int msb = 0; + + while (scale != 0) { + if ((ll & mask) != ll) { + msb |= scale; + ll >>= scale; + } + scale >>= 1; + mask >>= scale; + } + + return msb; + + } + + /** Compute the least significant bit of a long. + * @param l long from which the least significant bit is requested + * @return scale of the least significant bit of {@code l}, + * or 63 if {@code l} is zero + * @see #computeMSB + */ + private static int computeLSB(final long l) { + + long ll = l; + long mask = 0xffffffff00000000L; + int scale = 32; + int lsb = 0; + + while (scale != 0) { + if ((ll & mask) == ll) { + lsb |= scale; + ll >>= scale; + } + scale >>= 1; + mask >>= scale; + } + + return lsb; + + } + + /** Get a bit from the mantissa of a double. + * @param i index of the component + * @param k scale of the requested bit + * @return the specified bit (either 0 or 1), after the offset has + * been added to the double + */ + private int getBit(final int i, final int k) { + final long bits = Double.doubleToLongBits(components[i]); + final int e = exponent(bits); + if ((k < e) || (k > offset)) { + return 0; + } else if (k == offset) { + return (sign(bits) == 0L) ? 1 : 0; + } else if (k > (e + 52)) { + return (sign(bits) == 0L) ? 0 : 1; + } else { + final long m = (sign(bits) == 0L) ? mantissa(bits) : -mantissa(bits); + return (int) ((m >> (k - e)) & 0x1L); + } + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/doc-files/OrderedTuple.png b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/doc-files/OrderedTuple.png Binary files differnew file mode 100644 index 0000000..4eca233 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/doc-files/OrderedTuple.png diff --git a/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/package-info.java b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/package-info.java new file mode 100644 index 0000000..31f57f1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/partitioning/utilities/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 multidimensional ordering features for partitioning. + * </p> + * + */ +package org.apache.commons.math3.geometry.partitioning.utilities; diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Arc.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Arc.java new file mode 100644 index 0000000..af0388e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Arc.java @@ -0,0 +1,132 @@ +/* + * 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.spherical.oned; + +import org.apache.commons.math3.exception.NumberIsTooLargeException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.partitioning.Region.Location; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; +import org.apache.commons.math3.util.Precision; + + +/** This class represents an arc on a circle. + * @see ArcsSet + * @since 3.3 + */ +public class Arc { + + /** The lower angular bound of the arc. */ + private final double lower; + + /** The upper angular bound of the arc. */ + private final double upper; + + /** Middle point of the arc. */ + private final double middle; + + /** Tolerance below which angles are considered identical. */ + private final double tolerance; + + /** Simple constructor. + * <p> + * If either {@code lower} is equals to {@code upper} or + * the interval exceeds \( 2 \pi \), the arc is considered + * to be the full circle and its initial defining boundaries + * will be forgotten. {@code lower} is not allowed to be + * greater than {@code upper} (an exception is thrown in this case). + * {@code lower} will be canonicalized between 0 and \( 2 \pi \), and + * upper shifted accordingly, so the {@link #getInf()} and {@link #getSup()} + * may not return the value used at instance construction. + * </p> + * @param lower lower angular bound of the arc + * @param upper upper angular bound of the arc + * @param tolerance tolerance below which angles are considered identical + * @exception NumberIsTooLargeException if lower is greater than upper + */ + public Arc(final double lower, final double upper, final double tolerance) + throws NumberIsTooLargeException { + this.tolerance = tolerance; + if (Precision.equals(lower, upper, 0) || (upper - lower) >= MathUtils.TWO_PI) { + // the arc must cover the whole circle + this.lower = 0; + this.upper = MathUtils.TWO_PI; + this.middle = FastMath.PI; + } else if (lower <= upper) { + this.lower = MathUtils.normalizeAngle(lower, FastMath.PI); + this.upper = this.lower + (upper - lower); + this.middle = 0.5 * (this.lower + this.upper); + } else { + throw new NumberIsTooLargeException(LocalizedFormats.ENDPOINTS_NOT_AN_INTERVAL, + lower, upper, true); + } + } + + /** Get the lower angular bound of the arc. + * @return lower angular bound of the arc, + * always between 0 and \( 2 \pi \) + */ + public double getInf() { + return lower; + } + + /** Get the upper angular bound of the arc. + * @return upper angular bound of the arc, + * always between {@link #getInf()} and {@link #getInf()} \( + 2 \pi \) + */ + public double getSup() { + return upper; + } + + /** Get the angular size of the arc. + * @return angular size of the arc + */ + public double getSize() { + return upper - lower; + } + + /** Get the barycenter of the arc. + * @return barycenter of the arc + */ + public double getBarycenter() { + return middle; + } + + /** Get the tolerance below which angles are considered identical. + * @return tolerance below which angles are considered identical + */ + public double getTolerance() { + return tolerance; + } + + /** Check a point with respect to the arc. + * @param point point to check + * @return a code representing the point status: either {@link + * Location#INSIDE}, {@link Location#OUTSIDE} or {@link Location#BOUNDARY} + */ + public Location checkPoint(final double point) { + final double normalizedPoint = MathUtils.normalizeAngle(point, middle); + if (normalizedPoint < lower - tolerance || normalizedPoint > upper + tolerance) { + return Location.OUTSIDE; + } else if (normalizedPoint > lower + tolerance && normalizedPoint < upper - tolerance) { + return Location.INSIDE; + } else { + return (getSize() >= MathUtils.TWO_PI - tolerance) ? Location.INSIDE : Location.BOUNDARY; + } + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/ArcsSet.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/ArcsSet.java new file mode 100644 index 0000000..0a00aa7 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/ArcsSet.java @@ -0,0 +1,955 @@ +/* + * 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.spherical.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.exception.MathIllegalArgumentException; +import org.apache.commons.math3.exception.MathInternalError; +import org.apache.commons.math3.exception.NumberIsTooLargeException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +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.Side; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; +import org.apache.commons.math3.util.Precision; + +/** This class represents a region of a circle: a set of arcs. + * <p> + * Note that due to the wrapping around \(2 \pi\), barycenter is + * ill-defined here. It was defined only in order to fulfill + * the requirements of the {@link + * org.apache.commons.math3.geometry.partitioning.Region Region} + * interface, but its use is discouraged. + * </p> + * @since 3.3 + */ +public class ArcsSet extends AbstractRegion<Sphere1D, Sphere1D> implements Iterable<double[]> { + + /** Build an arcs set representing the whole circle. + * @param tolerance tolerance below which close sub-arcs are merged together + */ + public ArcsSet(final double tolerance) { + super(tolerance); + } + + /** Build an arcs set corresponding to a single arc. + * <p> + * If either {@code lower} is equals to {@code upper} or + * the interval exceeds \( 2 \pi \), the arc is considered + * to be the full circle and its initial defining boundaries + * will be forgotten. {@code lower} is not allowed to be greater + * than {@code upper} (an exception is thrown in this case). + * </p> + * @param lower lower bound of the arc + * @param upper upper bound of the arc + * @param tolerance tolerance below which close sub-arcs are merged together + * @exception NumberIsTooLargeException if lower is greater than upper + */ + public ArcsSet(final double lower, final double upper, final double tolerance) + throws NumberIsTooLargeException { + super(buildTree(lower, upper, tolerance), tolerance); + } + + /** Build an arcs 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 arcs set + * @param tolerance tolerance below which close sub-arcs are merged together + * @exception InconsistentStateAt2PiWrapping if the tree leaf nodes are not + * consistent across the \( 0, 2 \pi \) crossing + */ + public ArcsSet(final BSPTree<Sphere1D> tree, final double tolerance) + throws InconsistentStateAt2PiWrapping { + super(tree, tolerance); + check2PiConsistency(); + } + + /** Build an arcs 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 close sub-arcs are merged together + * @exception InconsistentStateAt2PiWrapping if the tree leaf nodes are not + * consistent across the \( 0, 2 \pi \) crossing + */ + public ArcsSet(final Collection<SubHyperplane<Sphere1D>> boundary, final double tolerance) + throws InconsistentStateAt2PiWrapping { + super(boundary, tolerance); + check2PiConsistency(); + } + + /** Build an inside/outside tree representing a single arc. + * @param lower lower angular bound of the arc + * @param upper upper angular bound of the arc + * @param tolerance tolerance below which close sub-arcs are merged together + * @return the built tree + * @exception NumberIsTooLargeException if lower is greater than upper + */ + private static BSPTree<Sphere1D> buildTree(final double lower, final double upper, + final double tolerance) + throws NumberIsTooLargeException { + + if (Precision.equals(lower, upper, 0) || (upper - lower) >= MathUtils.TWO_PI) { + // the tree must cover the whole circle + return new BSPTree<Sphere1D>(Boolean.TRUE); + } else if (lower > upper) { + throw new NumberIsTooLargeException(LocalizedFormats.ENDPOINTS_NOT_AN_INTERVAL, + lower, upper, true); + } + + // this is a regular arc, covering only part of the circle + final double normalizedLower = MathUtils.normalizeAngle(lower, FastMath.PI); + final double normalizedUpper = normalizedLower + (upper - lower); + final SubHyperplane<Sphere1D> lowerCut = + new LimitAngle(new S1Point(normalizedLower), false, tolerance).wholeHyperplane(); + + if (normalizedUpper <= MathUtils.TWO_PI) { + // simple arc starting after 0 and ending before 2 \pi + final SubHyperplane<Sphere1D> upperCut = + new LimitAngle(new S1Point(normalizedUpper), true, tolerance).wholeHyperplane(); + return new BSPTree<Sphere1D>(lowerCut, + new BSPTree<Sphere1D>(Boolean.FALSE), + new BSPTree<Sphere1D>(upperCut, + new BSPTree<Sphere1D>(Boolean.FALSE), + new BSPTree<Sphere1D>(Boolean.TRUE), + null), + null); + } else { + // arc wrapping around 2 \pi + final SubHyperplane<Sphere1D> upperCut = + new LimitAngle(new S1Point(normalizedUpper - MathUtils.TWO_PI), true, tolerance).wholeHyperplane(); + return new BSPTree<Sphere1D>(lowerCut, + new BSPTree<Sphere1D>(upperCut, + new BSPTree<Sphere1D>(Boolean.FALSE), + new BSPTree<Sphere1D>(Boolean.TRUE), + null), + new BSPTree<Sphere1D>(Boolean.TRUE), + null); + } + + } + + /** Check consistency. + * @exception InconsistentStateAt2PiWrapping if the tree leaf nodes are not + * consistent across the \( 0, 2 \pi \) crossing + */ + private void check2PiConsistency() throws InconsistentStateAt2PiWrapping { + + // start search at the tree root + BSPTree<Sphere1D> root = getTree(false); + if (root.getCut() == null) { + return; + } + + // find the inside/outside state before the smallest internal node + final Boolean stateBefore = (Boolean) getFirstLeaf(root).getAttribute(); + + // find the inside/outside state after the largest internal node + final Boolean stateAfter = (Boolean) getLastLeaf(root).getAttribute(); + + if (stateBefore ^ stateAfter) { + throw new InconsistentStateAt2PiWrapping(); + } + + } + + /** Get the first leaf node of a tree. + * @param root tree root + * @return first leaf node (i.e. node corresponding to the region just after 0.0 radians) + */ + private BSPTree<Sphere1D> getFirstLeaf(final BSPTree<Sphere1D> root) { + + if (root.getCut() == null) { + return root; + } + + // find the smallest internal node + BSPTree<Sphere1D> smallest = null; + for (BSPTree<Sphere1D> n = root; n != null; n = previousInternalNode(n)) { + smallest = n; + } + + return leafBefore(smallest); + + } + + /** Get the last leaf node of a tree. + * @param root tree root + * @return last leaf node (i.e. node corresponding to the region just before \( 2 \pi \) radians) + */ + private BSPTree<Sphere1D> getLastLeaf(final BSPTree<Sphere1D> root) { + + if (root.getCut() == null) { + return root; + } + + // find the largest internal node + BSPTree<Sphere1D> largest = null; + for (BSPTree<Sphere1D> n = root; n != null; n = nextInternalNode(n)) { + largest = n; + } + + return leafAfter(largest); + + } + + /** Get the node corresponding to the first arc start. + * @return smallest internal node (i.e. first after 0.0 radians, in trigonometric direction), + * or null if there are no internal nodes (i.e. the set is either empty or covers the full circle) + */ + private BSPTree<Sphere1D> getFirstArcStart() { + + // start search at the tree root + BSPTree<Sphere1D> 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 arc start + while (node != null && !isArcStart(node)) { + node = nextInternalNode(node); + } + + return node; + + } + + /** Check if an internal node corresponds to the start angle of an arc. + * @param node internal node to check + * @return true if the node corresponds to the start angle of an arc + */ + private boolean isArcStart(final BSPTree<Sphere1D> node) { + + if ((Boolean) leafBefore(node).getAttribute()) { + // it has an inside cell before it, it may end an arc 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 arcs + return false; + } + + // the cell has an outside before and an inside after it + // it is the start of an arc + return true; + + } + + /** Check if an internal node corresponds to the end angle of an arc. + * @param node internal node to check + * @return true if the node corresponds to the end angle of an arc + */ + private boolean isArcEnd(final BSPTree<Sphere1D> node) { + + if (!(Boolean) leafBefore(node).getAttribute()) { + // it has an outside cell before it, it may start an arc 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 arc + return false; + } + + // the cell has an inside before and an outside after it + // it is the end of an arc + return true; + + } + + /** Get the next internal node. + * @param node current internal node + * @return next internal node in trigonometric order, or null + * if this is the last internal node + */ + private BSPTree<Sphere1D> nextInternalNode(BSPTree<Sphere1D> 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 trigonometric order, or null + * if this is the first internal node + */ + private BSPTree<Sphere1D> previousInternalNode(BSPTree<Sphere1D> 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<Sphere1D> leafBefore(BSPTree<Sphere1D> 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<Sphere1D> leafAfter(BSPTree<Sphere1D> node) { + + node = childAfter(node); + while (node.getCut() != null) { + node = childBefore(node); + } + + return node; + + } + + /** Check if a node is the child before its parent in trigonometric order. + * @param node child node considered + * @return true is the node has a parent end is before it in trigonometric order + */ + private boolean isBeforeParent(final BSPTree<Sphere1D> node) { + final BSPTree<Sphere1D> parent = node.getParent(); + if (parent == null) { + return false; + } else { + return node == childBefore(parent); + } + } + + /** Check if a node is the child after its parent in trigonometric order. + * @param node child node considered + * @return true is the node has a parent end is after it in trigonometric order + */ + private boolean isAfterParent(final BSPTree<Sphere1D> node) { + final BSPTree<Sphere1D> 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<Sphere1D> childBefore(BSPTree<Sphere1D> node) { + if (isDirect(node)) { + // smaller angles are on minus side, larger angles are on plus side + return node.getMinus(); + } else { + // smaller angles are on plus side, larger angles 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<Sphere1D> childAfter(BSPTree<Sphere1D> node) { + if (isDirect(node)) { + // smaller angles are on minus side, larger angles are on plus side + return node.getPlus(); + } else { + // smaller angles are on plus side, larger angles are on minus side + return node.getMinus(); + } + } + + /** Check if an internal node has a direct limit angle. + * @param node internal node to check + * @return true if the limit angle is direct + */ + private boolean isDirect(final BSPTree<Sphere1D> node) { + return ((LimitAngle) node.getCut().getHyperplane()).isDirect(); + } + + /** Get the limit angle of an internal node. + * @param node internal node to check + * @return limit angle + */ + private double getAngle(final BSPTree<Sphere1D> node) { + return ((LimitAngle) node.getCut().getHyperplane()).getLocation().getAlpha(); + } + + /** {@inheritDoc} */ + @Override + public ArcsSet buildNew(final BSPTree<Sphere1D> tree) { + return new ArcsSet(tree, getTolerance()); + } + + /** {@inheritDoc} */ + @Override + protected void computeGeometricalProperties() { + if (getTree(false).getCut() == null) { + setBarycenter(S1Point.NaN); + setSize(((Boolean) getTree(false).getAttribute()) ? MathUtils.TWO_PI : 0); + } else { + double size = 0.0; + double sum = 0.0; + for (final double[] a : this) { + final double length = a[1] - a[0]; + size += length; + sum += length * (a[0] + a[1]); + } + setSize(size); + if (Precision.equals(size, MathUtils.TWO_PI, 0)) { + setBarycenter(S1Point.NaN); + } else if (size >= Precision.SAFE_MIN) { + setBarycenter(new S1Point(sum / (2 * size))); + } else { + final LimitAngle limit = (LimitAngle) getTree(false).getCut().getHyperplane(); + setBarycenter(limit.getLocation()); + } + } + } + + /** {@inheritDoc} + * @since 3.3 + */ + @Override + public BoundaryProjection<Sphere1D> projectToBoundary(final Point<Sphere1D> point) { + + // get position of test point + final double alpha = ((S1Point) point).getAlpha(); + + boolean wrapFirst = false; + double first = Double.NaN; + double previous = Double.NaN; + for (final double[] a : this) { + + if (Double.isNaN(first)) { + // remember the first angle in case we need it later + first = a[0]; + } + + if (!wrapFirst) { + if (alpha < a[0]) { + // the test point lies between the previous and the current arcs + // offset will be positive + if (Double.isNaN(previous)) { + // we need to wrap around the circle + wrapFirst = true; + } else { + final double previousOffset = alpha - previous; + final double currentOffset = a[0] - alpha; + if (previousOffset < currentOffset) { + return new BoundaryProjection<Sphere1D>(point, new S1Point(previous), previousOffset); + } else { + return new BoundaryProjection<Sphere1D>(point, new S1Point(a[0]), currentOffset); + } + } + } else if (alpha <= a[1]) { + // the test point lies within the current arc + // offset will be negative + final double offset0 = a[0] - alpha; + final double offset1 = alpha - a[1]; + if (offset0 < offset1) { + return new BoundaryProjection<Sphere1D>(point, new S1Point(a[1]), offset1); + } else { + return new BoundaryProjection<Sphere1D>(point, new S1Point(a[0]), offset0); + } + } + } + previous = a[1]; + } + + if (Double.isNaN(previous)) { + + // there are no points at all in the arcs set + return new BoundaryProjection<Sphere1D>(point, null, MathUtils.TWO_PI); + + } else { + + // the test point if before first arc and after last arc, + // somewhere around the 0/2 \pi crossing + if (wrapFirst) { + // the test point is between 0 and first + final double previousOffset = alpha - (previous - MathUtils.TWO_PI); + final double currentOffset = first - alpha; + if (previousOffset < currentOffset) { + return new BoundaryProjection<Sphere1D>(point, new S1Point(previous), previousOffset); + } else { + return new BoundaryProjection<Sphere1D>(point, new S1Point(first), currentOffset); + } + } else { + // the test point is between last and 2\pi + final double previousOffset = alpha - previous; + final double currentOffset = first + MathUtils.TWO_PI - alpha; + if (previousOffset < currentOffset) { + return new BoundaryProjection<Sphere1D>(point, new S1Point(previous), previousOffset); + } else { + return new BoundaryProjection<Sphere1D>(point, new S1Point(first), currentOffset); + } + } + + } + + } + + /** Build an ordered list of arcs representing the instance. + * <p>This method builds this arcs set as an ordered list of + * {@link Arc Arc} elements. An empty tree will build an empty list + * while a tree representing the whole circle will build a one + * element list with bounds set to \( 0 and 2 \pi \).</p> + * @return a new ordered list containing {@link Arc Arc} elements + */ + public List<Arc> asList() { + final List<Arc> list = new ArrayList<Arc>(); + for (final double[] a : this) { + list.add(new Arc(a[0], a[1], getTolerance())); + } + return list; + } + + /** {@inheritDoc} + * <p> + * The iterator returns the limit angles pairs of sub-arcs in trigonometric order. + * </p> + * <p> + * The iterator does <em>not</em> support the optional {@code remove} operation. + * </p> + */ + public Iterator<double[]> iterator() { + return new SubArcsIterator(); + } + + /** Local iterator for sub-arcs. */ + private class SubArcsIterator implements Iterator<double[]> { + + /** Start of the first arc. */ + private final BSPTree<Sphere1D> firstStart; + + /** Current node. */ + private BSPTree<Sphere1D> current; + + /** Sub-arc no yet returned. */ + private double[] pending; + + /** Simple constructor. + */ + SubArcsIterator() { + + firstStart = getFirstArcStart(); + current = firstStart; + + if (firstStart == 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 circle + pending = new double[] { + 0, MathUtils.TWO_PI + }; + } else { + pending = null; + } + } else { + selectPending(); + } + } + + /** Walk the tree to select the pending sub-arc. + */ + private void selectPending() { + + // look for the start of the arc + BSPTree<Sphere1D> start = current; + while (start != null && !isArcStart(start)) { + start = nextInternalNode(start); + } + + if (start == null) { + // we have exhausted the iterator + current = null; + pending = null; + return; + } + + // look for the end of the arc + BSPTree<Sphere1D> end = start; + while (end != null && !isArcEnd(end)) { + end = nextInternalNode(end); + } + + if (end != null) { + + // we have identified the arc + pending = new double[] { + getAngle(start), getAngle(end) + }; + + // prepare search for next arc + current = end; + + } else { + + // the final arc wraps around 2\pi, its end is before the first start + end = firstStart; + while (end != null && !isArcEnd(end)) { + end = previousInternalNode(end); + } + if (end == null) { + // this should never happen + throw new MathInternalError(); + } + + // we have identified the last arc + pending = new double[] { + getAngle(start), getAngle(end) + MathUtils.TWO_PI + }; + + // there won't be any other arcs + 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(); + } + + } + + /** Compute the relative position of the instance with respect + * to an arc. + * <p> + * The {@link Side#MINUS} side of the arc is the one covered by the arc. + * </p> + * @param arc arc to check instance against + * @return one of {@link Side#PLUS}, {@link Side#MINUS}, {@link Side#BOTH} + * or {@link Side#HYPER} + * @deprecated as of 3.6, replaced with {@link #split(Arc)}.{@link Split#getSide()} + */ + @Deprecated + public Side side(final Arc arc) { + return split(arc).getSide(); + } + + /** Split the instance in two parts by an arc. + * @param arc splitting arc + * @return an object containing both the part of the instance + * on the plus side of the arc and the part of the + * instance on the minus side of the arc + */ + public Split split(final Arc arc) { + + final List<Double> minus = new ArrayList<Double>(); + final List<Double> plus = new ArrayList<Double>(); + + final double reference = FastMath.PI + arc.getInf(); + final double arcLength = arc.getSup() - arc.getInf(); + + for (final double[] a : this) { + final double syncedStart = MathUtils.normalizeAngle(a[0], reference) - arc.getInf(); + final double arcOffset = a[0] - syncedStart; + final double syncedEnd = a[1] - arcOffset; + if (syncedStart < arcLength) { + // the start point a[0] is in the minus part of the arc + minus.add(a[0]); + if (syncedEnd > arcLength) { + // the end point a[1] is past the end of the arc + // so we leave the minus part and enter the plus part + final double minusToPlus = arcLength + arcOffset; + minus.add(minusToPlus); + plus.add(minusToPlus); + if (syncedEnd > MathUtils.TWO_PI) { + // in fact the end point a[1] goes far enough that we + // leave the plus part of the arc and enter the minus part again + final double plusToMinus = MathUtils.TWO_PI + arcOffset; + plus.add(plusToMinus); + minus.add(plusToMinus); + minus.add(a[1]); + } else { + // the end point a[1] is in the plus part of the arc + plus.add(a[1]); + } + } else { + // the end point a[1] is in the minus part of the arc + minus.add(a[1]); + } + } else { + // the start point a[0] is in the plus part of the arc + plus.add(a[0]); + if (syncedEnd > MathUtils.TWO_PI) { + // the end point a[1] wraps around to the start of the arc + // so we leave the plus part and enter the minus part + final double plusToMinus = MathUtils.TWO_PI + arcOffset; + plus.add(plusToMinus); + minus.add(plusToMinus); + if (syncedEnd > MathUtils.TWO_PI + arcLength) { + // in fact the end point a[1] goes far enough that we + // leave the minus part of the arc and enter the plus part again + final double minusToPlus = MathUtils.TWO_PI + arcLength + arcOffset; + minus.add(minusToPlus); + plus.add(minusToPlus); + plus.add(a[1]); + } else { + // the end point a[1] is in the minus part of the arc + minus.add(a[1]); + } + } else { + // the end point a[1] is in the plus part of the arc + plus.add(a[1]); + } + } + } + + return new Split(createSplitPart(plus), createSplitPart(minus)); + + } + + /** Add an arc limit to a BSP tree under construction. + * @param tree BSP tree under construction + * @param alpha arc limit + * @param isStart if true, the limit is the start of an arc + */ + private void addArcLimit(final BSPTree<Sphere1D> tree, final double alpha, final boolean isStart) { + + final LimitAngle limit = new LimitAngle(new S1Point(alpha), !isStart, getTolerance()); + final BSPTree<Sphere1D> node = tree.getCell(limit.getLocation(), getTolerance()); + if (node.getCut() != null) { + // this should never happen + throw new MathInternalError(); + } + + node.insertCut(limit); + node.setAttribute(null); + node.getPlus().setAttribute(Boolean.FALSE); + node.getMinus().setAttribute(Boolean.TRUE); + + } + + /** Create a split part. + * <p> + * As per construction, the list of limit angles is known to have + * an even number of entries, with start angles at even indices and + * end angles at odd indices. + * </p> + * @param limits limit angles of the split part + * @return split part (may be null) + */ + private ArcsSet createSplitPart(final List<Double> limits) { + if (limits.isEmpty()) { + return null; + } else { + + // collapse close limit angles + for (int i = 0; i < limits.size(); ++i) { + final int j = (i + 1) % limits.size(); + final double lA = limits.get(i); + final double lB = MathUtils.normalizeAngle(limits.get(j), lA); + if (FastMath.abs(lB - lA) <= getTolerance()) { + // the two limits are too close to each other, we remove both of them + if (j > 0) { + // regular case, the two entries are consecutive ones + limits.remove(j); + limits.remove(i); + i = i - 1; + } else { + // special case, i the the last entry and j is the first entry + // we have wrapped around list end + final double lEnd = limits.remove(limits.size() - 1); + final double lStart = limits.remove(0); + if (limits.isEmpty()) { + // the ends were the only limits, is it a full circle or an empty circle? + if (lEnd - lStart > FastMath.PI) { + // it was full circle + return new ArcsSet(new BSPTree<Sphere1D>(Boolean.TRUE), getTolerance()); + } else { + // it was an empty circle + return null; + } + } else { + // we have removed the first interval start, so our list + // currently starts with an interval end, which is wrong + // we need to move this interval end to the end of the list + limits.add(limits.remove(0) + MathUtils.TWO_PI); + } + } + } + } + + // build the tree by adding all angular sectors + BSPTree<Sphere1D> tree = new BSPTree<Sphere1D>(Boolean.FALSE); + for (int i = 0; i < limits.size() - 1; i += 2) { + addArcLimit(tree, limits.get(i), true); + addArcLimit(tree, limits.get(i + 1), false); + } + + if (tree.getCut() == null) { + // we did not insert anything + return null; + } + + return new ArcsSet(tree, getTolerance()); + + } + } + + /** Class holding the results of the {@link #split split} method. + */ + public static class Split { + + /** Part of the arcs set on the plus side of the splitting arc. */ + private final ArcsSet plus; + + /** Part of the arcs set on the minus side of the splitting arc. */ + private final ArcsSet minus; + + /** Build a Split from its parts. + * @param plus part of the arcs set on the plus side of the + * splitting arc + * @param minus part of the arcs set on the minus side of the + * splitting arc + */ + private Split(final ArcsSet plus, final ArcsSet minus) { + this.plus = plus; + this.minus = minus; + } + + /** Get the part of the arcs set on the plus side of the splitting arc. + * @return part of the arcs set on the plus side of the splitting arc + */ + public ArcsSet getPlus() { + return plus; + } + + /** Get the part of the arcs set on the minus side of the splitting arc. + * @return part of the arcs set on the minus side of the splitting arc + */ + public ArcsSet getMinus() { + return minus; + } + + /** Get the side of the split arc with respect to its splitter. + * @return {@link Side#PLUS} if only {@link #getPlus()} returns non-null, + * {@link Side#MINUS} if only {@link #getMinus()} returns non-null, + * {@link Side#BOTH} if both {@link #getPlus()} and {@link #getMinus()} + * return non-null or {@link Side#HYPER} if both {@link #getPlus()} and + * {@link #getMinus()} return null + * @since 3.6 + */ + public Side getSide() { + if (plus != null) { + if (minus != null) { + return Side.BOTH; + } else { + return Side.PLUS; + } + } else if (minus != null) { + return Side.MINUS; + } else { + return Side.HYPER; + } + } + + } + + /** Specialized exception for inconsistent BSP tree state inconsistency. + * <p> + * This exception is thrown at {@link ArcsSet} construction time when the + * {@link org.apache.commons.math3.geometry.partitioning.Region.Location inside/outside} + * state is not consistent at the 0, \(2 \pi \) crossing. + * </p> + */ + public static class InconsistentStateAt2PiWrapping extends MathIllegalArgumentException { + + /** Serializable UID. */ + private static final long serialVersionUID = 20140107L; + + /** Simple constructor. + */ + public InconsistentStateAt2PiWrapping() { + super(LocalizedFormats.INCONSISTENT_STATE_AT_2_PI_WRAPPING); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/LimitAngle.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/LimitAngle.java new file mode 100644 index 0000000..748a142 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/LimitAngle.java @@ -0,0 +1,127 @@ +/* + * 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.spherical.oned; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; + +/** This class represents a 1D oriented hyperplane on the circle. + * <p>An hyperplane on the 1-sphere is an angle with an orientation.</p> + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.3 + */ +public class LimitAngle implements Hyperplane<Sphere1D> { + + /** Angle location. */ + private S1Point location; + + /** Orientation. */ + private boolean direct; + + /** Tolerance below which angles are considered identical. */ + private final double tolerance; + + /** Simple constructor. + * @param location location of the hyperplane + * @param direct if true, the plus side of the hyperplane is towards + * angles greater than {@code location} + * @param tolerance tolerance below which angles are considered identical + */ + public LimitAngle(final S1Point location, final boolean direct, final double tolerance) { + this.location = location; + this.direct = direct; + this.tolerance = tolerance; + } + + /** Copy the instance. + * <p>Since instances are immutable, this method directly returns + * the instance.</p> + * @return the instance itself + */ + public LimitAngle copySelf() { + return this; + } + + /** {@inheritDoc} */ + public double getOffset(final Point<Sphere1D> point) { + final double delta = ((S1Point) point).getAlpha() - location.getAlpha(); + return direct ? delta : -delta; + } + + /** Check if the hyperplane orientation is direct. + * @return true if the plus side of the hyperplane is towards + * angles greater than hyperplane location + */ + public boolean isDirect() { + return direct; + } + + /** Get the reverse of the instance. + * <p>Get a limit angle with reversed orientation with respect to the + * instance. A new object is built, the instance is untouched.</p> + * @return a new limit angle, with orientation opposite to the instance orientation + */ + public LimitAngle getReverse() { + return new LimitAngle(location, !direct, tolerance); + } + + /** 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 SubLimitAngle wholeHyperplane() { + return new SubLimitAngle(this, null); + } + + /** Build a region covering the whole space. + * @return a region containing the instance (really an {@link + * ArcsSet IntervalsSet} instance) + */ + public ArcsSet wholeSpace() { + return new ArcsSet(tolerance); + } + + /** {@inheritDoc} */ + public boolean sameOrientationAs(final Hyperplane<Sphere1D> other) { + return !(direct ^ ((LimitAngle) other).direct); + } + + /** Get the hyperplane location on the circle. + * @return the hyperplane location + */ + public S1Point getLocation() { + return location; + } + + /** {@inheritDoc} */ + public Point<Sphere1D> project(Point<Sphere1D> point) { + return location; + } + + /** {@inheritDoc} */ + public double getTolerance() { + return tolerance; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/S1Point.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/S1Point.java new file mode 100644 index 0000000..263a559 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/S1Point.java @@ -0,0 +1,157 @@ +/* + * 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.spherical.oned; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; + +/** This class represents a point on the 1-sphere. + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.3 + */ +public class S1Point implements Point<Sphere1D> { + + // CHECKSTYLE: stop ConstantName + /** A vector with all coordinates set to NaN. */ + public static final S1Point NaN = new S1Point(Double.NaN, Vector2D.NaN); + // CHECKSTYLE: resume ConstantName + + /** Serializable UID. */ + private static final long serialVersionUID = 20131218L; + + /** Azimuthal angle \( \alpha \). */ + private final double alpha; + + /** Corresponding 2D normalized vector. */ + private final Vector2D vector; + + /** Simple constructor. + * Build a vector from its coordinates + * @param alpha azimuthal angle \( \alpha \) + * @see #getAlpha() + */ + public S1Point(final double alpha) { + this(MathUtils.normalizeAngle(alpha, FastMath.PI), + new Vector2D(FastMath.cos(alpha), FastMath.sin(alpha))); + } + + /** Build a point from its internal components. + * @param alpha azimuthal angle \( \alpha \) + * @param vector corresponding vector + */ + private S1Point(final double alpha, final Vector2D vector) { + this.alpha = alpha; + this.vector = vector; + } + + /** Get the azimuthal angle \( \alpha \). + * @return azimuthal angle \( \alpha \) + * @see #S1Point(double) + */ + public double getAlpha() { + return alpha; + } + + /** Get the corresponding normalized vector in the 2D euclidean space. + * @return normalized vector + */ + public Vector2D getVector() { + return vector; + } + + /** {@inheritDoc} */ + public Space getSpace() { + return Sphere1D.getInstance(); + } + + /** {@inheritDoc} */ + public boolean isNaN() { + return Double.isNaN(alpha); + } + + /** {@inheritDoc} */ + public double distance(final Point<Sphere1D> point) { + return distance(this, (S1Point) point); + } + + /** Compute the distance (angular separation) between two points. + * @param p1 first vector + * @param p2 second vector + * @return the angular separation between p1 and p2 + */ + public static double distance(S1Point p1, S1Point p2) { + return Vector2D.angle(p1.vector, p2.vector); + } + + /** + * Test for the equality of two points on the 2-sphere. + * <p> + * If all coordinates of two points are exactly the same, and none are + * <code>Double.NaN</code>, the two points 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 points on the 2-sphere objects are equal, false if + * object is null, not an instance of S2Point, or + * not equal to this S2Point instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof S1Point) { + final S1Point rhs = (S1Point) other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return alpha == rhs.alpha; + } + + 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 1759 * MathUtils.hash(alpha); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Sphere1D.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Sphere1D.java new file mode 100644 index 0000000..ce5c7cd --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/Sphere1D.java @@ -0,0 +1,106 @@ +/* + * 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.spherical.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 sphere (i.e. a circle). + * <p> + * We use here the topologists definition of the 1-sphere (see + * <a href="http://mathworld.wolfram.com/Sphere.html">Sphere</a> on + * MathWorld), i.e. the 1-sphere is the one-dimensional closed curve + * defined in 2D as x<sup>2</sup>+y<sup>2</sup>=1. + * </p> + * @since 3.3 + */ +public class Sphere1D implements Serializable, Space { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 20131218L; + + /** Private constructor for the singleton. + */ + private Sphere1D() { + } + + /** Get the unique instance. + * @return the unique instance + */ + public static Sphere1D getInstance() { + return LazyHolder.INSTANCE; + } + + /** {@inheritDoc} */ + public int getDimension() { + return 1; + } + + /** {@inheritDoc} + * <p> + * As the 1-dimension sphere 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 Sphere1D INSTANCE = new Sphere1D(); + } + // 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/spherical/oned/SubLimitAngle.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/SubLimitAngle.java new file mode 100644 index 0000000..ebd3627 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/SubLimitAngle.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.spherical.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 LimitAngle}. + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.3 + */ +public class SubLimitAngle extends AbstractSubHyperplane<Sphere1D, Sphere1D> { + + /** Simple constructor. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + public SubLimitAngle(final Hyperplane<Sphere1D> hyperplane, + final Region<Sphere1D> remainingRegion) { + super(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public double getSize() { + return 0; + } + + /** {@inheritDoc} */ + @Override + public boolean isEmpty() { + return false; + } + + /** {@inheritDoc} */ + @Override + protected AbstractSubHyperplane<Sphere1D, Sphere1D> buildNew(final Hyperplane<Sphere1D> hyperplane, + final Region<Sphere1D> remainingRegion) { + return new SubLimitAngle(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public SplitSubHyperplane<Sphere1D> split(final Hyperplane<Sphere1D> hyperplane) { + final double global = hyperplane.getOffset(((LimitAngle) getHyperplane()).getLocation()); + return (global < -1.0e-10) ? + new SplitSubHyperplane<Sphere1D>(null, this) : + new SplitSubHyperplane<Sphere1D>(this, null); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/oned/package-info.java b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/package-info.java new file mode 100644 index 0000000..d54bc0b --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/oned/package-info.java @@ -0,0 +1,30 @@ +/* + * 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 geometry components on the 1-sphere. + * </p> + * <p> + * We use here the topologists definition of the 1-sphere (see + * <a href="http://mathworld.wolfram.com/Sphere.html">Sphere</a> on + * MathWorld), i.e. the 1-sphere is the one-dimensional closed curve + * defined in 2D as x<sup>2</sup>+y<sup>2</sup>=1. + * </p> + * + */ +package org.apache.commons.math3.geometry.spherical.oned; diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Circle.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Circle.java new file mode 100644 index 0000000..a34db6d --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Circle.java @@ -0,0 +1,326 @@ +/* + * 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.spherical.twod; + +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +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.geometry.spherical.oned.Arc; +import org.apache.commons.math3.geometry.spherical.oned.ArcsSet; +import org.apache.commons.math3.geometry.spherical.oned.S1Point; +import org.apache.commons.math3.geometry.spherical.oned.Sphere1D; +import org.apache.commons.math3.util.FastMath; + +/** This class represents an oriented great circle on the 2-sphere. + + * <p>An oriented circle can be defined by a center point. The circle + * is the the set of points that are in the normal plan the center.</p> + + * <p>Since it is oriented the two spherical caps at its two sides are + * unambiguously identified as a left cap and a right cap. 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 spherical polygon boundary.</p> + + * @since 3.3 + */ +public class Circle implements Hyperplane<Sphere2D>, Embedding<Sphere2D, Sphere1D> { + + /** Pole or circle center. */ + private Vector3D pole; + + /** First axis in the equator plane, origin of the phase angles. */ + private Vector3D x; + + /** Second axis in the equator plane, in quadrature with respect to x. */ + private Vector3D y; + + /** Tolerance below which close sub-arcs are merged together. */ + private final double tolerance; + + /** Build a great circle from its pole. + * <p>The circle is oriented in the trigonometric direction around pole.</p> + * @param pole circle pole + * @param tolerance tolerance below which close sub-arcs are merged together + */ + public Circle(final Vector3D pole, final double tolerance) { + reset(pole); + this.tolerance = tolerance; + } + + /** Build a great circle from two non-aligned points. + * <p>The circle is oriented from first to second point using the path smaller than \( \pi \).</p> + * @param first first point contained in the great circle + * @param second second point contained in the great circle + * @param tolerance tolerance below which close sub-arcs are merged together + */ + public Circle(final S2Point first, final S2Point second, final double tolerance) { + reset(first.getVector().crossProduct(second.getVector())); + this.tolerance = tolerance; + } + + /** Build a circle from its internal components. + * <p>The circle is oriented in the trigonometric direction around center.</p> + * @param pole circle pole + * @param x first axis in the equator plane + * @param y second axis in the equator plane + * @param tolerance tolerance below which close sub-arcs are merged together + */ + private Circle(final Vector3D pole, final Vector3D x, final Vector3D y, + final double tolerance) { + this.pole = pole; + this.x = x; + this.y = y; + this.tolerance = tolerance; + } + + /** Copy constructor. + * <p>The created instance is completely independent from the + * original instance, it is a deep copy.</p> + * @param circle circle to copy + */ + public Circle(final Circle circle) { + this(circle.pole, circle.x, circle.y, circle.tolerance); + } + + /** {@inheritDoc} */ + public Circle copySelf() { + return new Circle(this); + } + + /** Reset the instance as if built from a pole. + * <p>The circle is oriented in the trigonometric direction around pole.</p> + * @param newPole circle pole + */ + public void reset(final Vector3D newPole) { + this.pole = newPole.normalize(); + this.x = newPole.orthogonal(); + this.y = Vector3D.crossProduct(newPole, x).normalize(); + } + + /** Revert the instance. + */ + public void revertSelf() { + // x remains the same + y = y.negate(); + pole = pole.negate(); + } + + /** Get the reverse of the instance. + * <p>Get a circle with reversed orientation with respect to the + * instance. A new object is built, the instance is untouched.</p> + * @return a new circle, with orientation opposite to the instance orientation + */ + public Circle getReverse() { + return new Circle(pole.negate(), x, y.negate(), tolerance); + } + + /** {@inheritDoc} */ + public Point<Sphere2D> project(Point<Sphere2D> point) { + return toSpace(toSubSpace(point)); + } + + /** {@inheritDoc} */ + public double getTolerance() { + return tolerance; + } + + /** {@inheritDoc} + * @see #getPhase(Vector3D) + */ + public S1Point toSubSpace(final Point<Sphere2D> point) { + return new S1Point(getPhase(((S2Point) point).getVector())); + } + + /** Get the phase angle of a direction. + * <p> + * The direction may not belong to the circle as the + * phase is computed for the meridian plane between the circle + * pole and the direction. + * </p> + * @param direction direction for which phase is requested + * @return phase angle of the direction around the circle + * @see #toSubSpace(Point) + */ + public double getPhase(final Vector3D direction) { + return FastMath.PI + FastMath.atan2(-direction.dotProduct(y), -direction.dotProduct(x)); + } + + /** {@inheritDoc} + * @see #getPointAt(double) + */ + public S2Point toSpace(final Point<Sphere1D> point) { + return new S2Point(getPointAt(((S1Point) point).getAlpha())); + } + + /** Get a circle point from its phase around the circle. + * @param alpha phase around the circle + * @return circle point on the sphere + * @see #toSpace(Point) + * @see #getXAxis() + * @see #getYAxis() + */ + public Vector3D getPointAt(final double alpha) { + return new Vector3D(FastMath.cos(alpha), x, FastMath.sin(alpha), y); + } + + /** Get the X axis of the circle. + * <p> + * This method returns the same value as {@link #getPointAt(double) + * getPointAt(0.0)} but it does not do any computation and always + * return the same instance. + * </p> + * @return an arbitrary x axis on the circle + * @see #getPointAt(double) + * @see #getYAxis() + * @see #getPole() + */ + public Vector3D getXAxis() { + return x; + } + + /** Get the Y axis of the circle. + * <p> + * This method returns the same value as {@link #getPointAt(double) + * getPointAt(0.5 * FastMath.PI)} but it does not do any computation and always + * return the same instance. + * </p> + * @return an arbitrary y axis point on the circle + * @see #getPointAt(double) + * @see #getXAxis() + * @see #getPole() + */ + public Vector3D getYAxis() { + return y; + } + + /** Get the pole of the circle. + * <p> + * As the circle is a great circle, the pole does <em>not</em> + * belong to it. + * </p> + * @return pole of the circle + * @see #getXAxis() + * @see #getYAxis() + */ + public Vector3D getPole() { + return pole; + } + + /** Get the arc of the instance that lies inside the other circle. + * @param other other circle + * @return arc of the instance that lies inside the other circle + */ + public Arc getInsideArc(final Circle other) { + final double alpha = getPhase(other.pole); + final double halfPi = 0.5 * FastMath.PI; + return new Arc(alpha - halfPi, alpha + halfPi, tolerance); + } + + /** {@inheritDoc} */ + public SubCircle wholeHyperplane() { + return new SubCircle(this, new ArcsSet(tolerance)); + } + + /** Build a region covering the whole space. + * @return a region containing the instance (really a {@link + * SphericalPolygonsSet SphericalPolygonsSet} instance) + */ + public SphericalPolygonsSet wholeSpace() { + return new SphericalPolygonsSet(tolerance); + } + + /** {@inheritDoc} + * @see #getOffset(Vector3D) + */ + public double getOffset(final Point<Sphere2D> point) { + return getOffset(((S2Point) point).getVector()); + } + + /** Get the offset (oriented distance) of a direction. + * <p>The offset is defined as the angular distance between the + * circle center and the direction minus the circle radius. It + * is therefore 0 on the circle, positive for directions outside of + * the cone delimited by the circle, and negative inside the cone.</p> + * @param direction direction to check + * @return offset of the direction + * @see #getOffset(Point) + */ + public double getOffset(final Vector3D direction) { + return Vector3D.angle(pole, direction) - 0.5 * FastMath.PI; + } + + /** {@inheritDoc} */ + public boolean sameOrientationAs(final Hyperplane<Sphere2D> other) { + final Circle otherC = (Circle) other; + return Vector3D.dotProduct(pole, otherC.pole) >= 0.0; + } + + /** Get a {@link org.apache.commons.math3.geometry.partitioning.Transform + * Transform} embedding a 3D rotation. + * @param rotation rotation to use + * @return a new transform that can be applied to either {@link + * Point Point}, {@link Circle Line} or {@link + * org.apache.commons.math3.geometry.partitioning.SubHyperplane + * SubHyperplane} instances + */ + public static Transform<Sphere2D, Sphere1D> getTransform(final Rotation rotation) { + return new CircleTransform(rotation); + } + + /** Class embedding a 3D rotation. */ + private static class CircleTransform implements Transform<Sphere2D, Sphere1D> { + + /** Underlying rotation. */ + private final Rotation rotation; + + /** Build a transform from a {@code Rotation}. + * @param rotation rotation to use + */ + CircleTransform(final Rotation rotation) { + this.rotation = rotation; + } + + /** {@inheritDoc} */ + public S2Point apply(final Point<Sphere2D> point) { + return new S2Point(rotation.applyTo(((S2Point) point).getVector())); + } + + /** {@inheritDoc} */ + public Circle apply(final Hyperplane<Sphere2D> hyperplane) { + final Circle circle = (Circle) hyperplane; + return new Circle(rotation.applyTo(circle.pole), + rotation.applyTo(circle.x), + rotation.applyTo(circle.y), + circle.tolerance); + } + + /** {@inheritDoc} */ + public SubHyperplane<Sphere1D> apply(final SubHyperplane<Sphere1D> sub, + final Hyperplane<Sphere2D> original, + final Hyperplane<Sphere2D> transformed) { + // as the circle is rotated, the limit angles are rotated too + return sub; + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Edge.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Edge.java new file mode 100644 index 0000000..a9ccb08 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Edge.java @@ -0,0 +1,222 @@ +/* + * 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.spherical.twod; + +import java.util.List; + +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.apache.commons.math3.geometry.spherical.oned.Arc; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; + +/** Spherical polygons boundary edge. + * @see SphericalPolygonsSet#getBoundaryLoops() + * @see Vertex + * @since 3.3 + */ +public class Edge { + + /** Start vertex. */ + private final Vertex start; + + /** End vertex. */ + private Vertex end; + + /** Length of the arc. */ + private final double length; + + /** Circle supporting the edge. */ + private final Circle circle; + + /** Build an edge not contained in any node yet. + * @param start start vertex + * @param end end vertex + * @param length length of the arc (it can be greater than \( \pi \)) + * @param circle circle supporting the edge + */ + Edge(final Vertex start, final Vertex end, final double length, final Circle circle) { + + this.start = start; + this.end = end; + this.length = length; + this.circle = circle; + + // 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 length of the arc. + * @return length of the arc (can be greater than \( \pi \)) + */ + public double getLength() { + return length; + } + + /** Get the circle supporting this edge. + * @return circle supporting this edge + */ + public Circle getCircle() { + return circle; + } + + /** Get an intermediate point. + * <p> + * The angle along the edge should normally be between 0 and {@link #getLength()} + * in order to remain within edge limits. However, there are no checks on the + * value of the angle, so user can rebuild the full circle on which an edge is + * defined if they want. + * </p> + * @param alpha angle along the edge, counted from {@link #getStart()} + * @return an intermediate point + */ + public Vector3D getPointAt(final double alpha) { + return circle.getPointAt(alpha + circle.getPhase(start.getLocation().getVector())); + } + + /** Connect the instance with a following edge. + * @param next edge following the instance + */ + void setNextEdge(final Edge next) { + end = next.getStart(); + end.setIncoming(this); + end.bindWith(getCircle()); + } + + /** Split the edge. + * <p> + * Once split, this edge is not referenced anymore by the vertices, + * it is replaced by the two or three sub-edges and intermediate splitting + * vertices are introduced to connect these sub-edges together. + * </p> + * @param splitCircle circle splitting the edge in several parts + * @param outsideList list where to put parts that are outside of the split circle + * @param insideList list where to put parts that are inside the split circle + */ + void split(final Circle splitCircle, + final List<Edge> outsideList, final List<Edge> insideList) { + + // get the inside arc, synchronizing its phase with the edge itself + final double edgeStart = circle.getPhase(start.getLocation().getVector()); + final Arc arc = circle.getInsideArc(splitCircle); + final double arcRelativeStart = MathUtils.normalizeAngle(arc.getInf(), edgeStart + FastMath.PI) - edgeStart; + final double arcRelativeEnd = arcRelativeStart + arc.getSize(); + final double unwrappedEnd = arcRelativeEnd - MathUtils.TWO_PI; + + // build the sub-edges + final double tolerance = circle.getTolerance(); + Vertex previousVertex = start; + if (unwrappedEnd >= length - tolerance) { + + // the edge is entirely contained inside the circle + // we don't split anything + insideList.add(this); + + } else { + + // there are at least some parts of the edge that should be outside + // (even is they are later be filtered out as being too small) + double alreadyManagedLength = 0; + if (unwrappedEnd >= 0) { + // the start of the edge is inside the circle + previousVertex = addSubEdge(previousVertex, + new Vertex(new S2Point(circle.getPointAt(edgeStart + unwrappedEnd))), + unwrappedEnd, insideList, splitCircle); + alreadyManagedLength = unwrappedEnd; + } + + if (arcRelativeStart >= length - tolerance) { + // the edge ends while still outside of the circle + if (unwrappedEnd >= 0) { + previousVertex = addSubEdge(previousVertex, end, + length - alreadyManagedLength, outsideList, splitCircle); + } else { + // the edge is entirely outside of the circle + // we don't split anything + outsideList.add(this); + } + } else { + // the edge is long enough to enter inside the circle + previousVertex = addSubEdge(previousVertex, + new Vertex(new S2Point(circle.getPointAt(edgeStart + arcRelativeStart))), + arcRelativeStart - alreadyManagedLength, outsideList, splitCircle); + alreadyManagedLength = arcRelativeStart; + + if (arcRelativeEnd >= length - tolerance) { + // the edge ends while still inside of the circle + previousVertex = addSubEdge(previousVertex, end, + length - alreadyManagedLength, insideList, splitCircle); + } else { + // the edge is long enough to exit outside of the circle + previousVertex = addSubEdge(previousVertex, + new Vertex(new S2Point(circle.getPointAt(edgeStart + arcRelativeStart))), + arcRelativeStart - alreadyManagedLength, insideList, splitCircle); + alreadyManagedLength = arcRelativeStart; + previousVertex = addSubEdge(previousVertex, end, + length - alreadyManagedLength, outsideList, splitCircle); + } + } + + } + + } + + /** Add a sub-edge to a list if long enough. + * <p> + * If the length of the sub-edge to add is smaller than the {@link Circle#getTolerance()} + * tolerance of the support circle, it will be ignored. + * </p> + * @param subStart start of the sub-edge + * @param subEnd end of the sub-edge + * @param subLength length of the sub-edge + * @param splitCircle circle splitting the edge in several parts + * @param list list where to put the sub-edge + * @return end vertex of the edge ({@code subEnd} if the edge was long enough and really + * added, {@code subStart} if the edge was too small and therefore ignored) + */ + private Vertex addSubEdge(final Vertex subStart, final Vertex subEnd, final double subLength, + final List<Edge> list, final Circle splitCircle) { + + if (subLength <= circle.getTolerance()) { + // the edge is too short, we ignore it + return subStart; + } + + // really add the edge + subEnd.bindWith(splitCircle); + final Edge edge = new Edge(subStart, subEnd, subLength, circle); + list.add(edge); + return subEnd; + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/EdgesBuilder.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/EdgesBuilder.java new file mode 100644 index 0000000..844cfb1 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/EdgesBuilder.java @@ -0,0 +1,169 @@ +/* + * 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.spherical.twod; + +import java.util.ArrayList; +import java.util.IdentityHashMap; +import java.util.List; +import java.util.Map; + +import org.apache.commons.math3.exception.MathIllegalStateException; +import org.apache.commons.math3.exception.util.LocalizedFormats; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +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.spherical.oned.Arc; +import org.apache.commons.math3.geometry.spherical.oned.ArcsSet; +import org.apache.commons.math3.geometry.spherical.oned.S1Point; + +/** Visitor building edges. + * @since 3.3 + */ +class EdgesBuilder implements BSPTreeVisitor<Sphere2D> { + + /** Root of the tree. */ + private final BSPTree<Sphere2D> root; + + /** Tolerance below which points are consider to be identical. */ + private final double tolerance; + + /** Built edges and their associated nodes. */ + private final Map<Edge, BSPTree<Sphere2D>> edgeToNode; + + /** Reversed map. */ + private final Map<BSPTree<Sphere2D>, List<Edge>> nodeToEdgesList; + + /** Simple constructor. + * @param root tree root + * @param tolerance below which points are consider to be identical + */ + EdgesBuilder(final BSPTree<Sphere2D> root, final double tolerance) { + this.root = root; + this.tolerance = tolerance; + this.edgeToNode = new IdentityHashMap<Edge, BSPTree<Sphere2D>>(); + this.nodeToEdgesList = new IdentityHashMap<BSPTree<Sphere2D>, List<Edge>>(); + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<Sphere2D> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<Sphere2D> node) { + nodeToEdgesList.put(node, new ArrayList<Edge>()); + @SuppressWarnings("unchecked") + final BoundaryAttribute<Sphere2D> attribute = (BoundaryAttribute<Sphere2D>) node.getAttribute(); + if (attribute.getPlusOutside() != null) { + addContribution((SubCircle) attribute.getPlusOutside(), false, node); + } + if (attribute.getPlusInside() != null) { + addContribution((SubCircle) attribute.getPlusInside(), true, node); + } + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<Sphere2D> node) { + } + + /** Add the contribution of a boundary edge. + * @param sub boundary facet + * @param reversed if true, the facet has the inside on its plus side + * @param node node to which the edge belongs + */ + private void addContribution(final SubCircle sub, final boolean reversed, + final BSPTree<Sphere2D> node) { + final Circle circle = (Circle) sub.getHyperplane(); + final List<Arc> arcs = ((ArcsSet) sub.getRemainingRegion()).asList(); + for (final Arc a : arcs) { + final Vertex start = new Vertex((S2Point) circle.toSpace(new S1Point(a.getInf()))); + final Vertex end = new Vertex((S2Point) circle.toSpace(new S1Point(a.getSup()))); + start.bindWith(circle); + end.bindWith(circle); + final Edge edge; + if (reversed) { + edge = new Edge(end, start, a.getSize(), circle.getReverse()); + } else { + edge = new Edge(start, end, a.getSize(), circle); + } + edgeToNode.put(edge, node); + nodeToEdgesList.get(node).add(edge); + } + } + + /** Get the edge that should naturally follow another one. + * @param previous edge to be continued + * @return other edge, starting where the previous one ends (they + * have not been connected yet) + * @exception MathIllegalStateException if there is not a single other edge + */ + private Edge getFollowingEdge(final Edge previous) + throws MathIllegalStateException { + + // get the candidate nodes + final S2Point point = previous.getEnd().getLocation(); + final List<BSPTree<Sphere2D>> candidates = root.getCloseCuts(point, tolerance); + + // the following edge we are looking for must start from one of the candidates nodes + double closest = tolerance; + Edge following = null; + for (final BSPTree<Sphere2D> node : candidates) { + for (final Edge edge : nodeToEdgesList.get(node)) { + if (edge != previous && edge.getStart().getIncoming() == null) { + final Vector3D edgeStart = edge.getStart().getLocation().getVector(); + final double gap = Vector3D.angle(point.getVector(), edgeStart); + if (gap <= closest) { + closest = gap; + following = edge; + } + } + } + } + + if (following == null) { + final Vector3D previousStart = previous.getStart().getLocation().getVector(); + if (Vector3D.angle(point.getVector(), previousStart) <= tolerance) { + // the edge connects back to itself + return previous; + } + + // this should never happen + throw new MathIllegalStateException(LocalizedFormats.OUTLINE_BOUNDARY_LOOP_OPEN); + + } + + return following; + + } + + /** Get the boundary edges. + * @return boundary edges + * @exception MathIllegalStateException if there is not a single other edge + */ + public List<Edge> getEdges() throws MathIllegalStateException { + + // connect the edges + for (final Edge previous : edgeToNode.keySet()) { + previous.setNextEdge(getFollowingEdge(previous)); + } + + return new ArrayList<Edge>(edgeToNode.keySet()); + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/PropertiesComputer.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/PropertiesComputer.java new file mode 100644 index 0000000..593180f --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/PropertiesComputer.java @@ -0,0 +1,173 @@ +/* + * 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.spherical.twod; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.exception.MathInternalError; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.apache.commons.math3.geometry.partitioning.BSPTree; +import org.apache.commons.math3.geometry.partitioning.BSPTreeVisitor; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; + +/** Visitor computing geometrical properties. + * @since 3.3 + */ +class PropertiesComputer implements BSPTreeVisitor<Sphere2D> { + + /** Tolerance below which points are consider to be identical. */ + private final double tolerance; + + /** Summed area. */ + private double summedArea; + + /** Summed barycenter. */ + private Vector3D summedBarycenter; + + /** List of points strictly inside convex cells. */ + private final List<Vector3D> convexCellsInsidePoints; + + /** Simple constructor. + * @param tolerance below which points are consider to be identical + */ + PropertiesComputer(final double tolerance) { + this.tolerance = tolerance; + this.summedArea = 0; + this.summedBarycenter = Vector3D.ZERO; + this.convexCellsInsidePoints = new ArrayList<Vector3D>(); + } + + /** {@inheritDoc} */ + public Order visitOrder(final BSPTree<Sphere2D> node) { + return Order.MINUS_SUB_PLUS; + } + + /** {@inheritDoc} */ + public void visitInternalNode(final BSPTree<Sphere2D> node) { + // nothing to do here + } + + /** {@inheritDoc} */ + public void visitLeafNode(final BSPTree<Sphere2D> node) { + if ((Boolean) node.getAttribute()) { + + // transform this inside leaf cell into a simple convex polygon + final SphericalPolygonsSet convex = + new SphericalPolygonsSet(node.pruneAroundConvexCell(Boolean.TRUE, + Boolean.FALSE, + null), + tolerance); + + // extract the start of the single loop boundary of the convex cell + final List<Vertex> boundary = convex.getBoundaryLoops(); + if (boundary.size() != 1) { + // this should never happen + throw new MathInternalError(); + } + + // compute the geometrical properties of the convex cell + final double area = convexCellArea(boundary.get(0)); + final Vector3D barycenter = convexCellBarycenter(boundary.get(0)); + convexCellsInsidePoints.add(barycenter); + + // add the cell contribution to the global properties + summedArea += area; + summedBarycenter = new Vector3D(1, summedBarycenter, area, barycenter); + + } + } + + /** Compute convex cell area. + * @param start start vertex of the convex cell boundary + * @return area + */ + private double convexCellArea(final Vertex start) { + + int n = 0; + double sum = 0; + + // loop around the cell + for (Edge e = start.getOutgoing(); n == 0 || e.getStart() != start; e = e.getEnd().getOutgoing()) { + + // find path interior angle at vertex + final Vector3D previousPole = e.getCircle().getPole(); + final Vector3D nextPole = e.getEnd().getOutgoing().getCircle().getPole(); + final Vector3D point = e.getEnd().getLocation().getVector(); + double alpha = FastMath.atan2(Vector3D.dotProduct(nextPole, Vector3D.crossProduct(point, previousPole)), + -Vector3D.dotProduct(nextPole, previousPole)); + if (alpha < 0) { + alpha += MathUtils.TWO_PI; + } + sum += alpha; + n++; + } + + // compute area using extended Girard theorem + // see Spherical Trigonometry: For the Use of Colleges and Schools by I. Todhunter + // article 99 in chapter VIII Area Of a Spherical Triangle. Spherical Excess. + // book available from project Gutenberg at http://www.gutenberg.org/ebooks/19770 + return sum - (n - 2) * FastMath.PI; + + } + + /** Compute convex cell barycenter. + * @param start start vertex of the convex cell boundary + * @return barycenter + */ + private Vector3D convexCellBarycenter(final Vertex start) { + + int n = 0; + Vector3D sumB = Vector3D.ZERO; + + // loop around the cell + for (Edge e = start.getOutgoing(); n == 0 || e.getStart() != start; e = e.getEnd().getOutgoing()) { + sumB = new Vector3D(1, sumB, e.getLength(), e.getCircle().getPole()); + n++; + } + + return sumB.normalize(); + + } + + /** Get the area. + * @return area + */ + public double getArea() { + return summedArea; + } + + /** Get the barycenter. + * @return barycenter + */ + public S2Point getBarycenter() { + if (summedBarycenter.getNormSq() == 0) { + return S2Point.NaN; + } else { + return new S2Point(summedBarycenter); + } + } + + /** Get the points strictly inside convex cells. + * @return points strictly inside convex cells + */ + public List<Vector3D> getConvexCellsInsidePoints() { + return convexCellsInsidePoints; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/S2Point.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/S2Point.java new file mode 100644 index 0000000..677e830 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/S2Point.java @@ -0,0 +1,237 @@ +/* + * 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.spherical.twod; + +import org.apache.commons.math3.exception.MathArithmeticException; +import org.apache.commons.math3.exception.OutOfRangeException; +import org.apache.commons.math3.geometry.Point; +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; + +/** This class represents a point on the 2-sphere. + * <p> + * We use the mathematical convention to use the azimuthal angle \( \theta \) + * in the x-y plane as the first coordinate, and the polar angle \( \varphi \) + * as the second coordinate (see <a + * href="http://mathworld.wolfram.com/SphericalCoordinates.html">Spherical + * Coordinates</a> in MathWorld). + * </p> + * <p>Instances of this class are guaranteed to be immutable.</p> + * @since 3.3 + */ +public class S2Point implements Point<Sphere2D> { + + /** +I (coordinates: \( \theta = 0, \varphi = \pi/2 \)). */ + public static final S2Point PLUS_I = new S2Point(0, 0.5 * FastMath.PI, Vector3D.PLUS_I); + + /** +J (coordinates: \( \theta = \pi/2, \varphi = \pi/2 \))). */ + public static final S2Point PLUS_J = new S2Point(0.5 * FastMath.PI, 0.5 * FastMath.PI, Vector3D.PLUS_J); + + /** +K (coordinates: \( \theta = any angle, \varphi = 0 \)). */ + public static final S2Point PLUS_K = new S2Point(0, 0, Vector3D.PLUS_K); + + /** -I (coordinates: \( \theta = \pi, \varphi = \pi/2 \)). */ + public static final S2Point MINUS_I = new S2Point(FastMath.PI, 0.5 * FastMath.PI, Vector3D.MINUS_I); + + /** -J (coordinates: \( \theta = 3\pi/2, \varphi = \pi/2 \)). */ + public static final S2Point MINUS_J = new S2Point(1.5 * FastMath.PI, 0.5 * FastMath.PI, Vector3D.MINUS_J); + + /** -K (coordinates: \( \theta = any angle, \varphi = \pi \)). */ + public static final S2Point MINUS_K = new S2Point(0, FastMath.PI, Vector3D.MINUS_K); + + // CHECKSTYLE: stop ConstantName + /** A vector with all coordinates set to NaN. */ + public static final S2Point NaN = new S2Point(Double.NaN, Double.NaN, Vector3D.NaN); + // CHECKSTYLE: resume ConstantName + + /** Serializable UID. */ + private static final long serialVersionUID = 20131218L; + + /** Azimuthal angle \( \theta \) in the x-y plane. */ + private final double theta; + + /** Polar angle \( \varphi \). */ + private final double phi; + + /** Corresponding 3D normalized vector. */ + private final Vector3D vector; + + /** Simple constructor. + * Build a vector from its spherical coordinates + * @param theta azimuthal angle \( \theta \) in the x-y plane + * @param phi polar angle \( \varphi \) + * @see #getTheta() + * @see #getPhi() + * @exception OutOfRangeException if \( \varphi \) is not in the [\( 0; \pi \)] range + */ + public S2Point(final double theta, final double phi) + throws OutOfRangeException { + this(theta, phi, vector(theta, phi)); + } + + /** Simple constructor. + * Build a vector from its underlying 3D vector + * @param vector 3D vector + * @exception MathArithmeticException if vector norm is zero + */ + public S2Point(final Vector3D vector) throws MathArithmeticException { + this(FastMath.atan2(vector.getY(), vector.getX()), Vector3D.angle(Vector3D.PLUS_K, vector), + vector.normalize()); + } + + /** Build a point from its internal components. + * @param theta azimuthal angle \( \theta \) in the x-y plane + * @param phi polar angle \( \varphi \) + * @param vector corresponding vector + */ + private S2Point(final double theta, final double phi, final Vector3D vector) { + this.theta = theta; + this.phi = phi; + this.vector = vector; + } + + /** Build the normalized vector corresponding to spherical coordinates. + * @param theta azimuthal angle \( \theta \) in the x-y plane + * @param phi polar angle \( \varphi \) + * @return normalized vector + * @exception OutOfRangeException if \( \varphi \) is not in the [\( 0; \pi \)] range + */ + private static Vector3D vector(final double theta, final double phi) + throws OutOfRangeException { + + if (phi < 0 || phi > FastMath.PI) { + throw new OutOfRangeException(phi, 0, FastMath.PI); + } + + final double cosTheta = FastMath.cos(theta); + final double sinTheta = FastMath.sin(theta); + final double cosPhi = FastMath.cos(phi); + final double sinPhi = FastMath.sin(phi); + + return new Vector3D(cosTheta * sinPhi, sinTheta * sinPhi, cosPhi); + + } + + /** Get the azimuthal angle \( \theta \) in the x-y plane. + * @return azimuthal angle \( \theta \) in the x-y plane + * @see #S2Point(double, double) + */ + public double getTheta() { + return theta; + } + + /** Get the polar angle \( \varphi \). + * @return polar angle \( \varphi \) + * @see #S2Point(double, double) + */ + public double getPhi() { + return phi; + } + + /** Get the corresponding normalized vector in the 3D euclidean space. + * @return normalized vector + */ + public Vector3D getVector() { + return vector; + } + + /** {@inheritDoc} */ + public Space getSpace() { + return Sphere2D.getInstance(); + } + + /** {@inheritDoc} */ + public boolean isNaN() { + return Double.isNaN(theta) || Double.isNaN(phi); + } + + /** Get the opposite of the instance. + * @return a new vector which is opposite to the instance + */ + public S2Point negate() { + return new S2Point(-theta, FastMath.PI - phi, vector.negate()); + } + + /** {@inheritDoc} */ + public double distance(final Point<Sphere2D> point) { + return distance(this, (S2Point) point); + } + + /** Compute the distance (angular separation) between two points. + * @param p1 first vector + * @param p2 second vector + * @return the angular separation between p1 and p2 + */ + public static double distance(S2Point p1, S2Point p2) { + return Vector3D.angle(p1.vector, p2.vector); + } + + /** + * Test for the equality of two points on the 2-sphere. + * <p> + * If all coordinates of two points are exactly the same, and none are + * <code>Double.NaN</code>, the two points 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 points on the 2-sphere objects are equal, false if + * object is null, not an instance of S2Point, or + * not equal to this S2Point instance + * + */ + @Override + public boolean equals(Object other) { + + if (this == other) { + return true; + } + + if (other instanceof S2Point) { + final S2Point rhs = (S2Point) other; + if (rhs.isNaN()) { + return this.isNaN(); + } + + return (theta == rhs.theta) && (phi == rhs.phi); + } + 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 134 * (37 * MathUtils.hash(theta) + MathUtils.hash(phi)); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Sphere2D.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Sphere2D.java new file mode 100644 index 0000000..93ff04e --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Sphere2D.java @@ -0,0 +1,80 @@ +/* + * 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.spherical.twod; + +import java.io.Serializable; + +import org.apache.commons.math3.geometry.Space; +import org.apache.commons.math3.geometry.spherical.oned.Sphere1D; + +/** + * This class implements a two-dimensional sphere (i.e. the regular sphere). + * <p> + * We use here the topologists definition of the 2-sphere (see + * <a href="http://mathworld.wolfram.com/Sphere.html">Sphere</a> on + * MathWorld), i.e. the 2-sphere is the two-dimensional surface + * defined in 3D as x<sup>2</sup>+y<sup>2</sup>+z<sup>2</sup>=1. + * </p> + * @since 3.3 + */ +public class Sphere2D implements Serializable, Space { + + /** Serializable version identifier. */ + private static final long serialVersionUID = 20131218L; + + /** Private constructor for the singleton. + */ + private Sphere2D() { + } + + /** Get the unique instance. + * @return the unique instance + */ + public static Sphere2D getInstance() { + return LazyHolder.INSTANCE; + } + + /** {@inheritDoc} */ + public int getDimension() { + return 2; + } + + /** {@inheritDoc} */ + public Sphere1D getSubSpace() { + return Sphere1D.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 Sphere2D INSTANCE = new Sphere2D(); + } + // 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/spherical/twod/SphericalPolygonsSet.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java new file mode 100644 index 0000000..8e41659 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java @@ -0,0 +1,565 @@ +/* + * 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.spherical.twod; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Collections; +import java.util.Iterator; +import java.util.List; + +import org.apache.commons.math3.exception.MathIllegalStateException; +import org.apache.commons.math3.geometry.enclosing.EnclosingBall; +import org.apache.commons.math3.geometry.enclosing.WelzlEncloser; +import org.apache.commons.math3.geometry.euclidean.threed.Euclidean3D; +import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; +import org.apache.commons.math3.geometry.euclidean.threed.SphereGenerator; +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +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.RegionFactory; +import org.apache.commons.math3.geometry.partitioning.SubHyperplane; +import org.apache.commons.math3.geometry.spherical.oned.Sphere1D; +import org.apache.commons.math3.util.FastMath; +import org.apache.commons.math3.util.MathUtils; + +/** This class represents a region on the 2-sphere: a set of spherical polygons. + * @since 3.3 + */ +public class SphericalPolygonsSet extends AbstractRegion<Sphere2D, Sphere1D> { + + /** Boundary defined as an array of closed loops start vertices. */ + private List<Vertex> loops; + + /** Build a polygons set representing the whole real 2-sphere. + * @param tolerance below which points are consider to be identical + */ + public SphericalPolygonsSet(final double tolerance) { + super(tolerance); + } + + /** Build a polygons set representing a hemisphere. + * @param pole pole of the hemisphere (the pole is in the inside half) + * @param tolerance below which points are consider to be identical + */ + public SphericalPolygonsSet(final Vector3D pole, final double tolerance) { + super(new BSPTree<Sphere2D>(new Circle(pole, tolerance).wholeHyperplane(), + new BSPTree<Sphere2D>(Boolean.FALSE), + new BSPTree<Sphere2D>(Boolean.TRUE), + null), + tolerance); + } + + /** Build a polygons set representing a regular polygon. + * @param center center of the polygon (the center is in the inside half) + * @param meridian point defining the reference meridian for first polygon vertex + * @param outsideRadius distance of the vertices to the center + * @param n number of sides of the polygon + * @param tolerance below which points are consider to be identical + */ + public SphericalPolygonsSet(final Vector3D center, final Vector3D meridian, + final double outsideRadius, final int n, + final double tolerance) { + this(tolerance, createRegularPolygonVertices(center, meridian, outsideRadius, n)); + } + + /** 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 + * @param tolerance below which points are consider to be identical + */ + public SphericalPolygonsSet(final BSPTree<Sphere2D> 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 below which points are consider to be identical + */ + public SphericalPolygonsSet(final Collection<SubHyperplane<Sphere2D>> boundary, final double tolerance) { + super(boundary, 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 #SphericalPolygonsSet(Collection, + * double) 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 circles 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 + * circles 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 SphericalPolygonsSet(final double hyperplaneThickness, final S2Point ... vertices) { + super(verticesToTree(hyperplaneThickness, vertices), hyperplaneThickness); + } + + /** Build the vertices representing a regular polygon. + * @param center center of the polygon (the center is in the inside half) + * @param meridian point defining the reference meridian for first polygon vertex + * @param outsideRadius distance of the vertices to the center + * @param n number of sides of the polygon + * @return vertices array + */ + private static S2Point[] createRegularPolygonVertices(final Vector3D center, final Vector3D meridian, + final double outsideRadius, final int n) { + final S2Point[] array = new S2Point[n]; + final Rotation r0 = new Rotation(Vector3D.crossProduct(center, meridian), + outsideRadius, RotationConvention.VECTOR_OPERATOR); + array[0] = new S2Point(r0.applyTo(center)); + + final Rotation r = new Rotation(center, MathUtils.TWO_PI / n, RotationConvention.VECTOR_OPERATOR); + for (int i = 1; i < n; ++i) { + array[i] = new S2Point(r.applyTo(array[i - 1].getVector())); + } + + return array; + } + + /** 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>This constructor handles only polygons with edges strictly shorter + * than \( \pi \). If longer edges are needed, they need to be broken up + * in smaller sub-edges so this constraint holds.</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<Sphere2D> verticesToTree(final double hyperplaneThickness, + final S2Point ... vertices) { + + final int n = vertices.length; + if (n == 0) { + // the tree represents the whole space + return new BSPTree<Sphere2D>(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); + Vertex end = vArray[n - 1]; + for (int i = 0; i < n; ++i) { + + // get the endpoints of the edge + final Vertex start = end; + end = vArray[i]; + + // get the circle 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 + Circle circle = start.sharedCircleWith(end); + if (circle == null) { + circle = new Circle(start.getLocation(), end.getLocation(), hyperplaneThickness); + } + + // create the edge and store it + edges.add(new Edge(start, end, + Vector3D.angle(start.getLocation().getVector(), + end.getLocation().getVector()), + circle)); + + // check if another vertex also happens to be on this circle + for (final Vertex vertex : vArray) { + if (vertex != start && vertex != end && + FastMath.abs(circle.getOffset(vertex.getLocation())) <= hyperplaneThickness) { + vertex.bindWith(circle); + } + } + + } + + // build the tree top-down + final BSPTree<Sphere2D> tree = new BSPTree<Sphere2D>(); + insertEdges(hyperplaneThickness, tree, edges); + + return tree; + + } + + /** Recursively build a tree by inserting cut sub-hyperplanes. + * @param hyperplaneThickness tolerance below which points are considered 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<Sphere2D> 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 (!node.insertCut(inserted.getCircle())) { + 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<Sphere2D> 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> outsideList = new ArrayList<Edge>(); + final List<Edge> insideList = new ArrayList<Edge>(); + for (final Edge edge : edges) { + if (edge != inserted) { + edge.split(inserted.getCircle(), outsideList, insideList); + } + } + + // recurse through lower levels + if (!outsideList.isEmpty()) { + insertEdges(hyperplaneThickness, node.getPlus(), outsideList); + } else { + node.getPlus().setAttribute(Boolean.FALSE); + } + if (!insideList.isEmpty()) { + insertEdges(hyperplaneThickness, node.getMinus(), insideList); + } else { + node.getMinus().setAttribute(Boolean.TRUE); + } + + } + + /** {@inheritDoc} */ + @Override + public SphericalPolygonsSet buildNew(final BSPTree<Sphere2D> tree) { + return new SphericalPolygonsSet(tree, getTolerance()); + } + + /** {@inheritDoc} + * @exception MathIllegalStateException if the tolerance setting does not allow to build + * a clean non-ambiguous boundary + */ + @Override + protected void computeGeometricalProperties() throws MathIllegalStateException { + + final BSPTree<Sphere2D> tree = getTree(true); + + if (tree.getCut() == null) { + + // the instance has a single cell without any boundaries + + if (tree.getCut() == null && (Boolean) tree.getAttribute()) { + // the instance covers the whole space + setSize(4 * FastMath.PI); + setBarycenter(new S2Point(0, 0)); + } else { + setSize(0); + setBarycenter(S2Point.NaN); + } + + } else { + + // the instance has a boundary + final PropertiesComputer pc = new PropertiesComputer(getTolerance()); + tree.visit(pc); + setSize(pc.getArea()); + setBarycenter(pc.getBarycenter()); + + } + + } + + /** Get the boundary loops of the polygon. + * <p>The polygon boundary can be represented as a list of closed loops, + * each loop being given by exactly one of its vertices. From each loop + * start vertex, one can follow the loop by finding the outgoing edge, + * then the end vertex, then the next outgoing edge ... until the start + * vertex of the loop (exactly the same instance) is found again once + * the full loop has been visited.</p> + * <p>If the polygon has no boundary at all, a zero length loop + * array will be returned.</p> + * <p>If the polygon is a simple one-piece polygon, then the returned + * array will contain a single vertex. + * </p> + * <p>All edges in the various loops have the inside of the region on + * their left side (i.e. toward their pole) and the outside on their + * right side (i.e. away from their pole) when moving in the underlying + * circle direction. This means that the closed loops obey the direct + * trigonometric orientation.</p> + * @return boundary of the polygon, organized as an unmodifiable list of loops start vertices. + * @exception MathIllegalStateException if the tolerance setting does not allow to build + * a clean non-ambiguous boundary + * @see Vertex + * @see Edge + */ + public List<Vertex> getBoundaryLoops() throws MathIllegalStateException { + + if (loops == null) { + if (getTree(false).getCut() == null) { + loops = Collections.emptyList(); + } else { + + // sort the arcs according to their start point + final BSPTree<Sphere2D> root = getTree(true); + final EdgesBuilder visitor = new EdgesBuilder(root, getTolerance()); + root.visit(visitor); + final List<Edge> edges = visitor.getEdges(); + + + // convert the list of all edges into a list of start vertices + loops = new ArrayList<Vertex>(); + while (!edges.isEmpty()) { + + // this is an edge belonging to a new loop, store it + Edge edge = edges.get(0); + final Vertex startVertex = edge.getStart(); + loops.add(startVertex); + + // remove all remaining edges in the same loop + do { + + // remove one edge + for (final Iterator<Edge> iterator = edges.iterator(); iterator.hasNext();) { + if (iterator.next() == edge) { + iterator.remove(); + break; + } + } + + // go to next edge following the boundary loop + edge = edge.getEnd().getOutgoing(); + + } while (edge.getStart() != startVertex); + + } + + } + } + + return Collections.unmodifiableList(loops); + + } + + /** Get a spherical cap enclosing the polygon. + * <p> + * This method is intended as a first test to quickly identify points + * that are guaranteed to be outside of the region, hence performing a full + * {@link #checkPoint(org.apache.commons.math3.geometry.Vector) checkPoint} + * only if the point status remains undecided after the quick check. It is + * is therefore mostly useful to speed up computation for small polygons with + * complex shapes (say a country boundary on Earth), as the spherical cap will + * be small and hence will reliably identify a large part of the sphere as outside, + * whereas the full check can be more computing intensive. A typical use case is + * therefore: + * </p> + * <pre> + * // compute region, plus an enclosing spherical cap + * SphericalPolygonsSet complexShape = ...; + * EnclosingBall<Sphere2D, S2Point> cap = complexShape.getEnclosingCap(); + * + * // check lots of points + * for (Vector3D p : points) { + * + * final Location l; + * if (cap.contains(p)) { + * // we cannot be sure where the point is + * // we need to perform the full computation + * l = complexShape.checkPoint(v); + * } else { + * // no need to do further computation, + * // we already know the point is outside + * l = Location.OUTSIDE; + * } + * + * // use l ... + * + * } + * </pre> + * <p> + * In the special cases of empty or whole sphere polygons, special + * spherical caps are returned, with angular radius set to negative + * or positive infinity so the {@link + * EnclosingBall#contains(org.apache.commons.math3.geometry.Point) ball.contains(point)} + * method return always false or true. + * </p> + * <p> + * This method is <em>not</em> guaranteed to return the smallest enclosing cap. + * </p> + * @return a spherical cap enclosing the polygon + */ + public EnclosingBall<Sphere2D, S2Point> getEnclosingCap() { + + // handle special cases first + if (isEmpty()) { + return new EnclosingBall<Sphere2D, S2Point>(S2Point.PLUS_K, Double.NEGATIVE_INFINITY); + } + if (isFull()) { + return new EnclosingBall<Sphere2D, S2Point>(S2Point.PLUS_K, Double.POSITIVE_INFINITY); + } + + // as the polygons is neither empty nor full, it has some boundaries and cut hyperplanes + final BSPTree<Sphere2D> root = getTree(false); + if (isEmpty(root.getMinus()) && isFull(root.getPlus())) { + // the polygon covers an hemisphere, and its boundary is one 2π long edge + final Circle circle = (Circle) root.getCut().getHyperplane(); + return new EnclosingBall<Sphere2D, S2Point>(new S2Point(circle.getPole()).negate(), + 0.5 * FastMath.PI); + } + if (isFull(root.getMinus()) && isEmpty(root.getPlus())) { + // the polygon covers an hemisphere, and its boundary is one 2π long edge + final Circle circle = (Circle) root.getCut().getHyperplane(); + return new EnclosingBall<Sphere2D, S2Point>(new S2Point(circle.getPole()), + 0.5 * FastMath.PI); + } + + // gather some inside points, to be used by the encloser + final List<Vector3D> points = getInsidePoints(); + + // extract points from the boundary loops, to be used by the encloser as well + final List<Vertex> boundary = getBoundaryLoops(); + for (final Vertex loopStart : boundary) { + int count = 0; + for (Vertex v = loopStart; count == 0 || v != loopStart; v = v.getOutgoing().getEnd()) { + ++count; + points.add(v.getLocation().getVector()); + } + } + + // find the smallest enclosing 3D sphere + final SphereGenerator generator = new SphereGenerator(); + final WelzlEncloser<Euclidean3D, Vector3D> encloser = + new WelzlEncloser<Euclidean3D, Vector3D>(getTolerance(), generator); + EnclosingBall<Euclidean3D, Vector3D> enclosing3D = encloser.enclose(points); + final Vector3D[] support3D = enclosing3D.getSupport(); + + // convert to 3D sphere to spherical cap + final double r = enclosing3D.getRadius(); + final double h = enclosing3D.getCenter().getNorm(); + if (h < getTolerance()) { + // the 3D sphere is centered on the unit sphere and covers it + // fall back to a crude approximation, based only on outside convex cells + EnclosingBall<Sphere2D, S2Point> enclosingS2 = + new EnclosingBall<Sphere2D, S2Point>(S2Point.PLUS_K, Double.POSITIVE_INFINITY); + for (Vector3D outsidePoint : getOutsidePoints()) { + final S2Point outsideS2 = new S2Point(outsidePoint); + final BoundaryProjection<Sphere2D> projection = projectToBoundary(outsideS2); + if (FastMath.PI - projection.getOffset() < enclosingS2.getRadius()) { + enclosingS2 = new EnclosingBall<Sphere2D, S2Point>(outsideS2.negate(), + FastMath.PI - projection.getOffset(), + (S2Point) projection.getProjected()); + } + } + return enclosingS2; + } + final S2Point[] support = new S2Point[support3D.length]; + for (int i = 0; i < support3D.length; ++i) { + support[i] = new S2Point(support3D[i]); + } + + final EnclosingBall<Sphere2D, S2Point> enclosingS2 = + new EnclosingBall<Sphere2D, S2Point>(new S2Point(enclosing3D.getCenter()), + FastMath.acos((1 + h * h - r * r) / (2 * h)), + support); + + return enclosingS2; + + } + + /** Gather some inside points. + * @return list of points known to be strictly in all inside convex cells + */ + private List<Vector3D> getInsidePoints() { + final PropertiesComputer pc = new PropertiesComputer(getTolerance()); + getTree(true).visit(pc); + return pc.getConvexCellsInsidePoints(); + } + + /** Gather some outside points. + * @return list of points known to be strictly in all outside convex cells + */ + private List<Vector3D> getOutsidePoints() { + final SphericalPolygonsSet complement = + (SphericalPolygonsSet) new RegionFactory<Sphere2D>().getComplement(this); + final PropertiesComputer pc = new PropertiesComputer(getTolerance()); + complement.getTree(true).visit(pc); + return pc.getConvexCellsInsidePoints(); + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SubCircle.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SubCircle.java new file mode 100644 index 0000000..97164cc --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SubCircle.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.spherical.twod; + +import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; +import org.apache.commons.math3.geometry.partitioning.AbstractSubHyperplane; +import org.apache.commons.math3.geometry.partitioning.Hyperplane; +import org.apache.commons.math3.geometry.partitioning.Region; +import org.apache.commons.math3.geometry.spherical.oned.Arc; +import org.apache.commons.math3.geometry.spherical.oned.ArcsSet; +import org.apache.commons.math3.geometry.spherical.oned.Sphere1D; +import org.apache.commons.math3.util.FastMath; + +/** This class represents a sub-hyperplane for {@link Circle}. + * @since 3.3 + */ +public class SubCircle extends AbstractSubHyperplane<Sphere2D, Sphere1D> { + + /** Simple constructor. + * @param hyperplane underlying hyperplane + * @param remainingRegion remaining region of the hyperplane + */ + public SubCircle(final Hyperplane<Sphere2D> hyperplane, + final Region<Sphere1D> remainingRegion) { + super(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + protected AbstractSubHyperplane<Sphere2D, Sphere1D> buildNew(final Hyperplane<Sphere2D> hyperplane, + final Region<Sphere1D> remainingRegion) { + return new SubCircle(hyperplane, remainingRegion); + } + + /** {@inheritDoc} */ + @Override + public SplitSubHyperplane<Sphere2D> split(final Hyperplane<Sphere2D> hyperplane) { + + final Circle thisCircle = (Circle) getHyperplane(); + final Circle otherCircle = (Circle) hyperplane; + final double angle = Vector3D.angle(thisCircle.getPole(), otherCircle.getPole()); + + if (angle < thisCircle.getTolerance() || angle > FastMath.PI - thisCircle.getTolerance()) { + // the two circles are aligned or opposite + return new SplitSubHyperplane<Sphere2D>(null, null); + } else { + // the two circles intersect each other + final Arc arc = thisCircle.getInsideArc(otherCircle); + final ArcsSet.Split split = ((ArcsSet) getRemainingRegion()).split(arc); + final ArcsSet plus = split.getPlus(); + final ArcsSet minus = split.getMinus(); + return new SplitSubHyperplane<Sphere2D>(plus == null ? null : new SubCircle(thisCircle.copySelf(), plus), + minus == null ? null : new SubCircle(thisCircle.copySelf(), minus)); + } + + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Vertex.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Vertex.java new file mode 100644 index 0000000..3003da8 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/Vertex.java @@ -0,0 +1,124 @@ +/* + * 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.spherical.twod; + +import java.util.ArrayList; +import java.util.List; + +/** Spherical polygons boundary vertex. + * @see SphericalPolygonsSet#getBoundaryLoops() + * @see Edge + * @since 3.3 + */ +public class Vertex { + + /** Vertex location. */ + private final S2Point location; + + /** Incoming edge. */ + private Edge incoming; + + /** Outgoing edge. */ + private Edge outgoing; + + /** Circles bound with this vertex. */ + private final List<Circle> circles; + + /** Build a non-processed vertex not owned by any node yet. + * @param location vertex location + */ + Vertex(final S2Point location) { + this.location = location; + this.incoming = null; + this.outgoing = null; + this.circles = new ArrayList<Circle>(); + } + + /** Get Vertex location. + * @return vertex location + */ + public S2Point getLocation() { + return location; + } + + /** Bind a circle considered to contain this vertex. + * @param circle circle to bind with this vertex + */ + void bindWith(final Circle circle) { + circles.add(circle); + } + + /** Get the common circle bound with both the instance and another vertex, if any. + * <p> + * When two vertices are both bound to the same circle, this means they are + * already handled by node associated with this circle, so there is no need + * to create a cut hyperplane for them. + * </p> + * @param vertex other vertex to check instance against + * @return circle bound with both the instance and another vertex, or null if the + * two vertices do not share a circle yet + */ + Circle sharedCircleWith(final Vertex vertex) { + for (final Circle circle1 : circles) { + for (final Circle circle2 : vertex.circles) { + if (circle1 == circle2) { + return circle1; + } + } + } + return null; + } + + /** Set incoming edge. + * <p> + * The circle supporting the incoming edge is automatically bound + * with the instance. + * </p> + * @param incoming incoming edge + */ + void setIncoming(final Edge incoming) { + this.incoming = incoming; + bindWith(incoming.getCircle()); + } + + /** Get incoming edge. + * @return incoming edge + */ + public Edge getIncoming() { + return incoming; + } + + /** Set outgoing edge. + * <p> + * The circle supporting the outgoing edge is automatically bound + * with the instance. + * </p> + * @param outgoing outgoing edge + */ + void setOutgoing(final Edge outgoing) { + this.outgoing = outgoing; + bindWith(outgoing.getCircle()); + } + + /** Get outgoing edge. + * @return outgoing edge + */ + public Edge getOutgoing() { + return outgoing; + } + +} diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/package-info.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/package-info.java new file mode 100644 index 0000000..3f3c5b0 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/package-info.java @@ -0,0 +1,30 @@ +/* + * 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 geometry components on the 2-sphere. + * </p> + * <p> + * We use here the topologists definition of the 2-sphere (see + * <a href="http://mathworld.wolfram.com/Sphere.html">Sphere</a> on + * MathWorld), i.e. the 2-sphere is the two-dimensional surface + * defined in 3D as x<sup>2</sup>+y<sup>2</sup>+z<sup>2</sup>=1. + * </p> + * + */ +package org.apache.commons.math3.geometry.spherical.twod; |
