/*
 *   GeomUtil  -- Static utility methods used by the geometry package.
 *
 *   Copyright (C) 2000-2025, by Joseph A. Huwaldt
 *   All rights reserved.
 *
 *   This library is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU Lesser General Public
 *   License as published by the Free Software Foundation; either
 *   version 2 of the License, or (at your option) any later version.
 *
 *   This library is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *   Library General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *  Or visit:  http://www.gnu.org/licenses/lgpl.html
 */
package geomss.geom;

import jahuwaldt.js.param.Parameter;
import jahuwaldt.tools.math.MathTools;
import java.text.MessageFormat;
import java.util.ArrayList;
import java.util.Collection;
import java.util.List;
import static java.util.Objects.isNull;
import static java.util.Objects.nonNull;
import static java.util.Objects.requireNonNull;
import java.util.ResourceBundle;
import javax.measure.converter.UnitConverter;
import javax.measure.quantity.Angle;
import javax.measure.quantity.Dimensionless;
import javax.measure.quantity.Length;
import javax.measure.unit.NonSI;
import javax.measure.unit.SI;
import javax.measure.unit.Unit;
import javolution.context.ConcurrentContext;
import javolution.context.ImmortalContext;
import javolution.context.StackContext;
import static javolution.lang.MathLib.PI;
import static javolution.lang.MathLib.TWO_PI;
import static javolution.lang.MathLib.abs;
import static javolution.lang.MathLib.sqrt;
import javolution.util.FastTable;
import org.apache.commons.math3.geometry.euclidean.twod.Vector2D;
import org.apache.commons.math3.geometry.euclidean.twod.hull.AklToussaintHeuristic;
import org.apache.commons.math3.geometry.euclidean.twod.hull.MonotoneChain;
import org.jscience.mathematics.vector.Float64Matrix;
import org.jscience.mathematics.vector.Float64Vector;

/**
 * A collection of static methods used by classes in the geometry package.
 *
 * <p> Modified by: Joseph A. Huwaldt </p>
 *
 * @author Joseph A. Huwaldt, Date: March 31, 2009
 * @version February 17, 2025
 */
public final class GeomUtil {

    /**
     * The resource bundle for this package.
     */
    private static final ResourceBundle RESOURCES = AbstractGeomElement.RESOURCES;

    /**
     * The machine epsilon or unit roundoff for <code>double</code> in the Java
     * environment. Machine epsilon gives an upper bound on the relative error due to
     * rounding in floating point arithmetic.
     */
    public static final double EPS = MathTools.EPS;

    /**
     * The square-root of EPS.
     *
     * @see #EPS
     */
    public static final double SQRT_EPS = MathTools.SQRT_EPS;

    //  A default geometry tolerance.
    private static final Parameter<Length> GTOL;

    static {
        ImmortalContext.enter();
        try {
            GTOL = Parameter.valueOf(1e-8, SI.METER);
        } finally {
            ImmortalContext.exit();
        }
    }

    /**
     * Compute the average of an array of points (returning the mid-point between them all).
     *
     * @param pnts The array of points to average. There must be at least one point.
     * @return The average of the input points.
     */
    public static Point averagePoints(GeomPoint... pnts) {
        StackContext.enter();
        try {
            
            Point pavg = pnts[0].immutable();
            int numPnts = pnts.length;
            for (int i=1; i < numPnts; ++i) {
                pavg = pavg.plus(pnts[i]);
            }
            pavg = pavg.divide(numPnts);
            return StackContext.outerCopy(pavg);
            
        } finally {
            StackContext.exit();
        }
    }
    
    /**
     * Compute the average of a list of points (returning the mid-point between them all).
     *
     * @param pnts The list of points to average. There must be at least one point.
     * @return The average of the input points.
     */
    public static Point averagePoints(List<? extends GeomPoint> pnts) {
        StackContext.enter();
        try {
            
            Point avg = pnts.get(0).immutable();
            int numPnts = pnts.size();
            for (int i=1; i < numPnts; ++i) {
                avg = avg.plus(pnts.get(i));
            }
            avg = avg.divide(numPnts);
            return StackContext.outerCopy(avg);
            
        } finally {
            StackContext.exit();
        }
    }
    
    /**
     * Returns the total transformation represented by an entire chain of GeomTransform
     * objects below the one provided.
     *
     * @param tElem The transformed geometry element to have the total TM returned for.
     *              May not be null.
     * @return The transformation matrix (TM) that represents the entire chain of
     *         transformations contained in a nested set of transformed geometry elements.
     */
    public static GTransform getTotalTransform(GeomTransform tElem) {
        StackContext.enter();
        try {

            GTransform T = tElem.getTransform();
            while (true) {
                Transformable elem = tElem.getChild();
                if (!(elem instanceof GeomTransform))
                    break;
                tElem = (GeomTransform)elem;
                T = T.times(tElem.getTransform());
            }
            return StackContext.outerCopy(T);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Returns the input angle (in radians) bounded to the range 0 - 2*PI.
     *
     * @param angle The angle (in radians) to be bounded.
     * @return The input angle bounded to the range 0 to 2*PI (radians).
     */
    public static double bound2Pi(double angle) {
        while (angle > TWO_PI)
            angle -= TWO_PI;
        while (angle < 0)
            angle += TWO_PI;
        return angle;
    }

    /**
     * Returns the input angle bounded to the range 0 - 2*PI.
     *
     * @param angle The angle to be bounded. May not be null.
     * @return The input angle bounded to the range 0 to 2*PI.
     */
    public static Parameter<Angle> bound2Pi(Parameter<Angle> angle) {
        StackContext.enter();
        try {

            while (angle.isGreaterThan(Parameter.TWOPI_ANGLE)) {
                angle = angle.minus(Parameter.TWOPI_ANGLE);
            }
            while (angle.isLessThan(Parameter.ZERO_ANGLE)) {
                angle = angle.plus(Parameter.TWOPI_ANGLE);
            }
            return StackContext.outerCopy(angle);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Returns the input angle (in radians) bounded to the range +/-PI.
     *
     * @param angle The angle (in radians) to be bounded.
     * @return The input angle bounded to the range -PI to +PI (radians).
     */
    public static double boundPi(double angle) {
        while (angle > PI)
            angle -= TWO_PI;
        while (angle < -PI)
            angle += TWO_PI;
        return angle;
    }

    /**
     * Returns the input angle bounded to the range +/-PI (+/- 180 deg).
     *
     * @param angle The angle to be bounded. May not be null.
     * @return The input angle bounded to the range -PI to +PI.
     */
    public static Parameter<Angle> boundPi(Parameter<Angle> angle) {
        StackContext.enter();
        try {

            while (angle.isGreaterThan(Parameter.PI_ANGLE)) {
                angle = angle.minus(Parameter.TWOPI_ANGLE);
            }
            Parameter NEG_PI_ANGLE = Parameter.PI_ANGLE.opposite();
            while (angle.isLessThan(NEG_PI_ANGLE)) {
                angle = angle.plus(Parameter.TWOPI_ANGLE);
            }
            return StackContext.outerCopy(angle);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Return the maximum physical dimension among a collection of geometry elements.
     *
     * @param elements The array of geometry elements to query. May not be null.
     * @return The maximum physical dimension among the input list of geometry elements.
     */
    public static int maxPhyDimension(GeomElement... elements) {
        int numDims = 0;
        int size = elements.length;
        for (int i = size - 1; i >= 0; --i)
            numDims = Math.max(numDims, elements[i].getPhyDimension());
        return numDims;
    }

    /**
     * Return the maximum physical dimension among a collection of geometry elements.
     *
     * @param elements The collection of geometry elements to query. May not be null.
     * @return The maximum physical dimension among the input list of geometry elements.
     */
    public static int maxPhyDimension(Collection<GeomElement> elements) {
        int numDims = 0;
        for (GeomElement item : elements)
            numDims = Math.max(numDims, item.getPhyDimension());
        return numDims;
    }

    /**
     * Return a unit vector, of <code>nDims</code> dimensions, in the X (dimension = 0)
     * direction.
     *
     * @param numDims The number of dimensions for the unit vector.
     * @return A unit vector, with the given dimensions, in the "X" direction.
     */
    public static Vector<Dimensionless> makeXVector(int numDims) {
        FastTable<Parameter<Dimensionless>> tmpList = FastTable.newInstance();

        tmpList.add(Parameter.ONE);
        for (int i = 1; i < numDims; ++i)
            tmpList.add(Parameter.ZERO);
        Vector<Dimensionless> V = Vector.valueOf(tmpList);

        FastTable.recycle(tmpList);
        return V;
    }

    /**
     * Return a unit vector, of <code>nDims</code> dimensions, in the Y (dimension = 1)
     * direction. If the number of dimensions is less than two, this will return null.
     *
     * @param numDims The number of dimensions for the unit vector.
     * @return A unit vector, with the given dimensions, in the "Y" direction.
     */
    public static Vector<Dimensionless> makeYVector(int numDims) {
        if (numDims < 2)
            return null;

        FastTable<Parameter<Dimensionless>> tmpList = FastTable.newInstance();

        for (int i = 0; i < numDims; ++i)
            tmpList.add(Parameter.ZERO);
        tmpList.set(1, Parameter.ONE);
        Vector<Dimensionless> V = Vector.valueOf(tmpList);

        FastTable.recycle(tmpList);
        return V;
    }

    /**
     * Return a unit vector, of <code>nDims</code> dimensions, in the Z (dimension = 2)
     * direction. If the number of dimensions is less than three, then this will return
     * null.
     *
     * @param numDims The number of dimensions for the unit vector.
     * @return A unit vector, with the given dimensions, in the "Z" direction.
     */
    public static Vector<Dimensionless> makeZVector(int numDims) {
        if (numDims < 3)
            return null;

        FastTable<Parameter<Dimensionless>> tmpList = FastTable.newInstance();

        for (int i = 0; i < numDims; ++i)
            tmpList.add(Parameter.ZERO);
        tmpList.set(2, Parameter.ONE);
        Vector<Dimensionless> V = Vector.valueOf(tmpList);

        FastTable.recycle(tmpList);
        return V;
    }

    /**
     * Return the yhat vector (a Y axis orthogonal to the specified plane normal). The
     * returned vector will have the origin set to zero.
     *
     * @param n A unit plane normal vector for the plane containing the new axis. Pass
     *          <code>null</code> for 2D geometry.
     * @return A unit vector orthogonal to the specified plane normal vector.
     */
    public static Vector<Dimensionless> calcYHat(GeomVector<Dimensionless> n) {
        StackContext.enter();
        try {
            Vector<Dimensionless> yhat;

            if (isNull(n)) {
                //  A 2D curve must be in the 2D plane.
                yhat = Vector.valueOf(0, 1);

            } else {
                int numDims = n.getPhyDimension();

                //  Create a yhat axis that is orthogonal to the normal vector.
                if (MathTools.isApproxEqual(n.getValue(0), 1))              //  n.getValue(0) == 1.0
                    //  Normal vector is along the X axis (parallel to X).
                    yhat = GeomUtil.makeYVector(numDims);

                else if (MathTools.isApproxEqual(n.getValue(0), -1))        //  n.getValue(0) == -1.0
                    yhat = GeomUtil.makeYVector(numDims).opposite();

                else {
                    Vector<Dimensionless> xV = GeomUtil.makeXVector(numDims);   //  Create a vector along the X axis.
                    if (n.getValue(2) < 0)
                        yhat = xV.cross(n).toUnitVector();
                    else {
                        yhat = n.cross(xV).toUnitVector();
                        yhat.setOrigin(Point.newInstance(numDims));
                    }
                }
            }

            return StackContext.outerCopy(yhat);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Return the xhat vector (an X axis orthogonal to the specified normal and Y-axis
     * vectors).
     *
     * @param n    A unit plane normal vector for the plane containing the new axis. Pass
     *             null for 2D geometry.
     * @param yhat The unit direction of the Y axis in the plane containing the new axis.
     * @return A unit vector orthogonal to the specified normal and Y-axis vectors.
     */
    public static Vector<Dimensionless> calcXHat(GeomVector<Dimensionless> n,
            GeomVector<Dimensionless> yhat) {
        Vector<Dimensionless> xhat;

        if (isNull(n))
            //  A 2D axis must be in the 2D plane.
            xhat = Vector.valueOf(1, 0);
        else
            //  Create an xhat axis that is orthogonal to yhat and the normal vector.
            xhat = (Vector<Dimensionless>)yhat.cross(n);

        return xhat;
    }

    /**
     * Return a vector that is the in-plane normal to the input 2D vector.
     * 
     * @param u The 2D vector that the normal vector is to be determined for.
     * @return The 2D vector normal of the input 2D vector.
     */
    public static Vector normal2D(GeomVector u) {
        if (u.getPhyDimension() != 2)
            throw new IllegalArgumentException("Input vector must be 2D.");
        
        Vector output = Vector.valueOf(u.get(Point.Y).opposite(), u.get(Point.X));
        
        return output;
    }
    
    /**
     * Return the vector or cross product area value for two vectors. The magnitude of the
     * vector or cross product results in a scalar that represents the the area of a
     * parallelogram with the vectors for sides. This is also the magnitude of the
     * determinant of the two vectors. For 2D vectors, a signed area will be returned with
     * positive area indicating a normal vector that is pointing "out of the page" and a
     * negative area indicating a normal vector pointing "into the page".
     *
     * @param v1 The 1st vector to be multiplied. May not be null.
     * @param v2 The 2nd vector multiplier. May not be null.
     * @return The vector or cross product between the input vectors (v1 X v2) as a scalar
     *         area.
     */
    public static Parameter crossArea(GeomVector v1, GeomVector v2) {
        //  Convert to highest dimension between input vectors.
        int numDims = Math.max(v1.getPhyDimension(), v2.getPhyDimension());
        v1 = v1.toDimension(numDims);
        v2 = v2.toDimension(numDims);

        if (numDims < 3) {
            //  Do a 2D cross product: area = (v1.X*v2.Y) - (v1.Y*v2.X);
            Parameter area = v1.get(Vector.X).times(v2.get(Vector.Y)).minus(v1.get(Vector.Y).times(v2.get(Vector.X)));
            return area;
        }

        //  Do a regular cross product and extract the area.
        GeomVector n = v1.cross(v2);
        return n.norm();
    }

    /**
     * Finds the shortest distance from a point to an infinite line.
     *
     * @param p The point being compared to the line to find the shortest distance. May
     *          not be null.
     * @param a A point on the line. May not be null.
     * @param u The unit direction vector for the line. May not be null.
     * @return The distance from the point to the line.
     */
    public static Parameter<Length> pointLineDistance(GeomPoint p, GeomPoint a, GeomVector<Dimensionless> u) {
        //  Reference:
        //  http://softsurfer.com/Archive/algorithm_0102/algorithm_0102.htm#Distance%20to%202-Point%20Line

        int numDims = maxPhyDimension(p, a, u);
        if (numDims < 3)
            numDims = 3;

        StackContext.enter();
        try {
            //  Convert all the points to the highest dimension between them.
            p = p.toDimension(numDims);
            a = a.toDimension(numDims);
            u = u.toDimension(numDims);

            //  Form a vector from the point on the line to the target point.
            Vector<Length> w = p.minus(a).toGeomVector();

            //  Cross-product of unit vector with point-line vector gives vector with magnitude that is distance.
            GeomVector<Length> uxw = (GeomVector<Length>)u.cross(w);
            Parameter<Length> mag = uxw.mag();

            return StackContext.outerCopy(mag);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Finds the shortest distance from a point to an finite line segment.
     *
     * @param p  The point being compared to the segment to find the shortest distance.
     *           May not be null.
     * @param p0 The start of the line segment. May not be null.
     * @param p1 The end of the line segment. May not be null.
     * @return The shortest distance from the point to the line segment.
     */
    public static Parameter<Length> pointLineSegDistance(GeomPoint p, GeomPoint p0, GeomPoint p1) {
        //  Reference:
        //  http://www.softsurfer.com/Archive/algorithm_0102/algorithm_0102.htm#Distance%20to%20Ray%20or%20Segment

        int numDims = maxPhyDimension(p, p0, p1);

        StackContext.enter();
        try {
            //  Convert all the points to the highest dimension between them.
            p = p.toDimension(numDims);
            p0 = p0.toDimension(numDims);
            p1 = p1.toDimension(numDims);

            //  Form a vector along the line.
            Vector<Length> v = p1.minus(p0).toGeomVector();

            //  Form a vector from the start of the line to the target point.
            Vector<Length> w = p.minus(p0).toGeomVector();

            //  Projection of w onto v.
            Parameter c1 = w.dot(v);

            if (c1.getValue() <= 0)
                //  Closest to start.
                return StackContext.outerCopy(p0.distance(p));

            //  Magnitude of v squared.
            Parameter c2 = v.dot(v);
            if (c2.compareTo(c1) <= 0)
                //  Closest to the end.
                return StackContext.outerCopy(p1.distance(p));

            Parameter<Dimensionless> b = c1.divide(c2);
            Point pb = p0.plus(Point.valueOf(v.times(b)));

            return StackContext.outerCopy(p.distance(pb));

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Finds the point on an infinite line that is closest to the specified point.
     *
     * @param p The point being compared to the line to find the closest point on the
     *          line. May not be null.
     * @param a A point on the line. May not be null.
     * @param u The unit direction vector for the line. May not be null.
     * @return The point on the line closest to the point "p".
     */
    public static Point pointLineClosest(GeomPoint p, GeomPoint a, GeomVector<Dimensionless> u) {

        int numDims = maxPhyDimension(p, a, u);
        if (numDims < 2)
            numDims = 2;

        StackContext.enter();
        try {
            //  Convert all the points to the highest dimension between them.
            p = p.toDimension(numDims);
            a = a.toDimension(numDims);
            u = u.toDimension(numDims);

            //  Form a vector from the point on the line to the target point.
            Vector<Length> w = p.minus(a).toGeomVector();

            //  Calculate vector from the end of the line to the closest point.
            //  Project w onto u.
            GeomVector<Length> cpV = (GeomVector<Length>)u.times(u.dot(w));

            //  Calculate the end point of the cpV vector.
            Point pOut = Point.valueOf(cpV).plus(a);

            return StackContext.outerCopy(pOut);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Finds the shortest signed distance from a point to an infinite plane.
     *
     * @param p The point being compared to the plane to find the closest point on the
     *          plane. May not be null.
     * @param a Any point in the plane. May not be null.
     * @param n The unit normal vector for the plane. May not be null.
     * @return The signed shortest distance from the input point "p" to the plane. The
     *         distance is positive for points on the side of the plane pointed to by the
     *         normal vector "n", and negative for points on the other side of the plane.
     * @throws DimensionException if input normal vector physical dimension is not at
     * least 3 dimensional.
     */
    public static Parameter<Length> pointPlaneDistance(GeomPoint p, GeomPoint a, GeomVector<Dimensionless> n) throws DimensionException {
        requireNonNull(p);
        requireNonNull(a);
        
        //  Convert all the points to the highest dimension between them.
        int numDims = n.getPhyDimension();
        if (numDims < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input normal vector", numDims));

        numDims = Math.max(numDims, Math.max(a.getPhyDimension(), p.getPhyDimension()));

        StackContext.enter();
        try {
            p = p.toDimension(numDims);
            a = a.toDimension(numDims);
            n = n.toDimension(numDims);

            //  Vector from point in plane to input point.
            Vector<Length> v = p.minus(a).toGeomVector();

            //  The signed distance is projection of v onto the normal vector.
            Parameter<Length> dist = (Parameter)v.dot(n);

            return StackContext.outerCopy(dist);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Finds the point on an infinite plane that is closest to the specified point. The
     * distance to the plane is simply:
     * <code>p.minus(pointPlaneClosest(p,...)).norm();</code>.
     *
     * @param p The point being compared to the plane to find the closest point on the
     *          plane. May not be null.
     * @param a Any point in the plane. May not be null.
     * @param n The unit normal vector for the plane. May not be null.
     * @return The point on the plane closest to the point "p". May return the input point
     *         "p" if it is in the plane.
     * @throws DimensionException if input normal vector physical dimension is not at
     * least 3 dimensional.
     */
    public static Point pointPlaneClosest(GeomPoint p, GeomPoint a, GeomVector<Dimensionless> n) throws DimensionException {
        requireNonNull(p);
        requireNonNull(a);
        
        //  Convert all the points to the highest dimension between them.
        int numDims = n.getPhyDimension();
        if (numDims < 3)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast3"),
                            "input normal vector", numDims));

        numDims = Math.max(numDims, Math.max(a.getPhyDimension(), p.getPhyDimension()));

        StackContext.enter();
        try {
            p = p.toDimension(numDims);
            a = a.toDimension(numDims);
            n = n.toDimension(numDims);

            //  Find the signed distance to the plane from the point.
            Parameter dist = pointPlaneDistance(p, a, n);

            //  If distance is zero, then the input point is already in the plane.
            if (abs(dist.getValue()) <= MathTools.SQRT_EPS)
                return StackContext.outerCopy(p.immutable());

            //  dv is parallel to normal vector and "dist" long.
            GeomVector dv = n.times(dist);

            //  Subtract dv from the input point to get point projected onto the plane.
            Point projPoint = p.minus(Point.valueOf(dv));

            return StackContext.outerCopy(projPoint);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Find a circle that passes through the supplied (not co-linear) points.
     *
     * @param p1 The 1st of the points that define the circle (must not be colinear with
     *           the other two). May not be null.
     * @param p2 The 2nd of the points that define the circle (must not be colinear with
     *           the other two). May not be null.
     * @param p3 The 3rd of the points that define the circle (must not be colinear with
     *           the other two). May not be null.
     * @return A CircleInfo record containing the geometry of the circle. The "xhat"
     *         parameter points to the first point supplied. The "yhat" parameter
     *         indicates the direction to the next point. The "angle" parameter gives the
     *         angle swept out from the 1st point to the 3rd point.
     * @throws DimensionException if one of the 3 points does not have at least 2 physical
     * dimensions.
     * @throws IllegalArgumentException if the input points are co-linear.
     */
    public static CircleInfo threePointCircle(GeomPoint p1, GeomPoint p2, GeomPoint p3) throws DimensionException {
        //  Reference: http://en.wikipedia.org/wiki/Circumscribed_circle#Barycentric_coordinates_as_a_function_of_the_side_lengths
        //      Also: http://stackoverflow.com/a/13992781

        //  Convert all the points to the highest dimension between them.
        int numDims = maxPhyDimension(p1, p2, p3);
        if (numDims < 2)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast2"), "input points", numDims));

        //  Up-cast the geometry to 3D if necessary to make some of the math easier.
        int saveDims = numDims;
        if (numDims < 3)
            numDims = 3;

        Parameter<Length> radius;
        Point Pc;
        Vector<Dimensionless> xhat, yhat;
        Parameter<Angle> angle;
        StackContext.enter();
        try {
            p1 = p1.toDimension(numDims);
            p2 = p2.toDimension(numDims);
            p3 = p3.toDimension(numDims);
            Unit<Length> unit = p1.getUnit();

            //  Calculate edges of the triangle being circumscribed.
            Vector<Length> t = p2.minus(p1).toGeomVector();
            Vector<Length> u = p3.minus(p1).toGeomVector();
            Vector<Length> v = p3.minus(p2).toGeomVector();

            //  Are the points colinear?
            double tDOTv = t.dot(v).getValue() / (t.normValue() * v.normValue());
            if (abs(tDOTv) > 1.0 - 1.e-6)
                throw new IllegalArgumentException(RESOURCES.getString("pointsColinearErr"));

            //  Compute the triangle normal.
            Vector w = t.cross(u);
            Parameter wmag = w.mag();
            Vector<Dimensionless> nhat = w.toUnitVector();

            //  Compute some dot products we will need.
            Parameter tt = t.dot(t);
            Parameter uu = u.dot(u);

            //  Compute the circle radius:  r = sqrt(tt*uu*(v*v))/(2*wmag)
            radius = tt.times(uu).times(v.dot(v)).sqrt().divide(2).divide(wmag).to(unit);

            //  Compute the circle center:  Pc = p1 + (u*tt*(u*v) - t*uu*(t*v))/(2*wmag^2)
            Parameter wmag2 = wmag.pow(2);
            Parameter uv = u.dot(v);
            Parameter tv = t.dot(v);
            Vector<Length> Pcv = u.times(tt).times(uv).minus((Vector)t.times(uu).times(tv)).divide(2).divide(wmag2).to(unit);
            Pc = p1.plus(Point.valueOf(Pcv));

            //  The xhat vector is the vector from the center to the 1st point.
            xhat = p1.minus(Pc).toGeomVector().toUnitVector();
            xhat.setOrigin(Point.newInstance(numDims));

            //  The yhat vector is perpendicular to the xhat and normal vectors.
            yhat = nhat.cross(xhat).toUnitVector();
            yhat.setOrigin(Point.newInstance(numDims));

            //  Calculate the angle between the 1st and last points.
            Vector<Dimensionless> p2V = p2.minus(Pc).toGeomVector().toUnitVector();
            Parameter dot = xhat.dot(p2V);
            angle = Parameter.acos(dot);
            Vector<Dimensionless> p3V = p3.minus(Pc).toGeomVector().toUnitVector();
            dot = p2V.dot(p3V);
            angle = angle.plus(Parameter.acos(dot));

            if (saveDims == 2) {
                //  Convert back to 2D geometry on output.
                Pc = Pc.toDimension(2);
                xhat = xhat.toDimension(2);
                yhat = yhat.toDimension(2);
            }

            //  Copy the things we need out of the stack context.
            radius = StackContext.outerCopy(radius);
            Pc = StackContext.outerCopy(Pc);
            xhat = StackContext.outerCopy(xhat);
            yhat = StackContext.outerCopy(yhat);
            angle = StackContext.outerCopy(angle);

        } finally {
            StackContext.exit();
        }

        //  Create and return a circle record.
        CircleInfo circle = new CircleInfo();
        circle.radius = radius;
        circle.center = Pc;
        circle.xhat = xhat;
        circle.yhat = yhat;
        circle.angle = angle;

        return circle;
    }

    /**
     * Finds a circle that is approximately centered on the specified origin point and
     * passes through the two input points. The origin point is used to determine the
     * plane that the circle lies in and is used to approximate the center of the circle.
     * The true origin/center of the circle is calculated to ensure that the circle passes
     * through the supplied edge points.
     *
     * @param o  Approximate origin or center to create the full-circle around. May not be
     *           null.
     * @param p1 The 1st of the points that define the circle (must not be colinear with
     *           the other two). May not be null.
     * @param p2 The 2nd of the points that define the circle (must not be colinear with
     *           the other two). May not be null.
     * @return A CircleInfo record containing the geometry of the circle. THe "xhat"
     *         parameter points to the first point supplied. The "yhat" parameter
     *         indicates the direction to the 2nd point. The "angle" parameter gives the
     *         angle swept out from the 1st point to the 2nd point.
     * @throws DimensionException if the origin or 2 points do not have at least 2
     * physical dimensions.
     */
    public static CircleInfo centerTwoPointCircle(GeomPoint o, GeomPoint p1, GeomPoint p2) throws DimensionException {

        //  Convert all the points to the highest dimension between them.
        int numDims = maxPhyDimension(o, p1, p2);
        if (numDims < 2)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast2"), "input points", numDims));

        //  Up-cast the geometry to 3D if necessary to make some of the math easier.
        int saveDims = numDims;
        if (numDims < 3)
            numDims = 3;

        o = o.toDimension(numDims);
        p1 = p1.toDimension(numDims);
        p2 = p2.toDimension(numDims);

        //  Find the true center of the circle.
        Vector<Length> a = p2.minus(p1).toGeomVector();

        //  Find the bisection point of the line segment "a".
        Point aB = p1.plus(p2).divide(2);    //  aB = (p1 + p2)/2

        //  Find normal vector and direction perpendicular to vector "a".
        //  Get vectors from the guess at the circle center to the end points.
        GeomVector<Length> p1v = p1.minus(o).toGeomVector();
        GeomVector<Length> p2v = p2.minus(o).toGeomVector();

        //  Determine the normal vector for the plane containing the circle.
        Vector<Dimensionless> n = p1v.cross(p2v).toUnitVector();

        //  Determine the perpendicular vector.
        Vector<Dimensionless> taB = a.cross(n).toUnitVector();

        //  The center is the nearest point to the user supplied center on the line that
        //  bisects the line between p1 and p2.
        Point c = pointLineClosest(o, aB, taB);

        //  Construct a circle information record from the center, two end points, and normal vector.
        CircleInfo circInfo = makeCircleInfo(c, p1, p2, n);
        if (saveDims == 2) {
            //  Convert back to 2D geometry on output.
            circInfo.center = circInfo.center.toDimension(2);
            circInfo.xhat = circInfo.xhat.toDimension(2);
            circInfo.yhat = circInfo.yhat.toDimension(2);
        }

        return circInfo;
    }

    /**
     * Finds a circle that passes through the two specified points with the specified
     * tangent vector at the start.
     *
     * @param p1 The 1st of the points that define the circle. May not be null.
     * @param t1 The tangent vector at p1 (must not point directly at p2). May not be null.
     * @param p2 The 2nd of the points that define the circle. May not be null.
     * @return A CircleInfo record containing the geometry of the circle. THe "xhat"
     *         parameter points to the first point supplied. The "yhat" parameter
     *         indicates the direction to the 2nd point. The "angle" parameter gives the
     *         angle swept out from the 1st point to the 2nd point.
     * @throws DimensionException if the 2 points do not have at least 2 physical
     * dimensions.
     */
    public static CircleInfo twoPointTangentCircle(GeomPoint p1, GeomVector t1, GeomPoint p2) throws DimensionException {

        //  Convert all the points to the highest dimension between them.
        int numDims = maxPhyDimension(p1, t1, p2);
        if (numDims < 2)
            throw new DimensionException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast2"), "input points", numDims));

        //  Up-cast the geometry to 3D if necessary to make some of the math easier.
        int saveDims = numDims;
        if (numDims < 3)
            numDims = 3;
        p1 = p1.toDimension(numDims);
        p2 = p2.toDimension(numDims);
        t1 = t1.toDimension(numDims);

        //  "a" is the vector between the two end points.
        Vector<Length> a = p2.minus(p1).toGeomVector();

        //  Make sure tangent vector is valid.  If parallel to p2-p1 then dot product will equal 1.
        if (t1.dot(a).divide(t1.mag()).divide(a.mag()).minus(Parameter.ONE).abs().isApproxZero())
            throw new IllegalArgumentException(RESOURCES.getString("t1ParallelTop2p1Err"));

        //  Find the bisection point of the line segment "a".
        Point aB = p1.plus(p2).divide(2);   //  aB = (p1 + p2)/2

        //  Find normal vector and direction perpendicular to vector "a".
        //  Determine the normal vector for the plane containing the circle.
        Vector<Dimensionless> n = t1.cross(a).toUnitVector();

        //  Determine the perpendicular vector.
        Vector<Dimensionless> taB = a.cross(n).toUnitVector();

        //  Form vector perpendicular to t1 and the normal vector
        //  (points toward the center of the circle).
        Vector<Dimensionless> t1hat = n.cross(t1).toUnitVector();

        //  The center/origin is where the p1/t1hat and perpendicular bisector
        //  lines intersect (or come closest to intersecting).
        MutablePoint c = MutablePoint.newInstance(numDims, p1.getUnit());
        lineLineIntersect(p1, t1hat, aB, taB, GTOL, null, null, c);

        //  Construct a circle information record from the center, two end points, and normal vector.
        CircleInfo circInfo = makeCircleInfo(c, p1, p2, n);
        if (saveDims == 2) {
            //  Convert back to 2D geometry on output.
            circInfo.center = circInfo.center.toDimension(2);
            circInfo.xhat = circInfo.xhat.toDimension(2);
            circInfo.yhat = circInfo.yhat.toDimension(2);
        }

        return circInfo;
    }

    /**
     * Return a CircleInfo record from the circle center, end points and normal vector.
     * All input geometry must be at least 3D. No error checking is performed. All points
     * & vectors must be the same dimension and compatible with a circle.
     *
     * @param c  The center of the circle.
     * @param p1 The 1st end point of the circle or circular arc.
     * @param p2 The 2nd end point of the circle or circular arc.
     * @param n  The normal vector for the circle.
     * @return A CircleInfo record containing the geometry of the circle. The "xhat"
     *         parameter points to the first point supplied. The "yhat" parameter
     *         indicates the direction to the next point. The "angle" parameter gives the
     *         angle swept out from the 1st point to the 2nd point.
     */
    private static CircleInfo makeCircleInfo(GeomPoint c, GeomPoint p1, GeomPoint p2, GeomVector n) {
        int numDims = c.getPhyDimension();

        //  The radius is the distance from the center point to any one of the edge points.
        Parameter<Length> r = c.distance(p1);

        //  The xhat vector is the vector from the center to the 1st point.
        Vector<Dimensionless> xhat = p1.minus(c).toGeomVector().toUnitVector();
        xhat.setOrigin(Point.newInstance(numDims));

        //  The yhat vector is perpendicular to the xhat and normal vectors.
        Vector<Dimensionless> yhat = n.cross(xhat).toUnitVector();
        yhat.setOrigin(Point.newInstance(numDims));

        //  Calculate the angle between the 1st and last points.
        Vector<Dimensionless> p2V = p2.minus(c).toGeomVector().toUnitVector();
        Parameter dot = xhat.dot(p2V);
        Parameter<Angle> angle = Parameter.acos(dot);

        //  Create and return a circle record.
        CircleInfo circle = new CircleInfo();
        circle.radius = r.to(p1.getUnit());
        circle.center = c.to(p1.getUnit());
        circle.xhat = xhat;
        circle.yhat = yhat;
        circle.angle = angle;

        return circle;
    }

    /**
     * Returns the default units for this system. The default unit is based on the user's
     * locale. If the user's locale is the United States, then a value of NonSI.INCH is
     * returned, otherwise a value of SI.CENTIMETER is returned.
     *
     * @return The default units for this system.
     */
    public static Unit<Length> getDefaultUnit() {
        Unit<Length> unit = SI.CENTIMETER;
        if (java.util.Locale.getDefault().equals(java.util.Locale.US))
            unit = NonSI.INCH;
        return unit;
    }

    /**
     * Finds the intersection point between two lines, or the point of closest approach if
     * the lines do not intersect, and returns the point on each line. The output data is
     * returned by modifying the input <code>sOut</code>, <code>out1</code> and 
     * <code>out2</code> points.
     *
     * @param p1   A point on the 1st line. May not be null.
     * @param t1   The direction vector for the 1st line (does not have to be a unit
     *             vector). May not be null.
     * @param p2   A point on the 2nd line. May not be null.
     * @param t2   The direction vector for the 2nd line (MUST be similar to t1; unit
     *             vector if t1 is unit vector and dimension vector if t1 is a dimensional
     *             vector). Do NOT mix dimensional and unit vectors in the same call. May
     *             not be null.
     * @param tol  Tolerance on how close the lines must be to be considered intersecting.
     *             A value of null indicates that an exact intersection is required.
     * @param sOut The pre-defined 2D output parametric positions on the input lines that
     *             will be filled in to represent the points where the lines intersect. If
     *             the input direction vectors are dimensionless (unit vectors), then the
     *             output point will be scaled to the units of p1 (indicating the distance
     *             that the intersection point is away from the origin in p1 units). If
     *             the input direction vectors are dimensional, then the point will be
     *             scaled such that 1.0 m represents the direction vector's length along
     *             the line. If there is no intersection, the point is not modified. If
     *             <code>null</code> is passed, then the parametric point is not
     *             calculated.
     * @param out1 The pre-defined output point on the 1st line representing the
     *             intersection or closest approach of the two lines (may be null if not
     *             required).
     * @param out2 The pre-defined output point on the 2nd line representing the
     *             intersection or closest approach of the two lines (may be null if not
     *             required).
     * @return DISJOINT if the lines do not intersect or INTERSECT if they do.
     * @throws DimensionException if the output points do not have at least as many
     * dimensions as the input points.
     */
    public static IntersectType lineLineIntersect(GeomPoint p1, GeomVector t1,
            GeomPoint p2, GeomVector t2, Parameter<Length> tol,
            MutablePoint sOut, MutablePoint out1, MutablePoint out2) throws DimensionException {
        //  For algorithm see:  http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
        
        int numDims = maxPhyDimension(p1, t1, p2, t2);
        if (numDims < 2)
            numDims = 2;

        boolean parallel = false;
        Parameter mu1, mu2;
        Vector<Length> v1, v2;
        StackContext.enter();
        try {
            //  Convert all the points to the highest dimension between them.
            p1 = p1.toDimension(numDims);
            t1 = t1.toDimension(numDims);
            p2 = p2.toDimension(numDims);
            t2 = t2.toDimension(numDims);

            Vector<Length> v12 = p1.minus(p2).toGeomVector();

            Parameter a = t1.dot(t1);
            Parameter b = t1.dot(t2);
            Parameter c = t2.dot(t2);
            Parameter d = t1.dot(v12);
            Parameter e = t2.dot(v12);
            Parameter denom = a.times(c).minus(b.times(b));     //  denom = a*c - b^2

            if (denom.isApproxZero()) {
                //  The lines are parallel.
                parallel = true;
                mu1 = Parameter.valueOf(0, d.getUnit().divide(b.getUnit()));    //  mu1 = 0;
                mu2 = b.isGreaterThan(c) ? d.divide(b) : e.divide(c);           //  b > c ? d / b : e / c;

            } else {
                mu1 = b.times(e).minus(c.times(d)).divide(denom);               //  mu1 = (b * e - c * d) / denom;
                mu2 = a.times(e).minus(b.times(d)).divide(denom);               //  mu2 = (a * e - b * d) / denom;
            }

            //  Compute the closest points on each line.
            v1 = t1.times(mu1);
            v1 = v1.plus(p1.toGeomVector());
            v2 = t2.times(mu2);
            v2 = v2.plus(p2.toGeomVector());

            //  Copy the things we need to the outer context.
            if (nonNull(sOut)) {
                mu1 = StackContext.outerCopy(mu1);
                mu2 = StackContext.outerCopy(mu2);
            }
            v1 = StackContext.outerCopy(v1);
            v2 = StackContext.outerCopy(v2);

        } finally {
            StackContext.exit();
        }

        if (nonNull(sOut)) {
            if (mu1.getUnit().isCompatible(Dimensionless.UNIT)) {
                UnitConverter cvtr = SI.METER.getConverterTo(sOut.getUnit());
                sOut.setValue(0, cvtr.convert(mu1.getValue(Dimensionless.UNIT)));
                sOut.setValue(1, cvtr.convert(mu2.getValue(Dimensionless.UNIT)));
            } else {
                sOut.set(0, mu1);
                sOut.set(1, mu2);
            }
        }

        if (nonNull(out1)) {
            out1.set(v1);
        }

        if (nonNull(out2)) {
            out2.set(v2);
        }

        if (v2.minus(v1).norm().isLessThan(tol)) {
            if (parallel)
                return IntersectType.COINCIDENT;
            return IntersectType.INTERSECT;
        }
        return IntersectType.DISJOINT;
    }

    /**
     * Finds the intersection point between a line and a plane (if there is an
     * intersection). If the line and the plane are parallel, then IntersectType.DISJOINT
     * is output, if there is an intersection, then 1 is output and the intersection point
     * is returned in the input <code>out</code> point. Finally, if the line is coincident
     * with the plane, then 2 is output and out is set to the L0 point.
     *
     * @param L0    A point on the line (origin of the line). May not be null.
     * @param Ldir  The direction vector for the line (does not have to be a unit vector).
     *              May not be null.
     * @param plane The plane being tested for intersection. May not be null.
     * @param out   The pre-defined output point on the line that will be filled in to
     *              represent the intersection of the line and the plane (not modified if
     *              there is no intersection). If <code>null</code> is passed, then the
     *              point is not calculated, but the return codes of this method are still
     *              valid.
     * @return DISJOINT if the line and plane are disjoint (parallel), INTERSECT if there
     *         is a unique intersection and COINCIDENT if the line is coincident with the
     *         plane.
     * @throws DimensionException if the output point does not have at least as many
     * dimensions as the input line and plane.
     */
    public static IntersectType linePlaneIntersect(GeomPoint L0, GeomVector Ldir,
            GeomPlane plane, MutablePoint out) throws DimensionException {
        //  For algorithm see:  http://www.softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm
        
        //  Convert all the input data to the highest dimension between them.
        int numDims = maxPhyDimension(L0, Ldir, plane);
        L0 = L0.toDimension(numDims);
        Vector<Dimensionless> Lu = Ldir.toDimension(numDims).toUnitVector();
        plane = plane.toDimension(numDims);

        //  Get data we need.
        GeomPoint V0 = plane.getRefPoint();
        Vector nw = V0.minus(L0).toGeomVector();
        GeomVector<Dimensionless> n = plane.getNormal();

        //  Check for the line parallel to the plane.
        Parameter ndotu = n.dot(Lu);
        if (ndotu.isApproxZero()) {
            Parameter N = n.dot(nw);
            if (N.isApproxZero()) {
                if (out != null)
                    out.set(L0);
                return IntersectType.COINCIDENT;
            }
            return IntersectType.DISJOINT;
        }

        if (nonNull(out)) {
            //  Find the intersection point.
            Parameter N = n.dot(nw);
            Parameter si = N.divide(ndotu);
            Vector v = Lu.times(si);
            v = v.plus(L0.toGeomVector());

            out.set(v);
        }

        return IntersectType.INTERSECT;                     //  A unique intersection.
    }

    /**
     * Finds the intersection point between a line and a plane (if there is an
     * intersection). If the line and the plane are parallel, then IntersectType.DISJOINT
     * is output, if there is an intersection, then 1 is output and the intersection point
     * is returned in the input <code>out</code> point. Finally, if the line is coincident
     * with the plane, then 2 is output and out is set to the L0 point.
     *
     * @param L0    A point on the line (origin of the line). May not be null.
     * @param Ldir  The direction vector for the line (does not have to be a unit vector).
     *              May not be null.
     * @param plane The plane being tested for intersection. May not be null.
     * @param out   The pre-defined output point on the line that will be filled in to
     *              represent the intersection of the line and the plane (not modified if
     *              there is no intersection). If <code>null</code> is passed, then the
     *              point is not calculated, but the return codes of this method are still
     *              valid.
     * @param tol   The fractional tolerance on determining if the line and plane are
     *              coincident or disjoint.
     * @return DISJOINT if the line and plane are disjoint (parallel), INTERSECT if there
     *         is a unique intersection and COINCIDENT if the line is coincident with the
     *         plane.
     * @throws DimensionException if the output point does not have at least as many
     * dimensions as the input line and plane.
     */
    public static IntersectType linePlaneIntersect(GeomPoint L0, GeomVector Ldir,
            GeomPlane plane, MutablePoint out, double tol) throws DimensionException {
        //  For algorithm see:  http://www.softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm
        
        //  Convert all the input data to the highest dimension between them.
        int numDims = maxPhyDimension(L0, Ldir, plane);
        L0 = L0.toDimension(numDims);
        Vector<Dimensionless> Lu = Ldir.toDimension(numDims).toUnitVector();
        plane = plane.toDimension(numDims);

        //  Get data we need.
        GeomPoint V0 = plane.getRefPoint();
        Vector nw = V0.minus(L0).toGeomVector();
        GeomVector<Dimensionless> n = plane.getNormal();

        //  Check for the line parallel to the plane.
        Parameter ptol = Parameter.valueOf(tol, Dimensionless.UNIT);
        Parameter ndotu = n.dot(Lu);
        if (!ndotu.isLargerThan(ptol)) {
            Parameter N = n.dot(nw.toUnitVector());
            if (!N.isLargerThan(ptol)) {
                if (out != null)
                    out.set(L0);
                return IntersectType.COINCIDENT;
            }
            return IntersectType.DISJOINT;
        }

        if (nonNull(out)) {
            //  Find the intersection point.
            Parameter N = n.dot(nw);
            Parameter si = N.divide(ndotu);
            Vector v = Lu.times(si);
            v = v.plus(L0.toGeomVector());

            out.set(v);
        }

        return IntersectType.INTERSECT;                     //  A unique intersection.
    }

    /**
     * Finds the intersection line between two planes (if there is an intersection). If
     * the two planes are parallel, then IntersectType.DISJOINT is output, if there is an
     * intersection, then IntersectType.INTERSECT is output and a point on the
     * intersection line is returned in the input <code>out</code> point and the direction
     * vector of the intersection line is returned in the input <code>dir</code> vector.
     * Finally, if the two planes are coincident, then IntersectType.COINCIDENT is output.
     *
     * @param plane1 The 1st plane being tested for intersection. May not be null.
     * @param plane2 The 2nd plane being tested for intersection. May not be null.
     * @param out    The pre-defined output point on the intersection line that will be
     *               filled in to represent the intersection of the two planes (not
     *               modified if there is no intersection or if the planes are
     *               coincident). If <code>null</code> is passed, then the intersection
     *               line point is not calculated, but the return codes of this method are
     *               still valid.
     * @param dir    The pre-defined output unit direction vector of the intersection line
     *               that will be filled in to represent the intersection of the two
     *               planes (not modified if there is no intersection or if the planes are
     *               coincident). If <code>null</code> is passed, then the intersection
     *               line direction is not calculated, but the return codes of this method
     *               are still valid.
     * @return DISJOINT if the line and plane are disjoint (parallel), INTERSECT if there
     *         is a unique intersection and COINCIDENT if the line is coincident with the
     *         plane.
     * @throws DimensionException if the output point and plane do not have at least as
     * many dimensions as the input planes.
     */
    public static IntersectType planePlaneIntersect(GeomPlane plane1,
            GeomPlane plane2, MutablePoint out, MutableVector<Dimensionless> dir) throws DimensionException {
        //  Reference:  http://mathworld.wolfram.com/Plane-PlaneIntersection.html
        
        //  Convert all the input data to the highest dimension between them.
        int numDims = Math.max(plane1.getPhyDimension(), plane2.getPhyDimension());

        StackContext.enter();
        try {
            //  Convert to the highest dimension input object.
            Unit<Length> unit = plane1.getUnit();
            plane1 = plane1.toDimension(numDims);
            plane2 = plane2.toDimension(numDims).to(unit);

            //  Get data we need.
            GeomPoint P1 = plane1.getRefPoint();
            GeomVector<Dimensionless> n1 = plane1.getNormal();
            GeomPoint P2 = plane2.getRefPoint();
            GeomVector<Dimensionless> n2 = plane2.getNormal();
            Vector nw = P2.minus(P1).toGeomVector();

            //  Check for the two planes being parallel.
            double n1dotn2 = n1.dot(n2).getValue();
            if (MathTools.isApproxEqual(n1dotn2, 1)) {         //  n1dotn2 == 1
                //  Are the two planes also coincident?
                Parameter N = n1.dot(nw);
                if (N.isApproxZero()) {
                    return IntersectType.COINCIDENT;
                }
                return IntersectType.DISJOINT;
            }

            if (nonNull(dir)) {
                //  Find the intersection line direction vector.
                Vector<Dimensionless> a = n1.cross(n2).toUnitVector();
                //  The following works because MutableVector.set() is context safe when provided
                //  a "Vector" instance, but ONLY in that case.
                dir.set(StackContext.outerCopy(a));
            }

            if (nonNull(out)) {
                //  Find the intersection point.

                //  Create elements of the "b" matrix.
                double np1 = plane1.getConstant().getValue();   //  -p1
                double np2 = plane2.getConstant().getValue();   //  -p2
                Float64Vector b = Float64Vector.valueOf(np1, np2);

                //  Create the "M" matrix.
                Float64Matrix M = Float64Matrix.valueOf(n1.toFloat64Vector(), n2.toFloat64Vector());
                M = M.transpose();

                //  Solve for the "x0" vector:  x0 = M^-1 * b.
                Float64Vector x0 = Float64Vector.valueOf(M.pseudoInverse().transpose().times(b));

                //  Store the newly calculated point in x0.
                Point x0p = Point.valueOf(x0, unit);
                //  The following works because MutablePoint.set() is context safe when provided
                //  a "Point" instance, but ONLY in that case.
                out.set(StackContext.outerCopy(x0p));
            }

        } finally {
            StackContext.exit();
        }

        //  We have an intersection if we have gotten here.
        if (nonNull(out) && nonNull(dir)) {
            //  If the line direction is requested, store the intersection point in it.
            dir.setOrigin(out.immutable());
        }

        return IntersectType.INTERSECT;             //  A unique intersection.
    }

    /**
     * Returns true if the bounding boxes of two geometry objects intersect or overlap.
     * This is an aligned axis bounding box (AABB) test.
     *
     * @param b1 The first geometric object to test for overlap. May not be null.
     * @param b2 The 2nd geometric object to test for overlap. May not be null.
     * @return <code>true</code> if the bounding boxes of the two input geometry objects
     *         intersect or overlap (including one box contained completely inside of the
     *         other).
     */
    public static boolean boundsIntersect(GeomElement b1, GeomElement b2) {
        //  Reference:
        //  http://www.gamasutra.com/view/feature/3383/simple_intersection_tests_for_games.php?page=3

        StackContext.enter();
        try {
            //  Make sure both geometry elements have the same units.
            Unit<Length> unit = b1.getUnit();

            //  Get physical dimension of each element.
            int dim1 = b1.getPhyDimension();
            int dim2 = b2.getPhyDimension();

            //  Get the bounding boxes of each geometry element.
            Point b1min = b1.getBoundsMin().to(unit);
            Point b1max = b1.getBoundsMax().to(unit);
            Point b2min = b2.getBoundsMin().to(unit);
            Point b2max = b2.getBoundsMax().to(unit);

            //  Make sure points are all the same dimension
            //  by making both have the dimension of the largest.
            if (dim1 > dim2) {
                b2min = b2min.toDimension(dim1);
                b2max = b2max.toDimension(dim1);

            } else if (dim1 != dim2) {  //  dim2 > dim1
                b1min = b1min.toDimension(dim2);
                b1max = b1max.toDimension(dim2);
            }

            //  Find center of each bounding box.
            Point p1 = b1min.plus(b1max).divide(2);
            Point p2 = b2min.plus(b2max).divide(2);

            //  Find extents of each bounding box.
            Point E1 = p1.minus(b1min);
            Point E2 = p2.minus(b2min);

            //  Find the distance between the centers of each box.
            Point T = p2.minus(p1);

            //  Do the overlap test.
            for (int i = 0; i < dim1; ++i) {            //  Loop over each axis.
                double Tv = abs(T.getValue(i));     //  Distance between centers along this axis.
                double E1v = abs(E1.getValue(i));   //  Bounding box extents along this axis.
                double E2v = abs(E2.getValue(i));
                if (Tv > E1v + E2v) {
                    //  Found a separating axis where
                    //  distance along this axis is > than box sizes along this axis.
                    //  So, the boxes must not overlap.
                    return false;
                }
            }

            return true;

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Returns true if the specified infinite line and bounding box of the specified
     * geometry element intersect. This is an infinite line - aligned axis bounding box
     * (AABB) test.
     *
     * @param L0   A point on the line (origin of the line). May not be null.
     * @param Ldir The direction vector for the line (does not have to be a unit vector).
     *             May not be null.
     * @param box  The geometry element to extract the bounding box from. May not be null.
     * @param sOut The pre-defined 2D output parametric position on the line (relative to
     *             the origin) that will be filled in to represent the points where the
     *             line enters and leaves the bounding box. If there is no intersection,
     *             the point is not modified. If <code>null</code> is passed, then the
     *             parametric point is not calculated.
     * @return <code>true</code> if the line and geometry element bounding box intersects.
     */
    public static boolean lineBoundsIntersect(GeomPoint L0, GeomVector Ldir, GeomElement box, MutablePoint sOut) {
        //  Reference:
        //  http://ggt.sourceforge.net/gmtlReferenceGuide-0.6.1-html/namespacegmtl.html#a5e30c7bc7cdaec04e04276eea798264f
        double tIn = -Double.MAX_VALUE;
        double tOut = Double.MAX_VALUE;

        int numDims = maxPhyDimension(L0, Ldir, box);
        if (numDims < 2)
            numDims = 2;

        StackContext.enter();
        try {
            //  Make sure everything uses the same units.
            Unit<Length> unit = box.getUnit();

            //  Make sure everything has the dimensions of the largest input object.
            L0 = L0.toDimension(numDims);
            Ldir = Ldir.toDimension(numDims);
            if (!Dimensionless.UNIT.equals(Ldir.getUnit()))
                Ldir = (GeomVector)Ldir.toDimension(numDims).to(unit);

            //  Get the bounding box of the geometry element.
            Point bmin = box.getBoundsMin().toDimension(numDims).minus(GTOL);
            Point bmax = box.getBoundsMax().toDimension(numDims).plus(GTOL);

            //  Do any of the principal axes form a separating axis?
            for (int i = 0; i < numDims; ++i) {     //  Loop over each axis.
                double ui = Ldir.getValue(i);       //  Component of the line this axis direction.

                //  Is the line parallel to this axis?
                if (MathTools.isApproxZero(ui)) {
                    double L0i = L0.getValue(i, unit);
                    if (L0i < bmin.getValue(i, unit) || L0i > bmax.getValue(i, unit))
                        return false;
                }
            }

            //  Check to see if there are any intersections with the bounding box.
            for (int i = 0; i < numDims; ++i) {
                double ui = Ldir.getValue(i);
                if (MathTools.isApproxZero(ui))
                    continue;
                double L0i = L0.getValue(i, unit);

                double t0 = (bmin.getValue(i, unit) - L0i) / ui; //  t0 = (bmin[i] - p0[i]) / u[i];
                double t1 = (bmax.getValue(i, unit) - L0i) / ui; //  t1 = (bmax[i] - p0[i]) / u[i];
                if (t0 > t1) {
                    //  Swap
                    double tmp = t0;
                    t0 = t1;
                    t1 = tmp;
                }

                if (t0 > tIn)
                    tIn = t0;
                if (t1 < tOut)
                    tOut = t1;
                if (tIn > tOut || tOut < 0)
                    return false;
            }

        } finally {
            StackContext.exit();
        }

        if (nonNull(sOut)) {
            sOut.setValue(0, tIn);
            sOut.setValue(1, tOut);
        }

        return true;
    }

    /**
     * Returns true if the specified line segment and bounding box of the specified
     * geometry element intersect or overlap. This is a line segment aligned axis bounding
     * box (AABB) test.
     *
     * @param p0  One end of the line segment. May not be null.
     * @param p1  The other end of the line segment. May not be null.
     * @param box The geometry element to extract the bounding box from. May not be null.
     * @return <code>true</code> if the line segment and geometry element bounding box
     *         intersects or overlaps (including the line segment contained completely
     *         inside of the box).
     */
    public static boolean lineSegBoundsIntersect(GeomPoint p0, GeomPoint p1, GeomElement box) {
        //  Reference:
        //  http://ggt.sourceforge.net/gmtlReferenceGuide-0.6.1-html/namespacegmtl.html#a4c99e51116eb3c68a52091d6a2480522
        requireNonNull(p0);
        requireNonNull(p1);
        requireNonNull(box);

        //  Define a point that represents the parametric position on the line where
        //  the intersection occurs.
        MutablePoint pt = MutablePoint.valueOf(-Double.MAX_VALUE, Double.MAX_VALUE);
        try {
            boolean result = lineBoundsIntersect(p0, p1.minus(p0).toGeomVector(), box, pt);
            if (!result)
                return false;

            double tIn = pt.getValue(0);
            double tOut = pt.getValue(1);
            if (tIn > 1.0)
                return false;
            if (tOut < 0.0)
                return false;

        } finally {
            MutablePoint.recycle(pt);
        }

        return true;
    }

    /**
     * Returns true if the specified infinite plane and bounding box of the specified
     * geometry element intersect. This is a aligned axis bounding box (AABB) - infinite
     * plane test.
     *
     * @param plane The infinite plane being used for the intersection test. May not be null.
     * @param box   The geometry element to extract the bounding box from. May not be null.
     * @return <code>true</code> if the plane and geometry element bounding box
     *         intersects.
     */
    public static boolean planeBoundsIntersect(GeomPlane plane, GeomElement box) {
        //  Reference:
        //  http://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?page=7
        //  http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/Lecture7.pdf

        int numDims = Math.max(plane.getPhyDimension(), box.getPhyDimension());
        if (numDims < 3)
            numDims = 3;

        StackContext.enter();
        try {
            //  Make sure everything uses the same units.
            Unit<Length> unit = box.getUnit();

            //  Make sure everything has the dimensions of the largest input object.
            GeomPoint P0 = plane.getRefPoint().toDimension(numDims).to(unit);
            GeomVector<Dimensionless> nhat = plane.getNormal().toDimension(numDims);

            //  Get the bounding box of the geometry element.
            Point bmin = box.getBoundsMin().toDimension(numDims).to(unit);
            Point bmax = box.getBoundsMax().toDimension(numDims).to(unit);

            //  Find center of the bounding box.
            Point p1 = bmin.plus(bmax).divide(2);

            //  Find extents of the bounding box.
            Point E1 = p1.minus(bmin);

            //  Find the shortest distance between the center of the box and the plane.
            double d = pointPlaneClosest(p1, P0, nhat).distanceValue(p1);

            //  Do the overlap test.
            double r = 0;
            for (int i = 0; i < numDims; ++i) {     //  Loop over each axis.
                double E1v = abs(E1.getValue(i));   //  Bounding box extents along this axis.
                double ni = abs(nhat.getValue(i));  //  Component of the plane normal vector.
                r += E1v * ni;
            }
            return d <= r;

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Return the approximate normal vector for a quadrilateral panel. Quadrilateral
     * panels are not necessarily planar, but in practical applications usually are close.
     * This method returns the cross-product of the diagonal vectors of the quadrilateral
     * panel which approximates the normal vector of the quadrilateral panel.
     *
     * @param quadA 1st corner point of a quadrilateral panel. May not be null.
     * @param quadB 2nd corner point in a counter-clockwise direction. May not be null.
     * @param quadC 3rd corner point in a counter-clockwise direction. May not be null.
     * @param quadD 4th corner point in a counter-clockwise direction. May not be null.
     * @return The normal vector formed by the cross-product of the diagonal vectors of
     *         the quadrilateral panel. Will be at least 3-dimensional no matter the
     *         inputs.
     */
    public static Vector<Dimensionless> quadNormal(GeomPoint quadA, GeomPoint quadB, GeomPoint quadC, GeomPoint quadD) {

        int numDims = maxPhyDimension(quadA, quadB, quadC, quadD);
        if (numDims < 3)
            numDims = 3;

        StackContext.enter();
        try {
            //  Make everything the same dimension.
            quadA = quadA.toDimension(numDims);
            quadB = quadB.toDimension(numDims);
            quadC = quadC.toDimension(numDims);
            quadD = quadD.toDimension(numDims);

            //  Compute diagonal vectors.
            Vector<Length> T1 = quadC.minus(quadA).toGeomVector();
            Vector<Length> T2 = quadD.minus(quadB).toGeomVector();

            // Compute normal vector as cross product:  N = T1 X T2
            Vector norm = T1.cross(T2);

            //  Compute average point.
            Point avg = quadA.plus(quadB).plus(quadC).plus(quadD).divide(4);
            norm.setOrigin(avg);

            //  Make the normal vector unit length.
            Vector<Dimensionless> n = norm.toUnitVector();
            return StackContext.outerCopy(n);

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Test to see if a line segment intersects a triangle.
     *
     * @param L0   The start point on the line segment (origin of the line). May not be
     *             null.
     * @param L1   The end point on the line segment. May not be null.
     * @param V0   1st corner point of a triangle. May not be null.
     * @param V1   2nd corner point in a counter-clockwise direction. May not be null.
     * @param V2   3rd corner point in a counter-clockwise direction. May not be null.
     * @param pOut A point that will be filled in with the intersection point in this
     *             triangle. If there is no intersection with this triangle, then pOut is
     *             not modified. Pass <code>null</code> if the intersection point is not
     *             required.
     * @return DISJOINT if the line and triangle are disjoint (do not intersect),
     *         INTERSECT if there is a unique intersection and COINCIDENT if the line is
     *         coincident with the plane of the triangle.
     */
    public static IntersectType lineSegTriIntersect(GeomPoint L0, GeomPoint L1,
            GeomPoint V0, GeomPoint V1, GeomPoint V2, MutablePoint pOut) {
        //  Reference: http://geomalgorithms.com/a06-_intersect-2.html#intersect3D_RayTriangle()

        int numDims = maxPhyDimension(L0, L1, V0, V1, V2);
        if (numDims < 3)
            numDims = 3;

        StackContext.enter();
        try {
            //  Make sure all the geometry is the same dimension and units.
            Unit<Length> unit = L0.getUnit();
            if (nonNull(pOut) && pOut.getPhyDimension() != numDims)
                throw new DimensionException(RESOURCES.getString("pOutDimErr"));
            L0 = L0.toDimension(numDims);
            L1 = L1.toDimension(numDims).to(unit);
            V0 = V0.toDimension(numDims).to(unit);
            V1 = V1.toDimension(numDims).to(unit);
            V2 = V2.toDimension(numDims).to(unit);

            //  Determine the triangle normal vector.
            Vector<Length> u = V1.minus(V0).toGeomVector();
            Vector<Length> v = V2.minus(V0).toGeomVector();
            Vector n = u.cross(v);
            if (n.magValue() < MathTools.SQRT_EPS) {
                //  Triangle is degenerate (a line or point).
                double um = u.magValue();
                double vm = v.magValue();
                if (um < MathTools.SQRT_EPS && vm < MathTools.SQRT_EPS) {
                    //  Triangle is actually a single point.
                    //  Does the input line segment cross this single point?
                    Parameter D = pointLineSegDistance(V0, L0, L1);
                    if (D.isApproxZero()) {
                        if (nonNull(pOut)) {
                            Point V0p = V0.immutable();
                            pOut.set(StackContext.outerCopy(V0p));
                        }
                        return IntersectType.INTERSECT;
                    } else
                        return IntersectType.DISJOINT;
                }

                //  The triangle is a straight line segment.
                if (um > vm)
                    v = u;
                Vector<Length> dir = L1.minus(L0).toGeomVector();
                MutablePoint sOut = MutablePoint.newInstance(2);
                IntersectType type = lineLineIntersect(L0, dir, V0, v, GTOL, sOut, null, null);
                if (type == IntersectType.INTERSECT) {
                    double s1 = sOut.getValue(0);
                    double s2 = sOut.getValue(1);
                    if (s1 > 0 && s1 < 1 && s2 > 0 && s2 < 1) {
                        //  The input line segment intersected the collapsed triangle line.
                        if (nonNull(pOut)) {
                            Point p = L0.plus(Point.valueOf(dir.times(s1)));
                            pOut.set(StackContext.outerCopy(p));
                        }
                        return IntersectType.INTERSECT;
                    }
                }
                return IntersectType.DISJOINT;
            }
            Vector<Dimensionless> nhat = n.toUnitVector();

            Vector<Length> dir = L1.minus(L0).toGeomVector();
            Vector<Length> w0 = L0.minus(V0).toGeomVector();
            double a = -nhat.dot(w0).getValue();
            double b = nhat.dot(dir).getValue();
            if (abs(b) < MathTools.SQRT_EPS) {
                //  Line is  parallel to triangle plane
                if (abs(a) < MathTools.SQRT_EPS)
                    //  Line lies in triangle plane
                    return IntersectType.COINCIDENT;
                else
                    //  Line disjoint from plane
                    return IntersectType.DISJOINT;
            }

            //  Get intersect point of line with triangle plane
            double rv = a / b;
            if (rv < 0.0) {
                //  Line goes away from triangle (=> no intersect)
                return IntersectType.DISJOINT;
            } else if (rv > 1.0) {
                //  Line does not reach the triangle (=> no intersect)
                return IntersectType.DISJOINT;
            }

            //  Intersect point of line and triangle
            Point I = L0.plus(Point.valueOf(dir.times(rv)));     //  I = L0 + r * dir

            //  Is I inside T?
            double uu = u.dot(u).getValue();
            double uv = u.dot(v).getValue();
            double vv = v.dot(v).getValue();
            Vector<Length> w = I.minus(V0).toGeomVector();
            double wu = w.dot(u).getValue();
            double wv = w.dot(v).getValue();
            double D = uv * uv - uu * vv;

            //  Get and test parametric coords
            double s = (uv * wv - vv * wu) / D;
            if (s < 0.0 || s > 1.0) {
                // I is outside T
                return IntersectType.DISJOINT;
            }
            double t = (uv * wu - uu * wv) / D;
            if (t < 0.0 || (s + t) > 1.0) {
                // I is outside T
                return IntersectType.DISJOINT;
            }

            //  Change the output point if we get this far.
            if (nonNull(pOut)) {
                pOut.set(StackContext.outerCopy(I));
            }

            return IntersectType.INTERSECT;

        } finally {
            StackContext.exit();
        }
    }

    /**
     * Test to see if an infinite line intersects a quadrilateral panel.
     *
     * @param L0    A point on the line (origin of the line). May not be null.
     * @param Ldir  The direction vector for the line (does not have to be a unit vector).
     *              May not be null.
     * @param quadA 1st corner point of a quadrilateral panel. May not be null.
     * @param quadB 2nd corner point in a counter-clockwise direction. May not be null.
     * @param quadC 3rd corner point in a counter-clockwise direction. May not be null.
     * @param quadD 4th corner point in a counter-clockwise direction. May not be null.
     * @param nhat  The normal vector for the quadrilateral panel. May not be null.
     * @param sOut  A 2D point that will be filled in with the parametric position of the
     *              intersection point in this quad (s, t) where "s" is the fractional
     *              distance between corners AB (or DC) and "t" is the fractional distance
     *              between corners AD (or BC). If there is no intersection with this
     *              panel, then sOut is not modified. Pass <code>null</code> if the
     *              parametric position is not required.
     * @return <code>true</code> if the line and quadrilateral panel intersect.
     */
    public static boolean lineQuadIntersect(GeomPoint L0, GeomVector Ldir,
            GeomPoint quadA, GeomPoint quadB, GeomPoint quadC, GeomPoint quadD,
            GeomVector nhat, MutablePoint sOut) {
        //  Reference:
        //  Graphic Gems V:  http://tog.acm.org/resources/GraphicsGems/gemsv/ch5-2/quad_gg.c

        //  Make sure all the geometry is the same dimension and units.
        Unit<Length> unit = L0.getUnit();
        int numDims = maxPhyDimension(L0, Ldir, quadA, quadB, quadC, quadD, nhat);
        if (numDims < 3)
            numDims = 3;
        Ldir = Ldir.toDimension(numDims).toUnitVector();
        nhat = nhat.toDimension(numDims).toUnitVector();

        // If the line is parallel to the facet, there is no intersection.
        Parameter D = Ldir.dot(nhat);
        if (D.isApproxZero())
            return false;

        L0 = L0.toDimension(numDims);
        quadA = quadA.toDimension(numDims).to(unit);
        quadB = quadB.toDimension(numDims).to(unit);
        quadC = quadC.toDimension(numDims).to(unit);
        quadD = quadD.toDimension(numDims).to(unit);

        // Compute line intersection with the plane of the facet
        Plane qPln = Plane.valueOf(nhat, averagePoints(quadA,quadB,quadC,quadD));
        MutablePoint hit = MutablePoint.newInstance(numDims);
        IntersectType intersection = linePlaneIntersect(L0, Ldir, qPln, hit);
        Plane.recycle(qPln);
        if (intersection != IntersectType.INTERSECT) {
            MutablePoint.recycle(hit);
            return false;
        }

        //  Is the intersection point inside the facet
        boolean inside = pointInQuad(quadA, quadB, quadC, quadD, nhat, hit, sOut);
        MutablePoint.recycle(hit);

        return inside;
    }

    /**
     * Tests if the specified point, in the plane of an approximately planar quadrilateral
     * panel, is inside the boundaries of the quadrilateral panel or not.
     */
    private static boolean pointInQuad(GeomPoint quadA, GeomPoint quadB, GeomPoint quadC, GeomPoint quadD,
            GeomVector nhat, GeomPoint hit, MutablePoint sOut) {
        //  Reference:
        //  Graphic Gems V:  http://tog.acm.org/resources/GraphicsGems/gemsv/ch5-2/quad_gg.c
        
        double u = 0, v = 0;
        boolean isInside = false;

        StackContext.enter();
        try {
            //  Projection on the plane that is most parallel to the facet
            int largestComp = largestComponent(nhat);

            Unit<Length> unit = quadA.getUnit();
            MutablePoint A = MutablePoint.newInstance(2, unit);        //  Projected vertices
            MutablePoint B = MutablePoint.newInstance(2, unit);
            MutablePoint C = MutablePoint.newInstance(2, unit);
            MutablePoint D = MutablePoint.newInstance(2, unit);
            MutablePoint M = MutablePoint.newInstance(2, unit);        //  Projected intersection point.
            if (largestComp == 0) {         //  X
                A.set(0, quadA.get(1));     //  A.x = Quad->A.y;
                B.set(0, quadB.get(1));     //  B.x = Quad->B.y;
                C.set(0, quadC.get(1));     //  C.x = Quad->C.y;
                D.set(0, quadD.get(1));     //  D.x = Quad->D.y;
                M.set(0, hit.get(1));       //  M.x = hit.y
            } else {
                A.set(0, quadA.get(0));     //  A.x = Quad->A.x;
                B.set(0, quadB.get(0));     //  B.x = Quad->B.x;
                C.set(0, quadC.get(0));     //  C.x = Quad->C.x;
                D.set(0, quadD.get(0));     //  D.x = Quad->D.x;
                M.set(0, hit.get(0));       //  M.x = hit.x;
            }

            if (largestComp == 2) {         //  Z
                A.set(1, quadA.get(1));     //  A.y = Quad->A.y;
                B.set(1, quadB.get(1));     //  B.y = Quad->B.y;
                C.set(1, quadC.get(1));     //  C.y = Quad->C.y;
                D.set(1, quadD.get(1));     //  D.y = Quad->D.y;
                M.set(1, hit.get(1));       //  M.y = Hit->Point.y;

            } else {
                A.set(1, quadA.get(2));     //  A.y = Quad->A.z;
                B.set(1, quadB.get(2));     //  B.y = Quad->B.z;
                C.set(1, quadC.get(2));     //  C.y = Quad->C.z;
                D.set(1, quadD.get(2));     //  D.y = Quad->D.z;
                M.set(1, hit.get(2));       //  M.y = Hit->Point.z;
            }

            Vector AB = B.minus(A).toGeomVector();
            Vector BC = C.minus(B).toGeomVector();
            Vector CD = D.minus(C).toGeomVector();
            Vector AD = D.minus(A).toGeomVector();
            Vector AE = CD.plus(AB).opposite();         //  AE = DC - AB = DA - CB
            Vector AM = M.minus(A).toGeomVector();

            if (abs(determinant2D(AB, CD)) < SQRT_EPS) {
                //  case AB // CD
                Vector vec = AB.minus(CD);
                v = determinant2D(AM, vec) / determinant2D(AD, vec);
                if ((v >= -SQRT_EPS) && (v <= 1.0 + SQRT_EPS)) {
                    double b = determinant2D(AB, AD) - determinant2D(AM, AE);
                    double c = determinant2D(AM, AD);
                    u = abs(b) <= EPS ? -1 : c / b;
                    isInside = ((u >= -SQRT_EPS) && (u <= 1.0 + SQRT_EPS));
                }

            } else if (abs(determinant2D(BC, AD)) < SQRT_EPS) {
                //  case AD // BC
                Vector vec = AD.plus(BC);
                u = determinant2D(AM, vec) / determinant2D(AB, vec);
                if ((u >= -SQRT_EPS) && (u <= 1.0 + SQRT_EPS)) {
                    double b = determinant2D(AD, AB) - determinant2D(AM, AE);
                    double c = determinant2D(AM, AB);
                    v = abs(b) <= EPS ? -1 : c / b;
                    isInside = ((v >= -SQRT_EPS) && (v <= 1.0 + SQRT_EPS));
                }

            } else {
                //  General case
                double a = determinant2D(AB, AE);
                double c = -determinant2D(AM, AD);
                double b = determinant2D(AB, AD) - determinant2D(AM, AE);
                a = -0.5 / a;
                b *= a;
                c *= (a + a);
                double SqrtDelta = b * b + c;
                if (SqrtDelta >= 0.0) {
                    SqrtDelta = sqrt(SqrtDelta);
                    u = b - SqrtDelta;
                    if ((u < -SQRT_EPS) || (u > 1.0 + SQRT_EPS))     // to choose u between 0 and 1
                        u = b + SqrtDelta;
                    if ((u >= -SQRT_EPS) && (u <= 1.0 + SQRT_EPS)) {
                        v = AD.getValue(0) + u * AE.getValue(0);
                        if (abs(v) <= EPS)
                            v = (AM.getValue(1) - u * AB.getValue(1)) / (AD.getValue(1) + u * AE.getValue(1));
                        else
                            v = (AM.getValue(0) - u * AB.getValue(0)) / v;
                        isInside = ((v >= -SQRT_EPS) && (v <= 1.0 + SQRT_EPS));
                    }
                }
            }

        } finally {
            StackContext.exit();
        }

        if (isInside && nonNull(sOut) && sOut.getPhyDimension() > 1) {
            sOut.setValue(0, u);
            sOut.setValue(1, v);
        }

        return isInside;
    }

    private static int largestComponent(GeomVector A) {
        int maxDim = 0;
        double maxValue = -1;
        int numDims = A.getPhyDimension();
        for (int i = 0; i < numDims; ++i) {
            double v = abs(A.getValue(i));
            if (v > maxValue) {
                maxValue = v;
                maxDim = i;
            }
        }
        return maxDim;
    }

    private static double determinant2D(Vector A, Vector B) {
        return A.getValue(Vector.X) * B.getValue(Vector.Y) - A.getValue(Vector.Y) * B.getValue(Vector.X);
    }

    /**
     * Test to see if an infinite plane intersects a quadrilateral panel.
     *
     * @param P0    A reference point in the plane to test for intersection with a
     *              quadrilateral panel. May not be null.
     * @param nhat  The unit normal vector for the plane. May not be null.
     * @param quadA 1st corner point of a quadrilateral panel. May not be null.
     * @param quadB 2nd corner point in a counter-clockwise direction. May not be null.
     * @param quadC 3rd corner point in a counter-clockwise direction. May not be null.
     * @param quadD 4th corner point in a counter-clockwise direction. May not be null.
     * @return <code>true</code> if the plane and quadrilateral panel intersect.
     */
    public static boolean planeQuadIntersect(GeomPoint P0, GeomVector<Dimensionless> nhat, 
            GeomPoint quadA, GeomPoint quadB, GeomPoint quadC, GeomPoint quadD) {

        int numDims = maxPhyDimension(P0, nhat, quadA, quadB, quadC, quadD);

        StackContext.enter();
        try {
            //  Convert everything to the same physical dimensions and units.
            Unit<Length> unit = quadA.getUnit();
            P0 = P0.toDimension(numDims).to(unit);
            nhat = nhat.toDimension(numDims);
            quadA = quadA.toDimension(numDims).to(unit);
            quadB = quadB.toDimension(numDims).to(unit);
            quadC = quadC.toDimension(numDims).to(unit);
            quadD = quadD.toDimension(numDims).to(unit);

            boolean intersects = false;

            //  Edge 1
            Parameter<Length> d0 = pointPlaneDistance(quadA, P0, nhat);
            Parameter<Length> d1 = pointPlaneDistance(quadB, P0, nhat);
            double d0v = d0.getValue();
            if (d0v * d1.getValue() < 0) {
                //  An intersection on this patch edge exists.
                intersects = true;

            } else {
                //  Edge 4
                Parameter<Length> d2 = pointPlaneDistance(quadD, P0, nhat);
                if (d0v * d2.getValue() < 0) {
                    //  An intersection on this patch edge exists.
                    intersects = true;

                } else {
                    //  Edge 3
                    d0 = GeomUtil.pointPlaneDistance(quadC, P0, nhat);
                    d0v = d0.getValue();
                    if (d0v * d2.getValue() < 0) {
                        intersects = true;
                    } else {
                        //  Edge 2
                        if (d0v * d1.getValue() < 0) {
                            intersects = true;
                        }
                    }
                }
            }

            return intersects;

        } finally {
            StackContext.exit();
        }
    }

    /**
     * <p>
     * Method that detects corners in a a list of 2D points which represent a planar
     * curve. The input array "sValues" is filled in with the sharpness of a corner
     * detected at each point. If no corner is detected at a point, that entry in
     * "sValues" is filled in with Math.PI. The opening angle of a triangle fit to each
     * corner can be calculated as: <code>alpha[i] = Math.PI - sValues[i];</code>.
     * </p>
     *
     * @param points  A list of at least 2D GeomPoint objects making up a planar curve. If
     *                the input points are greater than 2D, then the higher dimensions are
     *                simply ignored.
     * @param sValues An existing array of doubles of size points.size(). The sharpness
     *                (in radians) of a corner applied to each point will be stored here.
     * @param dmin    The minimum distance to consider (any points closer together than
     *                this are ignored.
     * @param dmax    The maximum distance to consider (any points further apart than this
     *                are ignored).
     * @param amax    The maximum opening angle to consider as a corner.
     * @return A count of the number of corners (sharper than <code>amax</code>) detected
     * in the curve.
     */
    public static int detectCorners(List<? extends GeomPoint> points, double[] sValues,
            Parameter<Length> dmin, Parameter<Length> dmax, Parameter<Angle> amax) {
        /*
            Uses the IPAN99 algorithm described in: Chetverinkov, D., Szabo, Z.,
            "A Simple and Efficient Algorithm for Detection of High Curvature Points
            in Planar Curves", Proc. 23rd Workshop of Austrian Pattern Recognition Group,
            Steyr, pp. 175-184, 1999.
         */
        
        //  Convert the input geometry to 2D.
        PointString pointsLst;
        if (points instanceof PointString)
            pointsLst = (PointString)points;
        else {
            pointsLst = PointString.newInstance();
            pointsLst.addAll(points);
        }
        if (pointsLst.getPhyDimension() < 2) {
            throw new IllegalArgumentException(
                    MessageFormat.format(RESOURCES.getString("dimensionNotAtLeast2"),
                            "input points", pointsLst.getPhyDimension()));
        }
        pointsLst = pointsLst.toDimension(2);
        
        //  Extract some things we need.
        Unit<Length> units = pointsLst.getUnit();
        double dminv = dmin.getValue(units);
        double dminv2 = dminv*dminv;
        double dmaxv = dmax.getValue(units);
        double dmaxv2 = dmaxv*dmaxv;
        double amaxv = amax.getValue(SI.RADIAN);

        //  Loop over all the points in this curve and
        //  calculate the opening angle of the sharpest triangle
        //  that will fit the curve at that point.
        int numPntsm1 = pointsLst.size() - 1;
        for (int j = 1; j < numPntsm1; ++j) {
            //  Return the sharpness of the sharpest triangle fit
            double sharpness = fitTriangle(pointsLst, j, dminv2, dmaxv2, amaxv);
            sValues[j] = sharpness;
        }
        
        //  Remove spurious corners.
        int cornerCount = removeAdjacentCorners(pointsLst, sValues, dminv2);
        
        return cornerCount;
    }

    /**
     *  Method that fits a series of triangles to a list of points
     *  at the specified index and returns the sharpness of the sharpest
     *  triangle.
     *
     *  @param  points  A list of 2D GeomPoint objects making up a planar curve.
     *  @param  ip      Index to the point for the tip of the triangle.  Can not be
     *                  outside range 1 to points.length-2.
     *  @param  dmin2   The minimum distance to consider squared.
     *  @param  dmax2   The maximum distance to consider squared.
     *  @param  amax    The maximum opening angle to consider (in radians).
     *  @return The sharpness of the sharpest triangle found is returned (radians).
     */
    private static double fitTriangle(PointString<GeomPoint> points, int ip, double dmin2, double dmax2, double amax) {
        double sharpest = Math.PI;
        
        GeomPoint p = points.get(ip);
        int ipm = ip - 1;
        int ipp = ip + 1;
        boolean firstPass = true;
        boolean notDone = true;
        while (notDone) {
            //  Get end points of the triangle.
            GeomPoint pm = points.get(ipm);
            GeomPoint pp = points.get(ipp);
            double a2 = p.distanceSqValue(pp);
            double b2 = p.distanceSqValue(pm);
            
            //  Check criteria.
            boolean c2 = (a2 < dmax2) || firstPass;
            boolean c4 = (b2 < dmax2) || firstPass;
            if (c2 && c4 && a2 > dmin2 && b2 > dmin2) {
                firstPass = false;
                
                //  Calculate the opening angle of the triangle.
                double alpha = calcAlpha(a2, b2, pp.distanceSqValue(pm));
                
                if (alpha < amax) {
                    //  Calculate sharpness for this triangle.
                    double sharpness = Math.PI - Math.abs(alpha);
                    if (sharpness < sharpest)
                        sharpest = sharpness;
                } else
                    notDone = false;
            }
            
            //  If we are not further away than the tolerance, then try another triangle.
            if (c2 && c4) {
                ++ipp;
                --ipm;
                if (ipp > points.size() - 1)
                    ipp = points.size() - 1;
                if (ipm < 0)
                    notDone = false;
            } else {
                notDone = false;
            }
        }
        
        return sharpest;
    }

    /**
     * Method that calculates a triangle opening angle (the angle at the tip of
     * the triangle that has legs of length "a", and "b" and a base of length
     * "c"). Returns the opening angle in radians.
     */
    private static double calcAlpha(double a2, double b2, double c2) {
        double ab = Math.sqrt(a2 * b2);
        double den = 2 * ab;
        double num = a2 + b2 - c2;
        return Math.acos(num / den);
    }

    /**
     * Method that removes adjacent corners. It searches through the provided array of
     * corners and removes any that are closer together than <code>dmin2</code>.
     *
     * @param points  A list of 2D GeomPoint objects representing the points in a planar curve.
     * @param sValues An array of sharpness values for each point in the point list
     *                (radians).
     * @param tol2    The square of the tolerance to use on determining if corners are
     *                adjacent.
     * @return A count of the number of corners remaining in the curve.
     */
    private static int removeAdjacentCorners(PointString<GeomPoint> points, double[] sValues, double tol2) {
        int cornerCount = 0;
        int numPntsm1 = points.size() - 1;

        for (int j = 1; j < numPntsm1; ++j) {
            double sharpness = sValues[j];
            if (sharpness < Math.PI) {
                //  Potential corner found.
                ++cornerCount;

                //  Does it have any sharper neighbors.
                int pp = j + 1;
                GeomPoint pointj = points.get(j);

                while (pp < numPntsm1) {
                    //  Make sure we aren't to far away from "j".
                    double d2 = pointj.distanceSqValue(points.get(pp));
                    if (d2 >= tol2)
                        break;

                    if (sValues[pp] < sharpness) {
                        //  Sharper neighbor found, discard corner "j".
                        sValues[j] = Math.PI;
                        --cornerCount;
                        break;

                    } else // If it is not sharper than "j", then discard it.
                        sValues[pp] = Math.PI;

                    //  Move on to the next point.
                    ++pp;
                }
            }
        }

        return cornerCount;
    }

    /**
     * Return an ordered list of 2D points that represent the convex hull of a
     * collection of unordered 2D points.
     *
     * @param points A collection of 2D points to find the convex hull of. If
     * the input points have dimensions &gt; 2, then only the 1st two dimensions
     * are used (higher dimensions are dropped).
     * @return The ordered 2D points representing the convex hull of the input
     * collection of points.
     */
    public static PointString<Point> convexHull(Collection<? extends GeomPoint> points) {
        if (points.isEmpty())
            return PointString.newInstance();
        
        //  Convert the GeomSS points into a set of apache commons 2D vectors.
        Unit<Length> units = null;
        boolean firstPass = true;
        Collection<Vector2D> points2d = new ArrayList();
        for (GeomPoint p : points) {
            if (firstPass) {
                firstPass = false;
                units = p.getUnit();
            }
            Vector2D v2d = new Vector2D(p.getValue(Point.X, units), p.getValue(Point.Y, units));
            points2d.add(v2d);
        }
        
        //  Create a convex hull from the 2D points.
        points2d = AklToussaintHeuristic.reducePoints(points2d);
        MonotoneChain mchain = new MonotoneChain();
        List<Vector2D> vertices2d = new ArrayList(mchain.findHullVertices(points2d));
 
        //  Convert the 2D apache commons verticies into 2D GeomSS Point objects.
        PointString<Point> chullPnts = PointString.newInstance();
        for (Vector2D v2d : vertices2d) {
            chullPnts.add(Point.valueOf(v2d.getX(),v2d.getY(),units));
        }
        
        return chullPnts;
    }
    
    /**
     * Return a list of strings of ordered 2D points that represent the 2D
     * convex hulls of each input collection of 2D points. The convex hull for
     * each collection of points in the list will be run concurrently.
     *
     * @param listOfPointCollections The list of collections of 2D points to
     * find the convex hull of. If the input points have dimensions &gt; 2, then
     * only the 1st two dimensions are used (higher dimensions are dropped).
     * @return A list of strings of ordered 2D points that represent the convex
     * hull of the input collections of points.
     */
    public static GeomList<PointString<Point>> convexHullList(List<Collection<? extends GeomPoint>> listOfPointCollections) {
        if (listOfPointCollections.isEmpty())
            return GeomList.newInstance();
        
        //  Create an array to hold the output strings of points.
        PointString<Point>[] outputArr = new PointString[listOfPointCollections.size()];
        
        //  Use a ConcurrentContext to run each convex hull in parallel.
        ConcurrentContext.enter();
        try {
            int size = listOfPointCollections.size();
            for (int i=0; i < size; ++i) {
                Collection<? extends GeomPoint> pointCollection = listOfPointCollections.get(i);
                
                //  Run each convex hull concurrently on its own thread.
                Runnable runner = new ConvexHullRunner(pointCollection, outputArr, i);
                ConcurrentContext.execute(runner);
            }
        } finally {
            ConcurrentContext.exit();
        }

        //  Convert the outputs from an array to a GeomList.
        GeomList<PointString<Point>> output = GeomList.newInstance();
        output.addAll(outputArr);
        
        return output;
    }

    /**
     * A Runnable for use with computing the convex hull of a collection of 2D points.
     */
    private static class ConvexHullRunner implements Runnable {
        private final Collection<? extends GeomPoint> points;
        private final PointString<Point>[] output;
        private final int idx;
        
        public ConvexHullRunner(Collection<? extends GeomPoint> points, 
                PointString<Point>[] outputArr, int outIdx) {
            this.points = points;
            this.output = outputArr;
            this.idx = outIdx;
        }

        @Override
        public void run() {
            //  Run the convex hull on this collection of points and store the outputs.
            output[idx] = convexHull(points);
        }
    }
}