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 */
017package org.apache.commons.math3.geometry.euclidean.threed;
018
019import org.apache.commons.math3.exception.MathArithmeticException;
020import org.apache.commons.math3.exception.util.LocalizedFormats;
021import org.apache.commons.math3.geometry.Point;
022import org.apache.commons.math3.geometry.Vector;
023import org.apache.commons.math3.geometry.euclidean.oned.Euclidean1D;
024import org.apache.commons.math3.geometry.euclidean.oned.Vector1D;
025import org.apache.commons.math3.geometry.euclidean.twod.Euclidean2D;
026import org.apache.commons.math3.geometry.euclidean.twod.PolygonsSet;
027import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
028import org.apache.commons.math3.geometry.partitioning.Embedding;
029import org.apache.commons.math3.geometry.partitioning.Hyperplane;
030import org.apache.commons.math3.util.FastMath;
031
032/** The class represent planes in a three dimensional space.
033 * @since 3.0
034 */
035public class Plane implements Hyperplane<Euclidean3D>, Embedding<Euclidean3D, Euclidean2D> {
036
037    /** Default value for tolerance. */
038    private static final double DEFAULT_TOLERANCE = 1.0e-10;
039
040    /** Offset of the origin with respect to the plane. */
041    private double originOffset;
042
043    /** Origin of the plane frame. */
044    private Vector3D origin;
045
046    /** First vector of the plane frame (in plane). */
047    private Vector3D u;
048
049    /** Second vector of the plane frame (in plane). */
050    private Vector3D v;
051
052    /** Third vector of the plane frame (plane normal). */
053    private Vector3D w;
054
055    /** Tolerance below which points are considered identical. */
056    private final double tolerance;
057
058    /** Build a plane normal to a given direction and containing the origin.
059     * @param normal normal direction to the plane
060     * @param tolerance tolerance below which points are considered identical
061     * @exception MathArithmeticException if the normal norm is too small
062     * @since 3.3
063     */
064    public Plane(final Vector3D normal, final double tolerance)
065        throws MathArithmeticException {
066        setNormal(normal);
067        this.tolerance = tolerance;
068        originOffset = 0;
069        setFrame();
070    }
071
072    /** Build a plane from a point and a normal.
073     * @param p point belonging to the plane
074     * @param normal normal direction to the plane
075     * @param tolerance tolerance below which points are considered identical
076     * @exception MathArithmeticException if the normal norm is too small
077     * @since 3.3
078     */
079    public Plane(final Vector3D p, final Vector3D normal, final double tolerance)
080        throws MathArithmeticException {
081        setNormal(normal);
082        this.tolerance = tolerance;
083        originOffset = -p.dotProduct(w);
084        setFrame();
085    }
086
087    /** Build a plane from three points.
088     * <p>The plane is oriented in the direction of
089     * {@code (p2-p1) ^ (p3-p1)}</p>
090     * @param p1 first point belonging to the plane
091     * @param p2 second point belonging to the plane
092     * @param p3 third point belonging to the plane
093     * @param tolerance tolerance below which points are considered identical
094     * @exception MathArithmeticException if the points do not constitute a plane
095     * @since 3.3
096     */
097    public Plane(final Vector3D p1, final Vector3D p2, final Vector3D p3, final double tolerance)
098        throws MathArithmeticException {
099        this(p1, p2.subtract(p1).crossProduct(p3.subtract(p1)), tolerance);
100    }
101
102    /** Build a plane normal to a given direction and containing the origin.
103     * @param normal normal direction to the plane
104     * @exception MathArithmeticException if the normal norm is too small
105     * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, double)}
106     */
107    @Deprecated
108    public Plane(final Vector3D normal) throws MathArithmeticException {
109        this(normal, DEFAULT_TOLERANCE);
110    }
111
112    /** Build a plane from a point and a normal.
113     * @param p point belonging to the plane
114     * @param normal normal direction to the plane
115     * @exception MathArithmeticException if the normal norm is too small
116     * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, Vector3D, double)}
117     */
118    @Deprecated
119    public Plane(final Vector3D p, final Vector3D normal) throws MathArithmeticException {
120        this(p, normal, DEFAULT_TOLERANCE);
121    }
122
123    /** Build a plane from three points.
124     * <p>The plane is oriented in the direction of
125     * {@code (p2-p1) ^ (p3-p1)}</p>
126     * @param p1 first point belonging to the plane
127     * @param p2 second point belonging to the plane
128     * @param p3 third point belonging to the plane
129     * @exception MathArithmeticException if the points do not constitute a plane
130     * @deprecated as of 3.3, replaced with {@link #Plane(Vector3D, Vector3D, Vector3D, double)}
131     */
132    @Deprecated
133    public Plane(final Vector3D p1, final Vector3D p2, final Vector3D p3)
134        throws MathArithmeticException {
135        this(p1, p2, p3, DEFAULT_TOLERANCE);
136    }
137
138    /** Copy constructor.
139     * <p>The instance created is completely independant of the original
140     * one. A deep copy is used, none of the underlying object are
141     * shared.</p>
142     * @param plane plane to copy
143     */
144    public Plane(final Plane plane) {
145        originOffset = plane.originOffset;
146        origin       = plane.origin;
147        u            = plane.u;
148        v            = plane.v;
149        w            = plane.w;
150        tolerance    = plane.tolerance;
151    }
152
153    /** Copy the instance.
154     * <p>The instance created is completely independant of the original
155     * one. A deep copy is used, none of the underlying objects are
156     * shared (except for immutable objects).</p>
157     * @return a new hyperplane, copy of the instance
158     */
159    public Plane copySelf() {
160        return new Plane(this);
161    }
162
163    /** Reset the instance as if built from a point and a normal.
164     * @param p point belonging to the plane
165     * @param normal normal direction to the plane
166     * @exception MathArithmeticException if the normal norm is too small
167     */
168    public void reset(final Vector3D p, final Vector3D normal) throws MathArithmeticException {
169        setNormal(normal);
170        originOffset = -p.dotProduct(w);
171        setFrame();
172    }
173
174    /** Reset the instance from another one.
175     * <p>The updated instance is completely independant of the original
176     * one. A deep reset is used none of the underlying object is
177     * shared.</p>
178     * @param original plane to reset from
179     */
180    public void reset(final Plane original) {
181        originOffset = original.originOffset;
182        origin       = original.origin;
183        u            = original.u;
184        v            = original.v;
185        w            = original.w;
186    }
187
188    /** Set the normal vactor.
189     * @param normal normal direction to the plane (will be copied)
190     * @exception MathArithmeticException if the normal norm is too small
191     */
192    private void setNormal(final Vector3D normal) throws MathArithmeticException {
193        final double norm = normal.getNorm();
194        if (norm < 1.0e-10) {
195            throw new MathArithmeticException(LocalizedFormats.ZERO_NORM);
196        }
197        w = new Vector3D(1.0 / norm, normal);
198    }
199
200    /** Reset the plane frame.
201     */
202    private void setFrame() {
203        origin = new Vector3D(-originOffset, w);
204        u = w.orthogonal();
205        v = Vector3D.crossProduct(w, u);
206    }
207
208    /** Get the origin point of the plane frame.
209     * <p>The point returned is the orthogonal projection of the
210     * 3D-space origin in the plane.</p>
211     * @return the origin point of the plane frame (point closest to the
212     * 3D-space origin)
213     */
214    public Vector3D getOrigin() {
215        return origin;
216    }
217
218    /** Get the normalized normal vector.
219     * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
220     * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
221     * frame).</p>
222     * @return normalized normal vector
223     * @see #getU
224     * @see #getV
225     */
226    public Vector3D getNormal() {
227        return w;
228    }
229
230    /** Get the plane first canonical vector.
231     * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
232     * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
233     * frame).</p>
234     * @return normalized first canonical vector
235     * @see #getV
236     * @see #getNormal
237     */
238    public Vector3D getU() {
239        return u;
240    }
241
242    /** Get the plane second canonical vector.
243     * <p>The frame defined by ({@link #getU getU}, {@link #getV getV},
244     * {@link #getNormal getNormal}) is a rigth-handed orthonormalized
245     * frame).</p>
246     * @return normalized second canonical vector
247     * @see #getU
248     * @see #getNormal
249     */
250    public Vector3D getV() {
251        return v;
252    }
253
254    /** {@inheritDoc}
255     * @since 3.3
256     */
257    public Point<Euclidean3D> project(Point<Euclidean3D> point) {
258        return toSpace(toSubSpace(point));
259    }
260
261    /** {@inheritDoc}
262     * @since 3.3
263     */
264    public double getTolerance() {
265        return tolerance;
266    }
267
268    /** Revert the plane.
269     * <p>Replace the instance by a similar plane with opposite orientation.</p>
270     * <p>The new plane frame is chosen in such a way that a 3D point that had
271     * {@code (x, y)} in-plane coordinates and {@code z} offset with
272     * respect to the plane and is unaffected by the change will have
273     * {@code (y, x)} in-plane coordinates and {@code -z} offset with
274     * respect to the new plane. This means that the {@code u} and {@code v}
275     * vectors returned by the {@link #getU} and {@link #getV} methods are exchanged,
276     * and the {@code w} vector returned by the {@link #getNormal} method is
277     * reversed.</p>
278     */
279    public void revertSelf() {
280        final Vector3D tmp = u;
281        u = v;
282        v = tmp;
283        w = w.negate();
284        originOffset = -originOffset;
285    }
286
287    /** Transform a space point into a sub-space point.
288     * @param vector n-dimension point of the space
289     * @return (n-1)-dimension point of the sub-space corresponding to
290     * the specified space point
291     */
292    public Vector2D toSubSpace(Vector<Euclidean3D> vector) {
293        return toSubSpace((Point<Euclidean3D>) vector);
294    }
295
296    /** Transform a sub-space point into a space point.
297     * @param vector (n-1)-dimension point of the sub-space
298     * @return n-dimension point of the space corresponding to the
299     * specified sub-space point
300     */
301    public Vector3D toSpace(Vector<Euclidean2D> vector) {
302        return toSpace((Point<Euclidean2D>) vector);
303    }
304
305    /** Transform a 3D space point into an in-plane point.
306     * @param point point of the space (must be a {@link Vector3D
307     * Vector3D} instance)
308     * @return in-plane point (really a {@link
309     * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance)
310     * @see #toSpace
311     */
312    public Vector2D toSubSpace(final Point<Euclidean3D> point) {
313        final Vector3D p3D = (Vector3D) point;
314        return new Vector2D(p3D.dotProduct(u), p3D.dotProduct(v));
315    }
316
317    /** Transform an in-plane point into a 3D space point.
318     * @param point in-plane point (must be a {@link
319     * org.apache.commons.math3.geometry.euclidean.twod.Vector2D Vector2D} instance)
320     * @return 3D space point (really a {@link Vector3D Vector3D} instance)
321     * @see #toSubSpace
322     */
323    public Vector3D toSpace(final Point<Euclidean2D> point) {
324        final Vector2D p2D = (Vector2D) point;
325        return new Vector3D(p2D.getX(), u, p2D.getY(), v, -originOffset, w);
326    }
327
328    /** Get one point from the 3D-space.
329     * @param inPlane desired in-plane coordinates for the point in the
330     * plane
331     * @param offset desired offset for the point
332     * @return one point in the 3D-space, with given coordinates and offset
333     * relative to the plane
334     */
335    public Vector3D getPointAt(final Vector2D inPlane, final double offset) {
336        return new Vector3D(inPlane.getX(), u, inPlane.getY(), v, offset - originOffset, w);
337    }
338
339    /** Check if the instance is similar to another plane.
340     * <p>Planes are considered similar if they contain the same
341     * points. This does not mean they are equal since they can have
342     * opposite normals.</p>
343     * @param plane plane to which the instance is compared
344     * @return true if the planes are similar
345     */
346    public boolean isSimilarTo(final Plane plane) {
347        final double angle = Vector3D.angle(w, plane.w);
348        return ((angle < 1.0e-10) && (FastMath.abs(originOffset - plane.originOffset) < tolerance)) ||
349               ((angle > (FastMath.PI - 1.0e-10)) && (FastMath.abs(originOffset + plane.originOffset) < tolerance));
350    }
351
352    /** Rotate the plane around the specified point.
353     * <p>The instance is not modified, a new instance is created.</p>
354     * @param center rotation center
355     * @param rotation vectorial rotation operator
356     * @return a new plane
357     */
358    public Plane rotate(final Vector3D center, final Rotation rotation) {
359
360        final Vector3D delta = origin.subtract(center);
361        final Plane plane = new Plane(center.add(rotation.applyTo(delta)),
362                                      rotation.applyTo(w), tolerance);
363
364        // make sure the frame is transformed as desired
365        plane.u = rotation.applyTo(u);
366        plane.v = rotation.applyTo(v);
367
368        return plane;
369
370    }
371
372    /** Translate the plane by the specified amount.
373     * <p>The instance is not modified, a new instance is created.</p>
374     * @param translation translation to apply
375     * @return a new plane
376     */
377    public Plane translate(final Vector3D translation) {
378
379        final Plane plane = new Plane(origin.add(translation), w, tolerance);
380
381        // make sure the frame is transformed as desired
382        plane.u = u;
383        plane.v = v;
384
385        return plane;
386
387    }
388
389    /** Get the intersection of a line with the instance.
390     * @param line line intersecting the instance
391     * @return intersection point between between the line and the
392     * instance (null if the line is parallel to the instance)
393     */
394    public Vector3D intersection(final Line line) {
395        final Vector3D direction = line.getDirection();
396        final double   dot       = w.dotProduct(direction);
397        if (FastMath.abs(dot) < 1.0e-10) {
398            return null;
399        }
400        final Vector3D point = line.toSpace((Point<Euclidean1D>) Vector1D.ZERO);
401        final double   k     = -(originOffset + w.dotProduct(point)) / dot;
402        return new Vector3D(1.0, point, k, direction);
403    }
404
405    /** Build the line shared by the instance and another plane.
406     * @param other other plane
407     * @return line at the intersection of the instance and the
408     * other plane (really a {@link Line Line} instance)
409     */
410    public Line intersection(final Plane other) {
411        final Vector3D direction = Vector3D.crossProduct(w, other.w);
412        if (direction.getNorm() < tolerance) {
413            return null;
414        }
415        final Vector3D point = intersection(this, other, new Plane(direction, tolerance));
416        return new Line(point, point.add(direction), tolerance);
417    }
418
419    /** Get the intersection point of three planes.
420     * @param plane1 first plane1
421     * @param plane2 second plane2
422     * @param plane3 third plane2
423     * @return intersection point of three planes, null if some planes are parallel
424     */
425    public static Vector3D intersection(final Plane plane1, final Plane plane2, final Plane plane3) {
426
427        // coefficients of the three planes linear equations
428        final double a1 = plane1.w.getX();
429        final double b1 = plane1.w.getY();
430        final double c1 = plane1.w.getZ();
431        final double d1 = plane1.originOffset;
432
433        final double a2 = plane2.w.getX();
434        final double b2 = plane2.w.getY();
435        final double c2 = plane2.w.getZ();
436        final double d2 = plane2.originOffset;
437
438        final double a3 = plane3.w.getX();
439        final double b3 = plane3.w.getY();
440        final double c3 = plane3.w.getZ();
441        final double d3 = plane3.originOffset;
442
443        // direct Cramer resolution of the linear system
444        // (this is still feasible for a 3x3 system)
445        final double a23         = b2 * c3 - b3 * c2;
446        final double b23         = c2 * a3 - c3 * a2;
447        final double c23         = a2 * b3 - a3 * b2;
448        final double determinant = a1 * a23 + b1 * b23 + c1 * c23;
449        if (FastMath.abs(determinant) < 1.0e-10) {
450            return null;
451        }
452
453        final double r = 1.0 / determinant;
454        return new Vector3D(
455                            (-a23 * d1 - (c1 * b3 - c3 * b1) * d2 - (c2 * b1 - c1 * b2) * d3) * r,
456                            (-b23 * d1 - (c3 * a1 - c1 * a3) * d2 - (c1 * a2 - c2 * a1) * d3) * r,
457                            (-c23 * d1 - (b1 * a3 - b3 * a1) * d2 - (b2 * a1 - b1 * a2) * d3) * r);
458
459    }
460
461    /** Build a region covering the whole hyperplane.
462     * @return a region covering the whole hyperplane
463     */
464    public SubPlane wholeHyperplane() {
465        return new SubPlane(this, new PolygonsSet(tolerance));
466    }
467
468    /** Build a region covering the whole space.
469     * @return a region containing the instance (really a {@link
470     * PolyhedronsSet PolyhedronsSet} instance)
471     */
472    public PolyhedronsSet wholeSpace() {
473        return new PolyhedronsSet(tolerance);
474    }
475
476    /** Check if the instance contains a point.
477     * @param p point to check
478     * @return true if p belongs to the plane
479     */
480    public boolean contains(final Vector3D p) {
481        return FastMath.abs(getOffset(p)) < tolerance;
482    }
483
484    /** Get the offset (oriented distance) of a parallel plane.
485     * <p>This method should be called only for parallel planes otherwise
486     * the result is not meaningful.</p>
487     * <p>The offset is 0 if both planes are the same, it is
488     * positive if the plane is on the plus side of the instance and
489     * negative if it is on the minus side, according to its natural
490     * orientation.</p>
491     * @param plane plane to check
492     * @return offset of the plane
493     */
494    public double getOffset(final Plane plane) {
495        return originOffset + (sameOrientationAs(plane) ? -plane.originOffset : plane.originOffset);
496    }
497
498    /** Get the offset (oriented distance) of a vector.
499     * @param vector vector to check
500     * @return offset of the vector
501     */
502    public double getOffset(Vector<Euclidean3D> vector) {
503        return getOffset((Point<Euclidean3D>) vector);
504    }
505
506    /** Get the offset (oriented distance) of a point.
507     * <p>The offset is 0 if the point is on the underlying hyperplane,
508     * it is positive if the point is on one particular side of the
509     * hyperplane, and it is negative if the point is on the other side,
510     * according to the hyperplane natural orientation.</p>
511     * @param point point to check
512     * @return offset of the point
513     */
514    public double getOffset(final Point<Euclidean3D> point) {
515        return ((Vector3D) point).dotProduct(w) + originOffset;
516    }
517
518    /** Check if the instance has the same orientation as another hyperplane.
519     * @param other other hyperplane to check against the instance
520     * @return true if the instance and the other hyperplane have
521     * the same orientation
522     */
523    public boolean sameOrientationAs(final Hyperplane<Euclidean3D> other) {
524        return (((Plane) other).w).dotProduct(w) > 0.0;
525    }
526
527}