/*****************************************************************************
 *                           J3D.org Copyright (c) 2000
 *                                Java Source
 *
 * This source is licensed under the GNU LGPL v2.1
 * Please read http://www.gnu.org/copyleft/lgpl.html for more information
 *
 ****************************************************************************/

package org.j3d.geom;

// Standard imports
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Matrix4d;

// Application specific imports
import org.j3d.util.UserSupplementData;

/**
 * A collection of utility methods to do geometry intersection tests.
 * <p>
 *
 * The design of the implementation is focused towards realtime intersection
 * requirements for collision detection and terrain following. We avoid the
 * standard pattern of making the methods static because we believe that you
 * may need multiple copies of this class floating around. Internally it will
 * also seek to reduce the amount of garbage generated by allocating arrays of
 * data and then maintaining those arrays between calls. Arrays are only
 * resized if they need to get bigger. Smaller data than the currently
 * allocated structures will use the existing data. For the same reason, we
 * do not synchronise any of the methods. If you expect to have multiple
 * threads needing to do intersection testing, we suggest you have separate
 * copies of this class as no results are guaranteed if you are accessing this
 * instance with multiple threads.
 * <p>
 *
 * Calculation of the values works by configuring the class for the sort of
 * data that you want returned. For the higher level methods that allow you
 * <p>
 *
 * If you need the intersection tools for collision detection only, then you
 * can tell the routines that you only need to know if they intersect. That is
 * as soon as you detect one polygon that intersects, exit immediately. This is
 * useful for doing collision detection because you really don't care where on
 * the object you collide, just that you have.
 * <p>
 *
 * The ray/polygon intersection test is a combination test. Firstly it will
 * check for the segment intersection if requested. Then, for an infinite ray
 * or an intersecting segment, we use the algorithm defined from the Siggraph
 * paper in their education course:
 * <ul>
 * <li><a href="http://www.education.siggraph.org/materials/HyperGraph/raytrace/raypolygon_intersection.htm">
 * Ray-Polygon</a></li>
 * <li><a href="http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter1.htm">Ray-Sphere</a></li>
 * <li><a href="http://www.siggraph.org/education/materials/HyperGraph/raytrace/rayplane_intersection.htm">Ray-Plane</a></li>
 * <li><a href="http://www.2tothex.com/raytracing/primitives.html">Ray-Cylinder</a></li>
 * </ul>
 *
 * @author Justin Couch
 * @version $Revision: 1.20 $
 */
public class IntersectionUtils
{
    /** Cylinder intersection axis X */
    public static final int X_AXIS = 1;

    /** Cylinder intersection axis Y */
    public static final int Y_AXIS = 2;

    /** Cylinder intersection axis Z */
    public static final int Z_AXIS = 3;

    /** A point that we use for working calculations (coord transforms) */
    private Point3d wkPoint;
    private Vector3d wkVec;

    /** Working vectors */
    private Vector3d v0;
    private Vector3d v1;
    private Vector3d normal;
    private Vector3d diffVec;

    /** Transformed pick items */
    protected Point3d pickStart;
    protected Vector3d pickDir;

    /** The current coordinate list that we work from */
    protected float[] workingCoords;
    protected int[] workingStrips;
    protected int[] workingIndicies;

    /** The current 2D coordinate list that we work from */
    protected float[] working2dCoords;

    /** Working places for a single quad */
    protected float[] wkPolygon;

    /** Place to invert the incoming transform for reverse mappings */
    private Matrix4d reverseTx;

    /**
     * Create a default instance of this class with no internal data
     * structures allocated.
     */
    public IntersectionUtils()
    {
        wkPoint = new Point3d();
        wkVec = new Vector3d();
        v0 = new Vector3d();
        v1 = new Vector3d();
        normal = new Vector3d();
        diffVec = new Vector3d();
        wkPolygon = new float[12];
        reverseTx = new Matrix4d();

        pickStart = new Point3d();
        pickDir = new Vector3d();
    }

    /**
     * Clear the current internal structures to reduce the amount of memory
     * used. It is recommended you use this method with caution as then next
     * time a user calls this class, all the internal structures will be
     * reallocated. If this is running in a realtime environment, that could
     * be very costly - both allocation and the garbage collection that
     * results from calling this method
     */
    public void clear()
    {
        workingCoords = null;
        working2dCoords = null;
        workingStrips = null;
        workingIndicies = null;
    }


    /**
     * Convenience method to process a {@link GeometryData} and ask the
     * intersection code to find out what the real geometry type is and
     * process it appropriately. If there is an intersection, the point will
     * contain the exact intersection point on the geometry.
     * <P>
     *
     * This code will be much more efficient than the other version because
     * we do not need to reallocate internal arrays all the time or have the
     * need to set capability bits, hurting performance optimisations. If the
     * geometry array does not understand the provided geometry type, it will
     * silently ignore the request and always return false.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param data The geometry to test against
     * @param point The intersection point for returning
     * @param vworldTransform Transformation matrix to go from the root of the
     *    world to this point
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayUnknownGeometry(Point3d origin,
                                      Vector3d direction,
                                      float length,
                                      GeometryData data,
                                      Matrix4d vworldTransform,
                                      Point3d point,
                                      boolean intersectOnly)
    {
        boolean ret_val = false;

        reverseTx.invert(vworldTransform);
        transformPicks(reverseTx, origin, direction);

        switch(data.geometryType)
        {
            case GeometryData.TRIANGLES:
                ret_val = rayTriangleArray(pickStart,
                                           pickDir,
                                           length,
                                           data.coordinates,
                                           data.vertexCount / 3,
                                           point,
                                           intersectOnly);
                break;

            case GeometryData.QUADS:
                ret_val = rayQuadArray(pickStart,
                                       pickDir,
                                       length,
                                       data.coordinates,
                                       data.vertexCount / 4,
                                       point,
                                       intersectOnly);
                break;

            case GeometryData.TRIANGLE_STRIPS:
                ret_val = rayTriangleStripArray(pickStart,
                                                pickDir,
                                                length,
                                                data.coordinates,
                                                data.stripCounts,
                                                data.numStrips,
                                                point,
                                                intersectOnly);
                break;

            case GeometryData.TRIANGLE_FANS:
                ret_val = rayTriangleFanArray(pickStart,
                                              pickDir,
                                              length,
                                              data.coordinates,
                                              data.stripCounts,
                                              data.numStrips,
                                              point,
                                              intersectOnly);
                break;

            case GeometryData.INDEXED_QUADS:
                ret_val = rayIndexedQuadArray(pickStart,
                                              pickDir,
                                              length,
                                              data.coordinates,
                                              data.indexes,
                                              data.indexesCount,
                                              point,
                                              intersectOnly);
                break;

            case GeometryData.INDEXED_TRIANGLES:
                ret_val = rayIndexedTriangleArray(pickStart,
                                                  pickDir,
                                                  length,
                                                  data.coordinates,
                                                  data.indexes,
                                                  data.indexesCount,
                                                  point,
                                                  intersectOnly);
                break;
/*
            case GeometryData.INDEXED_TRIANGLE_STRIPS:
                ret_val = rayTriangleArray(pickStart,
                                           pickDir,
                                           length,
                                           data.coordinates,
                                           data.vertexCount,
                                           point,
                                           intersectOnly);
                break;

            case GeometryData.INDEXED_TRIANGLE_FANS:
                ret_val = rayTriangleArray(pickStart,
                                           pickDir,
                                           length,
                                           data.coordinates,
                                           data.vertexCount,
                                           point,
                                           intersectOnly);
                break;
*/
        }

        if(ret_val)
            vworldTransform.transform(point);

        return ret_val;
    }

    /**
     * Test an array of triangles for intersection. Returns the closest
     * intersection point to the origin of the picking ray. Assumes that the
     * coordinates are ordered as [Xn, Yn, Zn] and are translated into the same
     * coordinate system that the the origin and direction are from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the triangles
     * @param numTris The number of triangles to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayTriangleArray(Point3d origin,
                                    Vector3d direction,
                                    float length,
                                    float[] coords,
                                    int numTris,
                                    Point3d point,
                                    boolean intersectOnly)
    {
        if(numTris == 0)
            return false;

        if(coords.length < numTris * 9)
            throw new IllegalArgumentException("coords too small for numCoords");

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;

        for(int i = 0; i < numTris; i++)
        {
            System.arraycopy(coords, i * 9, wkPolygon, 0, 9);

            if(rayPolygonChecked(origin,
                                 direction,
                                 length,
                                 wkPolygon,
                                 3,
                                 wkPoint))
            {
                found = true;
                diffVec.sub(origin, wkPoint);
                found = true;

                this_length = diffVec.lengthSquared();

                if(this_length < shortest_length)
                {
                    shortest_length = this_length;
                    point.set(wkPoint);

                    if(intersectOnly)
                        break;
                }
            }
        }

        return found;
    }

    /**
     * Test an array of quads for intersection. Returns the closest
     * intersection point to the origin of the picking ray. Assumes that the
     * coordinates are ordered as [Xn, Yn, Zn] and are translated into the same
     * coordinate system that the the origin and direction are from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the quads
     * @param numQuads The number of quads to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayQuadArray(Point3d origin,
                                Vector3d direction,
                                float length,
                                float[] coords,
                                int numQuads,
                                Point3d point,
                                boolean intersectOnly)
    {
        if(numQuads == 0)
            return false;

        if(coords.length < numQuads * 12)
            throw new IllegalArgumentException("coords too small for numCoords");

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;

        for(int i = 0; i < numQuads; i++)
        {
            System.arraycopy(coords, i * 12, wkPolygon, 0, 12);

            if(rayPolygonChecked(origin,
                                 direction,
                                 length,
                                 wkPolygon,
                                 4,
                                 wkPoint))
            {
                found = true;
                diffVec.sub(origin, wkPoint);

                this_length = diffVec.lengthSquared();

                if(this_length < shortest_length)
                {
                    shortest_length = this_length;
                    point.set(wkPoint);

                    if(intersectOnly)
                        break;
                }
            }
        }

        return found;
    }

    /**
     * Test an array of triangles strips for intersection. Returns the closest
     * intersection point to the origin of the picking ray. Assumes that the
     * coordinates are ordered as [Xn, Yn, Zn] and are translated into the same
     * coordinate system that the the origin and direction are from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the triangles
     * @param stripCounts The number of polygons in each strip
     * @param numStrips The number of strips to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayTriangleStripArray(Point3d origin,
                                         Vector3d direction,
                                         float length,
                                         float[] coords,
                                         int[] stripCounts,
                                         int numStrips,
                                         Point3d point,
                                         boolean intersectOnly)
    {
        if(numStrips == 0)
            return false;

        // Add all the strip lengths up first
        int total_coords = 0;

        for(int i = numStrips; --i >= 0; )
            total_coords += stripCounts[i];

        if(coords.length < total_coords * 3)
            throw new IllegalArgumentException("coords too small for numCoords");

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;
        int offset = 0;

        for(int i = 0; i < numStrips; i++)
        {
            for(int j = 0; j < stripCounts[i] - 2; j++)
            {
                System.arraycopy(coords, offset, wkPolygon, 0, 9);

                if(rayPolygonChecked(origin,
                                     direction,
                                     length,
                                     wkPolygon,
                                     3,
                                     wkPoint))
                {
                    found = true;
                    diffVec.sub(origin, wkPoint);

                    this_length = diffVec.lengthSquared();

                    if(this_length < shortest_length)
                    {
                        shortest_length = this_length;
                        point.set(wkPoint);

                        if(intersectOnly)
                            break;
                    }
                }

                offset += 3;
            }

            // shift the offset onto the start of the next strip.
            offset += 6;
        }

        return found;
    }

    /**
     * Test an array of triangle fans for intersection. Returns the closest
     * intersection point to the origin of the picking ray. Assumes that the
     * coordinates are ordered as [Xn, Yn, Zn] and are translated into the same
     * coordinate system that the the origin and direction are from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the triangles
     * @param stripCounts The number of polygons in each fan
     * @param numStrips The number of strips to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayTriangleFanArray(Point3d origin,
                                       Vector3d direction,
                                       float length,
                                       float[] coords,
                                       int[] stripCounts,
                                       int numStrips,
                                       Point3d point,
                                       boolean intersectOnly)
    {
        if(numStrips == 0)
            return false;

        // Add all the strip lengths up first
        int total_coords = 0;

        for(int i = numStrips; --i >= 0; )
            total_coords += stripCounts[i];

        if(coords.length < total_coords * 3)
            throw new IllegalArgumentException("coords too small for numCoords");

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;
        int offset = 0;

        for(int i = 0; i < numStrips; i++)
        {
            // setup the constant first position
            wkPolygon[0] = coords[offset];
            wkPolygon[1] = coords[offset + 1];
            wkPolygon[2] = coords[offset + 2];

            for(int j = 1; j < stripCounts[i] - 2; j++)
            {
                wkPolygon[3] = coords[offset + j * 3];
                wkPolygon[4] = coords[offset + j * 3 + 1];
                wkPolygon[5] = coords[offset + j * 3 + 2];

                wkPolygon[6] = coords[offset + j * 3 + 3];
                wkPolygon[7] = coords[offset + j * 3 + 4];
                wkPolygon[8] = coords[offset + j * 3 + 5];

                // Now the rest of the polygon
                if(rayPolygonChecked(origin,
                                     direction,
                                     length,
                                     wkPolygon,
                                     3,
                                     wkPoint))
                {
                    found = true;
                    diffVec.sub(origin, wkPoint);

                    this_length = diffVec.lengthSquared();

                    if(this_length < shortest_length)
                    {
                        shortest_length = this_length;
                        point.set(wkPoint);

                        if(intersectOnly)
                            break;
                    }
                }
            }

            offset += stripCounts[i] * 3;
        }

        return found;
    }

    /**
     * Test an array of indexed triangles for intersection. Returns the
     * closest intersection point to the origin of the picking ray. Assumes
     * that the coordinates are ordered as [Xn, Yn, Zn] and are translated
     * into the same coordinate system that the the origin and direction are
     * from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the triangles
     * @param indexes The list of indexes to use to construct triangles
     * @param numIndex The number of indexes to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayIndexedTriangleArray(Point3d origin,
                                           Vector3d direction,
                                           float length,
                                           float[] coords,
                                           int[] indexes,
                                           int numIndex,
                                           Point3d point,
                                           boolean intersectOnly)
    {
        if(numIndex == 0)
            return false;

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;
        int offset = 0;
        int i0, i1, i2;

        for(int i = 0; i < numIndex; )
        {
            i0 = indexes[i++] * 3;
            i1 = indexes[i++] * 3;
            i2 = indexes[i++] * 3;

            wkPolygon[0] = coords[i0++];
            wkPolygon[1] = coords[i0++];
            wkPolygon[2] = coords[i0];

            wkPolygon[3] = coords[i1++];
            wkPolygon[4] = coords[i1++];
            wkPolygon[5] = coords[i1];

            wkPolygon[6] = coords[i2++];
            wkPolygon[7] = coords[i2++];
            wkPolygon[8] = coords[i2];

            if(rayPolygonChecked(origin,
                                 direction,
                                 length,
                                 wkPolygon,
                                 3,
                                 wkPoint))
            {
                found = true;
                diffVec.sub(origin, wkPoint);

                this_length = diffVec.lengthSquared();

                if(this_length < shortest_length)
                {
                    shortest_length = this_length;
                    point.set(wkPoint);

                    if(intersectOnly)
                        break;
                }
            }
        }

        return found;
    }

    /**
     * Test an array of indexed quads for intersection. Returns the
     * closest intersection point to the origin of the picking ray. Assumes
     * that the coordinates are ordered as [Xn, Yn, Zn] and are translated
     * into the same coordinate system that the the origin and direction are
     * from.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the triangles
     * @param indexes The list of indexes to use to construct triangles
     * @param numIndex The number of indexes to use from the array
     * @param point The intersection point for returning
     * @param intersectOnly true if we only want to know if we have a
     *    intersection and don't really care which it is
     * @return true if there was an intersection, false if not
     */
    public boolean rayIndexedQuadArray(Point3d origin,
                                       Vector3d direction,
                                       float length,
                                       float[] coords,
                                       int[] indexes,
                                       int numIndex,
                                       Point3d point,
                                       boolean intersectOnly)
    {
        if(numIndex == 0)
            return false;

        // assign the working coords to be big enough for a quadrilateral as
        // that is what we are most likely to see as the biggest item
        if(working2dCoords == null)
            working2dCoords = new float[8];

        double shortest_length = Double.POSITIVE_INFINITY;
        boolean found = false;
        double this_length;
        int offset = 0;
        int i0, i1, i2, i3;

        for(int i = 0; i < numIndex; )
        {
            i0 = indexes[i++] * 3;
            i1 = indexes[i++] * 3;
            i2 = indexes[i++] * 3;
            i3 = indexes[i++] * 3;

            wkPolygon[0] = coords[i0++];
            wkPolygon[1] = coords[i0++];
            wkPolygon[2] = coords[i0];

            wkPolygon[3] = coords[i1++];
            wkPolygon[4] = coords[i1++];
            wkPolygon[5] = coords[i1];

            wkPolygon[6] = coords[i2++];
            wkPolygon[7] = coords[i2++];
            wkPolygon[8] = coords[i2];

            wkPolygon[9] = coords[i3++];
            wkPolygon[10] = coords[i3++];
            wkPolygon[11] = coords[i3];

            if(rayPolygonChecked(origin,
                                 direction,
                                 length,
                                 wkPolygon,
                                 4,
                                 wkPoint))
            {
                found = true;
                diffVec.sub(origin, wkPoint);

                this_length = diffVec.lengthSquared();

                if(this_length < shortest_length)
                {
                    shortest_length = this_length;
                    point.set(wkPoint);

                    if(intersectOnly)
                        break;
                }
            }
        }

        return found;
    }

    //----------------------------------------------------------
    // Lower level methods for individual polygons
    //----------------------------------------------------------

    /**
     * Compute the intersection point of the ray and an infinite cylinder.
     * This is limited to an intersection along one of the axes.
     * <p>
     * The cylAxis value refers to the coefficients for the axis of the
     * cylinder that does not pass through the origin. If we assume the
     * general equation for a cylinder that lies along the X axis is
     * <code>(y - b)^2 + (z - c)^2 = r^2</code> then the values of this
     * parameter represent the coefficients (a, b, c). For the given axis,
     * only the two coefficients for the other axes are used.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param axis Identifier of which axis this is aligned to
     * @param cylAxis The vector coefficients describing the axis of the cylinder
     * @param cylRadius The raduis of the cylinder
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     * @throws IllegalArgumentException The axis ID given is not valid
     */
    public boolean rayCylinder(float[] origin,
                               float[] direction,
                               int axis,
                               float[] cylAxis,
                               float cylRadius,
                               float[] point)
        throws IllegalArgumentException
    {
        return rayCylinder(origin[0], origin[1], origin[2],
                           direction[0], direction[1], direction[2],
                           axis,
                           cylAxis,
                           cylRadius,
                           point);
    }

    /**
     * Compute the intersection point of the ray and an infinite cylinder.
     * This is limited to an intersection along one of the axes.
     * <p>
     * The cylAxis value refers to the coefficients for the axis of the
     * cylinder that does not pass through the origin. If we assume the
     * general equation for a cylinder that lies along the X axis is
     * <code>(y - b)^2 + (z - c)^2 = r^2</code> then the values of this
     * parameter represent the coefficients (a, b, c). For the given axis,
     * only the two coefficients for the other axes are used.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param axis Identifier of which axis this is aligned to
     * @param cylAxis The vector coefficients describing the axis of the cylinder
     * @param cylRadius The raduis of the cylinder
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     * @throws IllegalArgumentException The axis ID given is not valid
     */
    public boolean rayCylinder(Point3d origin,
                               Vector3d direction,
                               int axis,
                               float[] cylAxis,
                               float cylRadius,
                               Point3d point)
        throws IllegalArgumentException
    {
        boolean ret_val = rayCylinder(origin.x, origin.y, origin.z,
                                      direction.x, direction.y, direction.z,
                                      axis,
                                      cylAxis,
                                      cylRadius,
                                      wkPolygon);

        point.x = wkPolygon[0];
        point.y = wkPolygon[1];
        point.z = wkPolygon[2];

        return ret_val;
    }

    /**
     * Internal computation of the intersection point of the ray and a cylinder.
     * Uses raw data types.
     *
     * @param Xo The X coordinate of the origin of the ray
     * @param Yo The Y coordinate of the origin of the ray
     * @param Zo The Z coordinate of the origin of the ray
     * @param Xd The X coordinate of the direction of the ray
     * @param Yd The Y coordinate of the direction of the ray
     * @param Zd The Z coordinate of the direction of the ray
     * @param axis Identifier of which axis this is aligned to
     * @param cylAxis The vector coefficients describing the axis of the cylinder
     * @param cylRadius The radius of the cylinder
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     * @throws IllegalArgumentException The axis ID given is not valid
     */
    private boolean rayCylinder(double Xo, double Yo, double Zo,
                                double Xd, double Yd, double Zd,
                                int axis,
                                float[] cylAxis,
                                float cylRadius,
                                float[] point)
        throws IllegalArgumentException
    {
        // Sanity check for parallel axis / pick vector stuff first
        switch(axis)
        {
            case X_AXIS:
                if(Xd == 1 && Yd == 0 && Zd == 0)
                    return false;
                break;

            case Y_AXIS:
                if(Xd == 0 && Yd == 1 && Zd == 0)
                    return false;
                break;

            case Z_AXIS:
                if(Xd == 0 && Yd == 0 && Zd == 1)
                    return false;
                break;

            default:
                throw new IllegalArgumentException("Invalid Axis ID " + axis);
        }

        // Create a whole heap of local variables to save all the lookups. Makes
        // generating the output easier and less use of switch statements later
        // on. D == direction, O == origin, C == axis of cylinder coefficients
        double D1 = 0;
        double D2 = 0;
        double O1 = 0;
        double O2 = 0;
        double C1 = 0;
        double C2 = 0;

        switch(axis)
        {
            case X_AXIS:
                D1 = Yd;
                D2 = Zd;
                O1 = Yo;
                O2 = Zo;
                C1 = cylAxis[1];
                C2 = cylAxis[2];
                break;

            case Y_AXIS:
                D1 = Xd;
                D2 = Zd;
                O1 = Xo;
                O2 = Zo;
                C1 = cylAxis[0];
                C2 = cylAxis[2];
                break;

            case Z_AXIS:
                D1 = Xd;
                D2 = Yd;
                O1 = Xo;
                O2 = Yo;
                C1 = cylAxis[0];
                C2 = cylAxis[1];
        }

        // compute A, B, C
        double a = D1 * D1 + D2 * D2;
        double b = 2 * (D1 * (O1 - C1) + D2 * (O2 - C2));
        double c = (O1 - C1) * (O1 - C1) + (O2 - C2) * (O2 - C2)
                   - cylRadius * cylRadius;

        // compute discriminant
        double disc = b * b - 4 * a * c;

        if(disc < 0)
            return false;
        if(disc == 0)
        {
            // tangent Intersection point is u = -b/2a
            double u = -b / a * 0.5;
            point[0] = (float)(Xo + u * Xd);
            point[1] = (float)(Yo + u * Yd);
            point[2] = (float)(Zo + u * Zd);
        }
        else
        {
            // Closest interection point with. If the t0 (subtraction)
            // is greater than zero then that's the intersection point,
            // if not then compute t1 which is the addition.
            double u = (-b - Math.sqrt(disc)) / (a * 2);

            if(u < 0)
                u = (-b + Math.sqrt(disc)) / (a * 2);

            point[0] = (float)(Xo + u * Xd);
            point[1] = (float)(Yo + u * Yd);
            point[2] = (float)(Zo + u * Zd);
        }

        return true;
    }

    /**
     * Compute the intersection point of the ray and a sphere.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param sphereCenter The coordinates of the center of the sphere
     * @param sphereRadius The raduis of the sphere
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    public boolean raySphere(float[] origin,
                             float[] direction,
                             float[] sphereCenter,
                             float sphereRadius,
                             float[] point)
    {
        return raySphere(origin[0], origin[1], origin[2],
                         direction[0], direction[1], direction[2],
                         sphereCenter,
                         sphereRadius,
                         point);
    }

    /**
     * Compute the intersection point of the ray and a sphere.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param sphereCenter The coordinates of the center of the sphere
     * @param sphereRadius The raduis of the sphere
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    public boolean raySphere(Point3d origin,
                             Vector3d direction,
                             float[] sphereCenter,
                             float sphereRadius,
                             Point3d point)
    {
        boolean ret_val = raySphere(origin.x, origin.y, origin.z,
                                    direction.x, direction.y, direction.z,
                                    sphereCenter,
                                    sphereRadius,
                                    wkPolygon);

        point.x = wkPolygon[0];
        point.y = wkPolygon[1];
        point.z = wkPolygon[2];

        return ret_val;
    }

    /**
     * Internal computation of the intersection point of the ray and a sphere.
     * Uses raw data types.
     *
     * @param Xo The X coordinate of the origin of the ray
     * @param Yo The Y coordinate of the origin of the ray
     * @param Zo The Z coordinate of the origin of the ray
     * @param Xd The X coordinate of the direction of the ray
     * @param Yd The Y coordinate of the direction of the ray
     * @param Zd The Z coordinate of the direction of the ray
     * @param sphereCenter The coordinates of the center of the sphere
     * @param sphereRadius The raduis of the sphere
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    private boolean raySphere(double Xo, double Yo, double Zo,
                              double Xd, double Yd, double Zd,
                              float[] sphereCenter,
                              float sphereRadius,
                              float[] point)
   {
        double Xc = sphereCenter[0];
        double Yc = sphereCenter[1];
        double Zc = sphereCenter[2];

        // compute A, B, C
        //C =
        double a = Xd * Xd + Yd * Yd + Zd * Zd;
        double b = 2 * (Xd * (Xo - Xc) + Yd * (Yo - Yc) + Zd * (Zo - Zc));
        double c = (Xo - Xc) * (Xo - Xc) + (Yo - Yc) * (Yo - Yc) +
                   (Zo - Zc) * (Zo - Zc) - sphereRadius * sphereRadius;

        // compute discriminant
        double disc = b * b - 4 * a * c;

        if(disc < 0)
            return false;
        if(disc == 0)
        {
            // tangent Intersection point is u = -b/2a
            double u = -b / a * 0.5;
            point[0] = (float)(Xo + u * Xd);
            point[1] = (float)(Yo + u * Yd);
            point[2] = (float)(Zo + u * Zd);
        }
        else
        {
            // Closest interection point with. If the t0 (subtraction)
            // is greater than zero then that's the intersection point,
            // if not then compute t1 which is the addition.
            double u = (-b - Math.sqrt(disc)) / (a * 2);

            if(u < 0)
                u = (-b + Math.sqrt(disc)) / (a * 2);

            point[0] = (float)(Xo + u * Xd);
            point[1] = (float)(Yo + u * Yd);
            point[2] = (float)(Zo + u * Zd);
        }

        return true;
    }

    /**
     * Compute the intersection point of the ray and a plane. Assumes that the
     * plane equation defines a unit normal in the coefficients a,b,c. If not,
     * weird things happen.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param plane The coefficients for the plane equation (ax + by + cz + d = 0)
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    public boolean rayPlane(float[] origin,
                             float[] direction,
                             float[] plane,
                             float[] point)
    {
        return rayPlane(origin[0], origin[1], origin[2],
                        direction[0], direction[1], direction[2],
                        plane,
                        point);
    }

    /**
     * Compute the intersection point of the ray and a plane. Assumes that the
     * plane equation defines a unit normal in the coefficients a,b,c. If not,
     * weird things happen.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param plane The coefficients for the plane equation (ax + by + cz + d = 0)
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    public boolean rayPlane(Point3d origin,
                            Vector3d direction,
                            float[] plane,
                            Point3d point)
    {
        boolean ret_val = rayPlane(origin.x, origin.y, origin.z,
                                   direction.x, direction.y, direction.z,
                                   plane,
                                   wkPolygon);

        point.x = wkPolygon[0];
        point.y = wkPolygon[1];
        point.z = wkPolygon[2];

        return ret_val;
    }


    /**
     * Internal computation of the intersection point of the ray and a plane.
     * Uses raw data types.
     *
     * @param Xo The X coordinate of the origin of the ray
     * @param Yo The Y coordinate of the origin of the ray
     * @param Zo The Z coordinate of the origin of the ray
     * @param Xd The X coordinate of the direction of the ray
     * @param Yd The Y coordinate of the direction of the ray
     * @param Zd The Z coordinate of the direction of the ray
     * @param plane The coefficients for the plane equation (ax + by + cz + d = 0)
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    private boolean rayPlane(double Xo, double Yo, double Zo,
                             double Xd, double Yd, double Zd,
                             float[] plane,
                             float[] point)
   {
        // Dot product between the ray and the normal to the plane
        double angle = Xd * plane[0] + Yd * plane[1] + Zd * plane[2];

        if(angle == 0)
            return false;

        // t = (Pn . Origin + D) / (Pn . Direction)
        // The divisor is the angle calc already calculated
        double Vo = -((plane[0] * Xo + plane[1] * Yo + plane[2] * Zo) + plane[3]);
        double t = Vo / angle;

        if(t < 0)
            return false;

        point[0] = (float)(Xo + Xd * t);
        point[1] = (float)(Yo + Yd * t);
        point[2] = (float)(Zo + Zd * t);

        return true;
    }

    /**
     * Test to see if the polygon intersects with the given ray. The
     * coordinates are ordered as [Xn, Yn, Zn]. The algorithm assumes that
     * the points are co-planar. If they are not, the results may not be
     * accurate. The normal is calculated based on the first 3 points of the
     * polygon. We don't do any testing for less than 3 points.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the polygon
     * @param numCoords The number of coordinates to use from the array
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    public boolean rayPolygon(Point3d origin,
                              Vector3d direction,
                              float length,
                              float[] coords,
                              int numCoords,
                              Point3d point)
    {
        if(coords.length < numCoords * 2)
            throw new IllegalArgumentException("coords too small for numCoords");

        if((working2dCoords == null) ||
           (working2dCoords.length < numCoords * 2))
            working2dCoords = new float[numCoords * 2];

        return rayPolygonChecked(origin,
                                 direction,
                                 length,
                                 coords,
                                 numCoords,
                                 point);
    }

    /**
     * Private version of the ray - Polygon intersection test that does not
     * do any bounds checking on arrays and assumes everything is correct.
     * Allows fast calls to this method for internal use as well as more
     * expensive calls with checks for the public interfaces.
     * <p>
     * This method does not use wkPoint.
     *
     * @param origin The origin of the ray
     * @param direction The direction of the ray
     * @param length An optional length for to make the ray a segment. If
     *   the value is zero, it is ignored
     * @param coords The coordinates of the polygon
     * @param numCoords The number of coordinates to use from the array
     * @param point The intersection point for returning
     * @return true if there was an intersection, false if not
     */
    private boolean rayPolygonChecked(Point3d origin,
                                      Vector3d direction,
                                      float length,
                                      float[] coords,
                                      int numCoords,
                                      Point3d point)
    {
        int i, j;

        v0.x = coords[3] - coords[0];
        v0.y = coords[4] - coords[1];
        v0.z = coords[5] - coords[2];

        v1.x = coords[6] - coords[3];
        v1.y = coords[7] - coords[4];
        v1.z = coords[8] - coords[5];

        normal.cross(v0, v1);

        // degenerate polygon?
        if(normal.lengthSquared() == 0)
            return false;

        double n_dot_dir = normal.dot(direction);

        // ray and plane parallel?
        if(n_dot_dir == 0)
            return false;

        wkVec.x = coords[0];
        wkVec.y = coords[1];
        wkVec.z = coords[2];
        double d = normal.dot(wkVec);

        wkVec.set(origin);
        double n_dot_o = normal.dot(wkVec);

        // t = (d - N.O) / N.D
        double t = (d - n_dot_o) / n_dot_dir;

        // intersection before the origin
        if(t < 0)
            return false;

        // So we have an intersection with the plane of the polygon and the
        // segment/ray. Using the winding rule to see if inside or outside
        // First store the exact intersection point anyway, regardless of
        // whether this is an intersection or not.
        point.x = origin.x + direction.x * t;
        point.y = origin.y + direction.y * t;
        point.z = origin.z + direction.z * t;

        // Intersection point after the end of the segment?
        if((length != 0) && (origin.distance(point) > length))
            return false;

        // bounds check

        // find the dominant axis to resolve to a 2 axis system
        double abs_nrm_x = (normal.x >= 0) ? normal.x : -normal.x;
        double abs_nrm_y = (normal.y >= 0) ? normal.y : -normal.y;
        double abs_nrm_z = (normal.z >= 0) ? normal.z : -normal.z;

        int dom_axis;

        if(abs_nrm_x > abs_nrm_y)
            dom_axis = 0;
        else
            dom_axis = 1;

        if(dom_axis == 0)
        {
            if(abs_nrm_x < abs_nrm_z)
                dom_axis = 2;
        }
        else if(abs_nrm_y < abs_nrm_z)
        {
            dom_axis = 2;
        }

        // Map all the coordinates to the 2D plane. The u and v coordinates
        // are interleaved as u == even indicies and v = odd indicies

        // Steps 1 & 2 combined
        // 1. For NV vertices [Xn Yn Zn] where n = 0..Nv-1, project polygon
        // vertices [Xn Yn Zn] onto dominant coordinate plane (Un Vn).
        // 2. Translate (U, V) polygon so intersection point is origin from
        // (Un', Vn').
        j = 2 * numCoords - 1;

        switch(dom_axis)
        {
            case 0:
                for(i = numCoords; --i >= 0; )
                {
                    working2dCoords[j--] = coords[i * 3 + 2] - (float)point.z;
                    working2dCoords[j--] = coords[i * 3 + 1] - (float)point.y;
                }
                break;

            case 1:
                for(i = numCoords; --i >= 0; )
                {
                    working2dCoords[j--] = coords[i * 3 + 2] - (float)point.z;
                    working2dCoords[j--] = coords[i * 3]     - (float)point.x;
                }
                break;

            case 2:
                for(i = numCoords; --i >= 0; )
                {
                    working2dCoords[j--] = coords[i * 3 + 1] - (float)point.y;
                    working2dCoords[j--] = coords[i * 3]     - (float)point.x;
                }
                break;
        }

        int sh;  // current sign holder
        int nsh; // next sign holder
        float dist;
        int crossings = 0;

        // Step 4.
        // Set sign holder as f(V' o) ( V' value of 1st vertex of 1st edge)
        if(working2dCoords[1] < 0.0)
            sh = -1;
        else
            sh = 1;

        for(i = 0; i < numCoords; i++)
        {
            // Step 5.
            // For each edge of polygon (Ua' V a') -> (Ub', Vb') where
            // a = 0..Nv-1 and b = (a + 1) mod Nv

            // b = (a + 1) mod Nv
            j = (i + 1) % numCoords;

            int i_u = i * 2;           // index of Ua'
            int j_u = j * 2;           // index of Ub'
            int i_v = i * 2 + 1;       // index of Va'
            int j_v = j * 2 + 1;       // index of Vb'

            // Set next sign holder (Nsh) as f(Vb')
            // Nsh = -1 if Vb' < 0
            // Nsh = +1 if Vb' >= 0
            if(working2dCoords[j_v] < 0.0)
                nsh = -1;
            else
                nsh = 1;

            // If Sh <> NSH then if = then edge doesn't cross +U axis so no
            // ray intersection and ignore

            if(sh != nsh)
            {
                // if Ua' > 0 and Ub' > 0 then edge crosses + U' so Nc = Nc + 1
                if((working2dCoords[i_u] > 0.0) && (working2dCoords[j_u] > 0.0))
                {
                    crossings++;
                }
                else if ((working2dCoords[i_u] > 0.0) ||
                         (working2dCoords[j_u] > 0.0))
                {
                    // if either Ua' or U b' > 0 then line might cross +U, so
                    // compute intersection of edge with U' axis
                    dist = working2dCoords[i_u] -
                           (working2dCoords[i_v] *
                            (working2dCoords[j_u] - working2dCoords[i_u])) /
                           (working2dCoords[j_v] - working2dCoords[i_v]);

                    // if intersection point is > 0 then must cross,
                    // so Nc = Nc + 1
                    if(dist > 0)
                        crossings++;
                }

                // Set SH = Nsh and process the next edge
                sh = nsh;
            }
        }

        // Step 6. If Nc odd, point inside else point outside.
        // Note that we have already stored the intersection point way back up
        // the start.
        return ((crossings % 2) == 1);
    }

    /**
     * Convenience method to transform the picking coordinates to the local
     * coordinates of the geometry. Takes the coordinates and stores them in
     * the picking variables used internally
     */
    private void transformPicks(Matrix4d vw, Point3d start, Vector3d dir)
    {
        vw.transform(start, pickStart);
        vw.transform(dir, pickDir);
    }
}