Plane.java

/*
 * 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.geometry.euclidean.threed;

import java.util.Objects;

import org.apache.commons.geometry.core.Transform;
import org.apache.commons.geometry.core.partitioning.AbstractHyperplane;
import org.apache.commons.geometry.core.partitioning.Hyperplane;
import org.apache.commons.geometry.euclidean.threed.line.Line3D;
import org.apache.commons.geometry.euclidean.threed.line.Lines3D;
import org.apache.commons.geometry.euclidean.threed.rotation.QuaternionRotation;
import org.apache.commons.geometry.euclidean.twod.ConvexArea;
import org.apache.commons.numbers.core.Precision;

/** Class representing a plane in 3 dimensional Euclidean space. Each plane is defined by a
 * {@link #getNormal() normal} and an {@link #getOriginOffset() origin offset}. If \(\vec{n}\) is the plane normal,
 * \(d\) is the origin offset, and \(p\) and \(q\) are any points in the plane, then the following are true:
 * <ul>
 *  <li>\(\lVert \vec{n} \rVert\) = 1</li>
 *  <li>\(\vec{n} \cdot (p - q) = 0\)</li>
 *  <li>\(d = - (\vec{n} \cdot q)\)</li>
 *  </ul>
 *  In other words, the normal is a unit vector such that the dot product of the normal and the difference of
 *  any two points in the plane is always equal to \(0\). Similarly, the {@code origin offset} is equal to the
 *  negation of the dot product of the normal and any point in the plane. The projection of the origin onto the
 *  plane (given by {@link #getOrigin()}), is computed as \(-d \vec{n}\).
 *
 * <p>Instances of this class are guaranteed to be immutable.</p>
 * @see Planes
 */
public class Plane extends AbstractHyperplane<Vector3D> {

    /** Plane normal. */
    private final Vector3D.Unit normal;

    /** Offset of the origin with respect to the plane. */
    private final double originOffset;

    /** Construct a plane from its component parts.
     * @param normal unit normal vector
     * @param originOffset offset of the origin with respect to the plane
     * @param precision precision context used to compare floating point values
     */
    Plane(final Vector3D.Unit normal, final double originOffset,
          final Precision.DoubleEquivalence precision) {

        super(precision);

        this.normal = normal;
        this.originOffset = originOffset;
    }

    /** Get the orthogonal projection of the 3D-space origin in the plane.
     * @return the origin point of the plane frame (point closest to the 3D-space
     *         origin)
     */
    public Vector3D getOrigin() {
        return normal.multiply(-originOffset);
    }

    /** Get the offset of the spatial origin ({@code 0, 0, 0}) with respect to the plane.
     * @return the offset of the origin with respect to the plane.
     */
    public double getOriginOffset() {
        return originOffset;
    }

    /** Get the plane normal vector.
     * @return plane normal vector
     */
    public Vector3D.Unit getNormal() {
        return normal;
    }

    /** Return an {@link EmbeddingPlane} instance suitable for embedding 2D geometric objects
     * into this plane. Returned instances are guaranteed to be equal between invocations.
     * @return a plane instance suitable for embedding 2D subspaces
     */
    public EmbeddingPlane getEmbedding() {
        final Vector3D.Unit u = normal.orthogonal();
        final Vector3D.Unit v = normal.cross(u).normalize();

        return new EmbeddingPlane(u, v, normal, originOffset, getPrecision());
    }

    /** {@inheritDoc} */
    @Override
    public double offset(final Vector3D point) {
        return point.dot(normal) + originOffset;
    }

    /** Get the offset (oriented distance) of the given line with respect to the plane. The value
     * closest to zero is returned, which will always be zero if the line is not parallel to the plane.
     * @param line line to calculate the offset of
     * @return the offset of the line with respect to the plane or 0.0 if the line
     *      is not parallel to the plane.
     */
    public double offset(final Line3D line) {
        if (!isParallel(line)) {
            return 0.0;
        }
        return offset(line.getOrigin());
    }

    /** Get the offset (oriented distance) of the given plane with respect to this instance. The value
     * closest to zero is returned, which will always be zero if the planes are not parallel.
     * @param plane plane to calculate the offset of
     * @return the offset of the plane with respect to this instance or 0.0 if the planes
     *      are not parallel.
     */
    public double offset(final Plane plane) {
        if (!isParallel(plane)) {
            return 0.0;
        }
        return originOffset + (similarOrientation(plane) ? -plane.originOffset : plane.originOffset);
    }

    /** Check if the instance contains a point.
     * @param p point to check
     * @return true if p belongs to the plane
     */
    @Override
    public boolean contains(final Vector3D p) {
        return getPrecision().eqZero(offset(p));
    }

    /** Check if the instance contains a line.
     * @param line line to check
     * @return true if line is contained in this plane
     */
    public boolean contains(final Line3D line) {
        return isParallel(line) && contains(line.getOrigin());
    }

    /** Check if the instance contains another plane. Planes are considered similar if they contain
     * the same points. This does not mean they are equal since they can have opposite normals.
     * @param plane plane to which the instance is compared
     * @return true if the planes are similar
     */
    public boolean contains(final Plane plane) {
        final double angle = normal.angle(plane.normal);
        final Precision.DoubleEquivalence precision = getPrecision();

        return ((precision.eqZero(angle)) && precision.eq(originOffset, plane.originOffset)) ||
                ((precision.eq(angle, Math.PI)) && precision.eq(originOffset, -plane.originOffset));
    }

    /** {@inheritDoc} */
    @Override
    public Vector3D project(final Vector3D point) {
        return getOrigin().add(point.reject(normal));
    }

    /** Project a 3D line onto the plane.
     * @param line the line to project
     * @return the projection of the given line onto the plane.
     */
    public Line3D project(final Line3D line) {
        final Vector3D direction = line.getDirection();
        final Vector3D projection = normal.multiply(direction.dot(normal) * (1 / normal.normSq()));

        final Vector3D projectedLineDirection = direction.subtract(projection);
        final Vector3D p1 = project(line.getOrigin());
        final Vector3D p2 = p1.add(projectedLineDirection);

        return Lines3D.fromPoints(p1, p2, getPrecision());
    }

    /** {@inheritDoc} */
    @Override
    public PlaneConvexSubset span() {
        return Planes.subsetFromConvexArea(getEmbedding(), ConvexArea.full());
    }

    /** Check if the line is parallel to the instance.
     * @param line line to check.
     * @return true if the line is parallel to the instance, false otherwise.
     */
    public boolean isParallel(final Line3D line) {
        final double dot = normal.dot(line.getDirection());

        return getPrecision().eqZero(dot);
    }

    /** Check if the plane is parallel to the instance.
     * @param plane plane to check.
     * @return true if the plane is parallel to the instance, false otherwise.
     */
    public boolean isParallel(final Plane plane) {
        return getPrecision().eqZero(normal.cross(plane.normal).norm());
    }

    /** {@inheritDoc} */
    @Override
    public boolean similarOrientation(final Hyperplane<Vector3D> other) {
        return (((Plane) other).normal).dot(normal) > 0;
    }

    /** Get the intersection of a line with this plane.
     * @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 Line3D line) {
        final Vector3D direction = line.getDirection();
        final double dot = normal.dot(direction);

        if (getPrecision().eqZero(dot)) {
            return null;
        }

        final Vector3D point = line.pointAt(0);
        final double k = -(originOffset + normal.dot(point)) / dot;

        return Vector3D.Sum.of(point)
                .addScaled(k, direction)
                .get();
    }

    /** Get the line formed by the intersection of this instance with the given plane.
     * The returned line lies in both planes and points in the direction of
     * the cross product <code>n<sub>1</sub> x n<sub>2</sub></code>, where <code>n<sub>1</sub></code>
     * is the normal of the current instance and <code>n<sub>2</sub></code> is the normal
     * of the argument.
     *
     * <p>Null is returned if the planes are parallel.</p>
     *
     * @param other other plane
     * @return line at the intersection of the instance and the other plane, or null
     *      if no such line exists
     */
    public Line3D intersection(final Plane other) {
        final Vector3D direction = normal.cross(other.normal);

        if (getPrecision().eqZero(direction.norm())) {
            return null;
        }

        final Vector3D point = intersection(this, other, Planes.fromNormal(direction, getPrecision()));

        return Lines3D.fromPointAndDirection(point, direction, getPrecision());
    }

    /** Build a new reversed version of this plane, with opposite orientation.
     * @return a new reversed plane
     */
    @Override
    public Plane reverse() {
        return new Plane(normal.negate(), -originOffset, getPrecision());
    }

    /** {@inheritDoc}
     *
     * <p>Instances are transformed by selecting 3 representative points from the
     * plane, transforming them, and constructing a new plane from the transformed points.
     * Since the normal is not transformed directly, but rather is constructed new from the
     * transformed points, the relative orientations of points in the plane are preserved,
     * even for transforms that do not
     * {@link Transform#preservesOrientation() preserve orientation}. The example below shows
     * a plane being transformed by a non-orientation-preserving transform. The normal of the
     * transformed plane retains its counterclockwise relationship to the points in the plane,
     * in contrast with the normal that is transformed directly by the transform.
     * </p>
     * <pre>
     * // construct a plane from 3 points; the normal will be selected such that the
     * // points are ordered counterclockwise when looking down the plane normal.
     * Vector3D p1 = Vector3D.of(0, 0, 0);
     * Vector3D p2 = Vector3D.of(+1, 0, 0);
     * Vector3D p3 = Vector3D.of(0, +1, 0);
     *
     * Plane plane = Planes.fromPoints(p1, p2, p3, precision); // normal is (0, 0, +1)
     *
     * // create a transform that negates all x-values; this transform does not
     * // preserve orientation, i.e. it will convert a right-handed system into a left-handed
     * // system and vice versa
     * AffineTransformMatrix3D transform = AffineTransformMatrix3D.createScale(-1, 1,  1);
     *
     * // transform the plane
     * Plane transformedPlane = plane.transform(transform);
     *
     * // the plane normal is oriented such that transformed points are still ordered
     * // counterclockwise when looking down the plane normal; since the point (1, 0, 0) has
     * // now become (-1, 0, 0), the normal has flipped to (0, 0, -1)
     * transformedPlane.getNormal();
     *
     * // directly transform the original plane normal; the normal is unchanged by the transform
     * // since the target space of the transform is left-handed
     * AffineTransformMatrix3D normalTransform = transform.normalTransform();
     * Vector3D directlyTransformedNormal = normalTransform.apply(plane.getNormal()); // (0, 0, +1)
     * </pre>
     */
    @Override
    public Plane transform(final Transform<Vector3D> transform) {
        // create 3 representation points lying on the plane, transform them,
        // and use the transformed points to create a new plane

        final Vector3D u = normal.orthogonal();
        final Vector3D v = normal.cross(u);

        final Vector3D p1 = getOrigin();
        final Vector3D p2 = p1.add(u);
        final Vector3D p3 = p1.add(v);

        final Vector3D t1 = transform.apply(p1);
        final Vector3D t2 = transform.apply(p2);
        final Vector3D t3 = transform.apply(p3);

        return Planes.fromPoints(t1, t2, t3, getPrecision());
    }

    /** Translate the plane by the specified amount.
     * @param translation translation to apply
     * @return a new plane
     */
    public Plane translate(final Vector3D translation) {
        final Vector3D tOrigin = getOrigin().add(translation);

        return Planes.fromPointAndNormal(tOrigin, normal, getPrecision());
    }

    /** Rotate the plane around the specified point.
     * @param center rotation center
     * @param rotation 3-dimensional rotation
     * @return a new plane
     */
    public Plane rotate(final Vector3D center, final QuaternionRotation rotation) {
        final Vector3D delta = getOrigin().subtract(center);
        final Vector3D tOrigin = center.add(rotation.apply(delta));

        // we can directly apply the rotation to the normal since it will transform
        // it properly (there is no translation or scaling involved)
        final Vector3D.Unit tNormal = rotation.apply(normal).normalize();

        return Planes.fromPointAndNormal(tOrigin, tNormal, getPrecision());
    }

    /** Return true if this instance should be considered equivalent to the argument, using the
     * given precision context for comparison. Instances are considered equivalent if they contain
     * the same points, which is determined by comparing the plane {@code origins} and {@code normals}.
     * @param other the point to compare with
     * @param precision precision context to use for the comparison
     * @return true if this instance should be considered equivalent to the argument
     * @see Vector3D#eq(Vector3D, Precision.DoubleEquivalence)
     */
    public boolean eq(final Plane other, final Precision.DoubleEquivalence precision) {
        return getOrigin().eq(other.getOrigin(), precision) &&
                normal.eq(other.normal, precision);
    }

    /** {@inheritDoc} */
    @Override
    public int hashCode() {
        return Objects.hash(normal, originOffset, getPrecision());
    }

    /** {@inheritDoc} */
    @Override
    public boolean equals(final Object obj) {
        if (this == obj) {
            return true;
        } else if (obj == null || obj.getClass() != this.getClass()) {
            return false;
        }

        final Plane other = (Plane) obj;

        return Objects.equals(this.normal, other.normal) &&
                Double.compare(this.originOffset, other.originOffset) == 0 &&
                Objects.equals(this.getPrecision(), other.getPrecision());
    }

    /** {@inheritDoc} */
    @Override
    public String toString() {
        final StringBuilder sb = new StringBuilder();
        sb.append(getClass().getSimpleName())
            .append("[origin= ")
            .append(getOrigin())
            .append(", normal= ")
            .append(normal)
            .append(']');

        return sb.toString();
    }

    /** Get the intersection point of three planes. Returns null if no unique intersection point
     * exists (ie, there are no intersection points or an infinite number).
     * @param plane1 first plane1
     * @param plane2 second plane2
     * @param plane3 third plane2
     * @return intersection point of the three planes or null if no unique intersection point exists
     */
    public static Vector3D intersection(final Plane plane1, final Plane plane2, final Plane plane3) {

        // coefficients of the three planes linear equations
        final double a1 = plane1.normal.getX();
        final double b1 = plane1.normal.getY();
        final double c1 = plane1.normal.getZ();
        final double d1 = plane1.originOffset;

        final double a2 = plane2.normal.getX();
        final double b2 = plane2.normal.getY();
        final double c2 = plane2.normal.getZ();
        final double d2 = plane2.originOffset;

        final double a3 = plane3.normal.getX();
        final double b3 = plane3.normal.getY();
        final double c3 = plane3.normal.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);

        // use the precision context of the first plane to determine equality
        if (plane1.getPrecision().eqZero(determinant)) {
            return null;
        }

        final double r = 1.0 / determinant;
        return Vector3D.of((-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);
    }
}