    001    /*
    002     * Licensed to the Apache Software Foundation (ASF) under one or more
    003     * contributor license agreements.  See the NOTICE file distributed with
    004     * this work for additional information regarding copyright ownership.
    005     * The ASF licenses this file to You under the Apache License, Version 2.0
    006     * (the "License"); you may not use this file except in compliance with
    007     * the License.  You may obtain a copy of the License at
    008     *
    009     *      http://www.apache.org/licenses/LICENSE-2.0
    010     *
    011     * Unless required by applicable law or agreed to in writing, software
    012     * distributed under the License is distributed on an "AS IS" BASIS,
    013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    014     * See the License for the specific language governing permissions and
    015     * limitations under the License.
    016     */
    017    package org.apache.commons.math3.geometry.euclidean.threed;
    018
    019    import org.apache.commons.math3.exception.MathArithmeticException;
    020    import org.apache.commons.math3.exception.util.LocalizedFormats;
    021    import org.apache.commons.math3.geometry.Vector;
    022    import org.apache.commons.math3.geometry.euclidean.oned.Vector1D;
    023    import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D;
    024    import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet;
    025    import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
    026    import org.apache.commons.math3.geometry.partitioning.Embedding;
    027    import org.apache.commons.math3.geometry.partitioning.Hyperplane;
    028    import org.apache.commons.math3.util.FastMath;
    029
    030    /** The class represent planes in a three dimensional space.
    031     * @version $Id: Plane.java 1416643 2012-12-03 19:37:14Z tn $
    032     * @since 3.0
    033     */
    034    public class Plane implements Hyperplane<Euclidean3D>, Embedding<Euclidean3D, Euclidean2D> {
    035
    036        /** Offset of the origin with respect to the plane. */
    037        private double originOffset;
    038
    039        /** Origin of the plane frame. */
    040        private Vector3D origin;
    041
    042        /** First vector of the plane frame (in plane). */
    043        private Vector3D u;
    044
    045        /** Second vector of the plane frame (in plane). */
    046        private Vector3D v;
    047
    048        /** Third vector of the plane frame (plane normal). */
    049        private Vector3D w;
    050
    051        /** Build a plane normal to a given direction and containing the origin.
    052         * @param normal normal direction to the plane
    053         * @exception MathArithmeticException if the normal norm is too small
    054         */
    055        public Plane(final Vector3D normal) throws MathArithmeticException {
    056            setNormal(normal);
    057            originOffset = 0;
    058            setFrame();
    059        }
    060
    061        /** Build a plane from a point and a normal.
    062         * @param p point belonging to the plane
    063         * @param normal normal direction to the plane
    064         * @exception MathArithmeticException if the normal norm is too small
    065         */
    066        public Plane(final Vector3D p, final Vector3D normal) throws MathArithmeticException {
    067            setNormal(normal);
    068            originOffset = -p.dotProduct(w);
    069            setFrame();
    070        }
    071
    072        /** Build a plane from three points.
    073         * <p>The plane is oriented in the direction of
    074         * {@code (p2-p1) ^ (p3-p1)}</p>
    075         * @param p1 first point belonging to the plane
    076         * @param p2 second point belonging to the plane
    077         * @param p3 third point belonging to the plane
    078         * @exception MathArithmeticException if the points do not constitute a plane
    079         */
    080        public Plane(final Vector3D p1, final Vector3D p2, final Vector3D p3)
    081            throws MathArithmeticException {
    082            this(p1, p2.subtract(p1).crossProduct(p3.subtract(p1)));
    083        }
    084
    085        /** Copy constructor.
    086         * <p>The instance created is completely independant of the original
    087         * one. A deep copy is used, none of the underlying object are
    088         * shared.</p>
    089         * @param plane plane to copy
    090         */
    091        public Plane(final Plane plane) {
    092            originOffset = plane.originOffset;
    093            origin = plane.origin;
    094            u      = plane.u;
    095            v      = plane.v;
    096            w      = plane.w;
    097        }
    098
    099        /** Copy the instance.
    100         * <p>The instance created is completely independant of the original
    101         * one. A deep copy is used, none of the underlying objects are
    102         * shared (except for immutable objects).</p>
    103         * @return a new hyperplane, copy of the instance
    104         */
    105        public Plane copySelf() {
    106            return new Plane(this);
    107        }
    108
    109        /** Reset the instance as if built from a point and a normal.
    110         * @param p point belonging to the plane
    111         * @param normal normal direction to the plane
    112         * @exception MathArithmeticException if the normal norm is too small
    113         */
    114        public void reset(final Vector3D p, final Vector3D normal) throws MathArithmeticException {
    115            setNormal(normal);
    116            originOffset = -p.dotProduct(w);
    117            setFrame();
    118        }
    119
    120        /** Reset the instance from another one.
    121         * <p>The updated instance is completely independant of the original
    122         * one. A deep reset is used none of the underlying object is
    123         * shared.</p>
    124         * @param original plane to reset from
    125         */
    126        public void reset(final Plane original) {
    127            originOffset = original.originOffset;
    128            origin       = original.origin;
    129            u            = original.u;
    130            v            = original.v;
    131            w            = original.w;
    132        }
    133
    134        /** Set the normal vactor.
    135         * @param normal normal direction to the plane (will be copied)
    136         * @exception MathArithmeticException if the normal norm is too small
    137         */
    138        private void setNormal(final Vector3D normal) throws MathArithmeticException {
    139            final double norm = normal.getNorm();
    140            if (norm < 1.0e-10) {
    141                throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
    142            }
    143            w = new Vector3D(1.0 / norm, normal);
    144        }
    145
    146        /** Reset the plane frame.
    147         */
    148        private void setFrame() {
    149            origin = new Vector3D(-originOffset, w);
    150            u = w.orthogonal();
    151            v = Vector3D.crossProduct(w, u);
    152        }
    153
    154        /** Get the origin point of the plane frame.
    155         * <p>The point returned is the orthogonal projection of the
    156         * 3D-space origin in the plane.</p>
    157         * @return the origin point of the plane frame (point closest to the
    158         * 3D-space origin)
    159         */
    160        public Vector3D getOrigin() {
    161            return origin;
    162        }
    163
    164        /** Get the normalized normal vector.
    165         * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
    166         * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
    167         * frame).</p>
    168         * @return normalized normal vector
    169         * @see #getU
    170         * @see #getV
    171         */
    172        public Vector3D getNormal() {
    173            return w;
    174        }
    175
    176        /** Get the plane first canonical vector.
    177         * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
    178         * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
    179         * frame).</p>
    180         * @return normalized first canonical vector
    181         * @see #getV
    182         * @see #getNormal
    183         */
    184        public Vector3D getU() {
    185            return u;
    186        }
    187
    188        /** Get the plane second canonical vector.
    189         * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
    190         * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
    191         * frame).</p>
    192         * @return normalized second canonical vector
    193         * @see #getU
    194         * @see #getNormal
    195         */
    196        public Vector3D getV() {
    197            return v;
    198        }
    199
    200        /** Revert the plane.
    201         * <p>Replace the instance by a similar plane with opposite orientation.</p>
    202         * <p>The new plane frame is chosen in such a way that a 3D point that had
    203         * {@code (x, y)} in-plane coordinates and {@code z} offset with
    204         * respect to the plane and is unaffected by the change will have
    205         * {@code (y, x)} in-plane coordinates and {@code -z} offset with
    206         * respect to the new plane. This means that the {@code u} and {@code v}
    207         * vectors returned by the {@link #getU} and {@link #getV} methods are exchanged,
    208         * and the {@code w} vector returned by the {@link #getNormal} method is
    209         * reversed.</p>
    210         */
    211        public void revertSelf() {
    212            final Vector3D tmp = u;
    213            u = v;
    214            v = tmp;
    215            w = w.negate();
    216            originOffset = -originOffset;
    217        }
    218
    219        /** Transform a 3D space point into an in-plane point.
    220         * @param point point of the space (must be a {@link Vector3D
    221         * Vector3D} instance)
    222         * @return in-plane point (really a {@link
    223         * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance)
    224         * @see #toSpace
    225         */
    226        public Vector2D toSubSpace(final Vector<Euclidean3D> point) {
    227            return new Vector2D(point.dotProduct(u), point.dotProduct(v));
    228        }
    229
    230        /** Transform an in-plane point into a 3D space point.
    231         * @param point in-plane point (must be a {@link
    232         * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance)
    233         * @return 3D space point (really a {@link Vector3D Vector3D} instance)
    234         * @see #toSubSpace
    235         */
    236        public Vector3D toSpace(final Vector<Euclidean2D> point) {
    237            final Vector2D p2D = (Vector2D) point;
    238            return new Vector3D(p2D.getX(), u, p2D.getY(), v, -originOffset, w);
    239        }
    240
    241        /** Get one point from the 3D-space.
    242         * @param inPlane desired in-plane coordinates for the point in the
    243         * plane
    244         * @param offset desired offset for the point
    245         * @return one point in the 3D-space, with given coordinates and offset
    246         * relative to the plane
    247         */
    248        public Vector3D getPointAt(final Vector2D inPlane, final double offset) {
    249            return new Vector3D(inPlane.getX(), u, inPlane.getY(), v, offset - originOffset, w);
    250        }
    251
    252        /** Check if the instance is similar to another plane.
    253         * <p>Planes are considered similar if they contain the same
    254         * points. This does not mean they are equal since they can have
    255         * opposite normals.</p>
    256         * @param plane plane to which the instance is compared
    257         * @return true if the planes are similar
    258         */
    259        public boolean isSimilarTo(final Plane plane) {
    260            final double angle = Vector3D.angle(w, plane.w);
    261            return ((angle < 1.0e-10) && (FastMath.abs(originOffset - plane.originOffset) < 1.0e-10)) ||
    262                   ((angle > (FastMath.PI - 1.0e-10)) && (FastMath.abs(originOffset + plane.originOffset) < 1.0e-10));
    263        }
    264
    265        /** Rotate the plane around the specified point.
    266         * <p>The instance is not modified, a new instance is created.</p>
    267         * @param center rotation center
    268         * @param rotation vectorial rotation operator
    269         * @return a new plane
    270         */
    271        public Plane rotate(final Vector3D center, final Rotation rotation) {
    272
    273            final Vector3D delta = origin.subtract(center);
    274            final Plane plane = new Plane(center.add(rotation.applyTo(delta)),
    275                                          rotation.applyTo(w));
    276
    277            // make sure the frame is transformed as desired
    278            plane.u = rotation.applyTo(u);
    279            plane.v = rotation.applyTo(v);
    280
    281            return plane;
    282
    283        }
    284
    285        /** Translate the plane by the specified amount.
    286         * <p>The instance is not modified, a new instance is created.</p>
    287         * @param translation translation to apply
    288         * @return a new plane
    289         */
    290        public Plane translate(final Vector3D translation) {
    291
    292            final Plane plane = new Plane(origin.add(translation), w);
    293
    294            // make sure the frame is transformed as desired
    295            plane.u = u;
    296            plane.v = v;
    297
    298            return plane;
    299
    300        }
    301
    302        /** Get the intersection of a line with the instance.
    303         * @param line line intersecting the instance
    304         * @return intersection point between between the line and the
    305         * instance (null if the line is parallel to the instance)
    306         */
    307        public Vector3D intersection(final Line line) {
    308            final Vector3D direction = line.getDirection();
    309            final double   dot       = w.dotProduct(direction);
    310            if (FastMath.abs(dot) < 1.0e-10) {
    311                return null;
    312            }
    313            final Vector3D point = line.toSpace(Vector1D.ZERO);
    314            final double   k     = -(originOffset + w.dotProduct(point)) / dot;
    315            return new Vector3D(1.0, point, k, direction);
    316        }
    317
    318        /** Build the line shared by the instance and another plane.
    319         * @param other other plane
    320         * @return line at the intersection of the instance and the
    321         * other plane (really a {@link Line Line} instance)
    322         */
    323        public Line intersection(final Plane other) {
    324            final Vector3D direction = Vector3D.crossProduct(w, other.w);
    325            if (direction.getNorm() < 1.0e-10) {
    326                return null;
    327            }
    328            final Vector3D point = intersection(this, other, new Plane(direction));
    329            return new Line(point, point.add(direction));
    330        }
    331
    332        /** Get the intersection point of three planes.
    333         * @param plane1 first plane1
    334         * @param plane2 second plane2
    335         * @param plane3 third plane2
    336         * @return intersection point of three planes, null if some planes are parallel
    337         */
    338        public static Vector3D intersection(final Plane plane1, final Plane plane2, final Plane plane3) {
    339
    340            // coefficients of the three planes linear equations
    341            final double a1 = plane1.w.getX();
    342            final double b1 = plane1.w.getY();
    343            final double c1 = plane1.w.getZ();
    344            final double d1 = plane1.originOffset;
    345
    346            final double a2 = plane2.w.getX();
    347            final double b2 = plane2.w.getY();
    348            final double c2 = plane2.w.getZ();
    349            final double d2 = plane2.originOffset;
    350
    351            final double a3 = plane3.w.getX();
    352            final double b3 = plane3.w.getY();
    353            final double c3 = plane3.w.getZ();
    354            final double d3 = plane3.originOffset;
    355
    356            // direct Cramer resolution of the linear system
    357            // (this is still feasible for a 3x3 system)
    358            final double a23         = b2 * c3 - b3 * c2;
    359            final double b23         = c2 * a3 - c3 * a2;
    360            final double c23         = a2 * b3 - a3 * b2;
    361            final double determinant = a1 * a23 + b1 * b23 + c1 * c23;
    362            if (FastMath.abs(determinant) < 1.0e-10) {
    363                return null;
    364            }
    365
    366            final double r = 1.0 / determinant;
    367            return new Vector3D(
    368                                (-a23 * d1 - (c1 * b3 - c3 * b1) * d2 - (c2 * b1 - c1 * b2) * d3) * r,
    369                                (-b23 * d1 - (c3 * a1 - c1 * a3) * d2 - (c1 * a2 - c2 * a1) * d3) * r,
    370                                (-c23 * d1 - (b1 * a3 - b3 * a1) * d2 - (b2 * a1 - b1 * a2) * d3) * r);
    371
    372        }
    373
    374        /** Build a region covering the whole hyperplane.
    375         * @return a region covering the whole hyperplane
    376         */
    377        public SubPlane wholeHyperplane() {
    378            return new SubPlane(this, new PolygonsSet());
    379        }
    380
    381        /** Build a region covering the whole space.
    382         * @return a region containing the instance (really a {@link
    383         * PolyhedronsSet PolyhedronsSet} instance)
    384         */
    385        public PolyhedronsSet wholeSpace() {
    386            return new PolyhedronsSet();
    387        }
    388
    389        /** Check if the instance contains a point.
    390         * @param p point to check
    391         * @return true if p belongs to the plane
    392         */
    393        public boolean contains(final Vector3D p) {
    394            return FastMath.abs(getOffset(p)) < 1.0e-10;
    395        }
    396
    397        /** Get the offset (oriented distance) of a parallel plane.
    398         * <p>This method should be called only for parallel planes otherwise
    399         * the result is not meaningful.</p>
    400         * <p>The offset is 0 if both planes are the same, it is
    401         * positive if the plane is on the plus side of the instance and
    402         * negative if it is on the minus side, according to its natural
    403         * orientation.</p>
    404         * @param plane plane to check
    405         * @return offset of the plane
    406         */
    407        public double getOffset(final Plane plane) {
    408            return originOffset + (sameOrientationAs(plane) ? -plane.originOffset : plane.originOffset);
    409        }
    410
    411        /** Get the offset (oriented distance) of a point.
    412         * <p>The offset is 0 if the point is on the underlying hyperplane,
    413         * it is positive if the point is on one particular side of the
    414         * hyperplane, and it is negative if the point is on the other side,
    415         * according to the hyperplane natural orientation.</p>
    416         * @param point point to check
    417         * @return offset of the point
    418         */
    419        public double getOffset(final Vector<Euclidean3D> point) {
    420            return point.dotProduct(w) + originOffset;
    421        }
    422
    423        /** Check if the instance has the same orientation as another hyperplane.
    424         * @param other other hyperplane to check against the instance
    425         * @return true if the instance and the other hyperplane have
    426         * the same orientation
    427         */
    428        public boolean sameOrientationAs(final Hyperplane<Euclidean3D> other) {
    429            return (((Plane) other).w).dotProduct(w) > 0.0;
    430        }
    431
    432    }