using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VisiMath.Geometry;

namespace VisiMath.Collisions
{
    [Flags]
    public enum SpatialDirection
    {
        North = 0x02,
        South = 0x04,
        East = 0x08, 
        West = 0x10, 
        Up = 0x12, 
        Down = 0x14        
    }
    
    /// <summary>
    /// Type of the 3d spatial index
    /// </summary>
    public enum SpatialIndexType
    {
        /// <summary>
        /// Invalid spatial index type
        /// </summary>
        Invalid = 0,
        /// <summary>
        /// Just position in Point3D
        /// </summary>
        Position,
        /// <summary>
        /// Simple line segment
        /// </summary>
        Line,
        /// <summary>
        /// Sphere
        /// </summary>
        Sphere,
        /// <summary>
        /// AA Bounding box
        /// </summary>
        Box,
        /// <summary>
        /// Generic convex object (IConvexObject)
        /// Note this is helper function for quick selection, using hulls as base indices is not reccomended (with the exception of generic cell / portal systems)
        /// </summary>
        ConvexObject,
        /// <summary>
        /// All encompassing index
        /// </summary>
        Infinity
    }

    /// <summary>
    /// Spatial index interface
    /// </summary>
    public interface ISpatialIndex : IConvexObject
    {
        /// <summary>
        /// Returns spatial index as box
        /// </summary>
        AABox AsBox { get; }
        /// <summary>
        /// Returns spatial index as sphere
        /// </summary>
        Sphere AsSphere { get; }
        /// <summary>
        /// returns spatial index as point (center)
        /// </summary>
        Vector3 AsPoint { get; }

        /// <summary>
        /// return true if index contains (intersects) another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(ref ISpatialIndex index);
        
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(Vector3 index);
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(ref Vector3 index);
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(Point3D index);

        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        bool ContainsLine(Vector3 line1, Vector3 line2);
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if intersection exists</returns>
        bool ContainsLine(ref Vector3 line1, ref Vector3 line2);
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="line">line segment</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(LineSegment3D line);

        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(Vector3 center, float radius);
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(ref Vector3 center, float radius);
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(Sphere sphere);

        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        bool ContainsBox(Vector3 min, Vector3 max);
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if intersection exists</returns>
        bool ContainsBox(ref Vector3 min, ref Vector3 max);
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(AABox box);

        /// <summary>
        /// Return true if index contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if intersection exists</returns>
        bool Contains(IConvexObject convex);


        /// <summary>
        /// return true if index fully contains another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(ref ISpatialIndex index);

        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFullyLine(Vector3 line1, Vector3 line2);
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line1">Start of the line</param>
        /// <param name="line2">End of the line</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFullyLine(ref Vector3 line1, ref Vector3 line2);
        /// <summary>
        /// Return true if index fully contains line
        /// </summary>
        /// <param name="line">Line segment</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(LineSegment3D line);

        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(Vector3 center, float radius);
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="center">sphere center</param>
        /// <param name="radius">sphere radius</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(ref Vector3 center, float radius);
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(Sphere sphere);

        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFullyBox(Vector3 min, Vector3 max);
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="max">Maximal corner</param>
        /// <param name="min">Minimal corner</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFullyBox(ref Vector3 min, ref Vector3 max);
        /// <summary>
        /// Return true if index fully contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if index fully contains other object</returns>
        bool ContainsFully(AABox box);

        /// <summary>
        /// Return true if index FULLY contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if object is fully contained in index</returns>
        bool ContainsFully(IConvexObject convex);        
    }



    #region obsoleteversion
/*
    /// <summary>
    /// Type of the 3d spatial index
    /// </summary>
    public enum SpatialIndexType
    {
        /// <summary>
        /// Invalid spatial index type
        /// </summary>
        Invalid = 0,
        /// <summary>
        /// Just position 
        /// </summary>
        Position,
        /// <summary>
        /// Simple line, two values per item
        /// </summary>
        Line,
        /// <summary>
        /// Sphere saved in one value, position x y z and radius in w
        /// </summary>
        Sphere,
        /// <summary>
        /// Bounding box saved as minimal and maximal corner (2 vectors). 
        /// </summary>
        Box,
        /// <summary>
        /// Generic convex object, array must hold triplets of positions for each vertex.
        /// Note this is helper function for quick selection, using hulls as base indices is not reccomended (with the exception of generic cell / portal systems)
        /// </summary>
        ConvexObject,
        /// <summary>
        /// All encompassing index
        /// </summary>
        Infinity

        //(or lines if more values), 
        //(or point cloud if more values are saved in SpatialIndex.Values)
        //SphereHierarchy
        // In this case first value is ALLWAYS checked first, it should encompass all others
        //BoxHierarchy
        //If more values are saved, other values specify other boxes, which are nested in the first
        /// bounding box
    }
*/

    /*
    //obsolete version
    
    /// <summary>
    /// Spatial index structure
    /// </summary>
    public struct SpatialIndex : IConvexObject
    {
        /// <summary>
        /// Values of index, saved in vector4 (w used for spheres as radius, and to padd size to 16 bytes per item
        /// Note, c# would use padding annyway
        /// </summary>
        public Vector4[] Values;
        /// <summary>
        /// Type of the spatial index, specifies type of data in values
        /// </summary>
        public SpatialIndexType ValueType;

        #region quick type check

        /// <summary>
        /// Is index simple point?
        /// </summary>
        public bool IsSimplePoint
        {
            get
            {
                return (ValueType == SpatialIndexType.Position) && (Values.Length == 1);
            }
        }

        /// <summary>
        /// Is index one simple box?
        /// </summary>
        public bool IsSimpleBox
        {
            get
            {
                return (ValueType == SpatialIndexType.Box) && (Values.Length == 2);
            }
        }

        /// <summary>
        /// Is index one simple sphere?
        /// </summary>
        public bool IsSimpleSphere
        {
            get
            {
                return (ValueType == SpatialIndexType.Sphere) && (Values.Length == 1);
            }
        }

        #endregion

        #region Constructors


        /// <summary>
        /// Point type
        /// </summary>
        /// <param name="pos">Position vector</param>
        public SpatialIndex(Vector3 pos) : this(ref pos) { }

        /// <summary>
        /// Point type
        /// </summary>
        /// <param name="pos">Position vector</param>
        public SpatialIndex(ref Vector3 pos)
        {
            Values = new Vector4[1];
            Values[0].XYZ = pos;
            ValueType = SpatialIndexType.Position;
        }

        /// <summary>
        /// Box or line type
        /// </summary>
        /// <param name="vec1">Minimal corner or line start</param>
        /// <param name="vec2">Maximal corner or line end</param>
        /// <param name="st">Type (should be either box or line!)</param>
        public SpatialIndex(ref Vector3 vec1, ref Vector3 vec2, SpatialIndexType st = SpatialIndexType.Box)
        {
            ValueType = st;
            Values = new Vector4[2];
            Values[0] = new Vector4(ref vec1);
            Values[1] = new Vector4(ref vec2);

            //sanity check

            if (st == SpatialIndexType.Sphere)
                throw new ArgumentException("Can't initialize spherical spatial index with two vectors!");
        }

        /// <summary>
        /// Spherical constructor
        /// </summary>
        /// <param name="vec">Vector4 holding position and radius of the sphere</param>
        public SpatialIndex(ref Vector4 vec)
        {
            Values = new Vector4[1];
            Values[0] = vec;
            ValueType = SpatialIndexType.Sphere;
        }

        /// <summary>
        /// Spherical constructor
        /// </summary>
        /// <param name="vec">Vector4 holding position and radius of the sphere</param>
        public SpatialIndex(Vector4 vec) : this(ref vec) { }

        /// <summary>
        /// Spherical constructor
        /// </summary>
        /// <param name="vec1">Vector holding position</param>
        /// <param name="rad">radius of the sphere</param>
        public SpatialIndex(ref Vector3 vec1, float rad)
            : this(new Vector4(vec1.X, vec1.Y, vec1.Z, rad)) { }

        /// <summary>
        /// Creates spatial index from collection of vectors3
        /// </summary>
        /// <param name="array">source collection</param>
        /// <param name="type">required type</param>
        public SpatialIndex(IEnumerable<Vector3> array, SpatialIndexType type)
        {
            List<Vector4> list = new List<Vector4>();
            foreach (Vector3 vec in array)
            {
                list.Add(new Vector4(vec.X, vec.Y, vec.Z, 0));
            }
            Values = list.ToArray();
            ValueType = type;
            //sanity check
            if (type == SpatialIndexType.Sphere)
                throw new ArgumentException("Can't initialize spherical spatial index from just vector3!");
        }

        /// <summary>
        /// Creates spatial index from user values (warning, no sanity check)
        /// </summary>
        /// <param name="values">Values array</param>
        /// <param name="type">Type</param>
        public SpatialIndex(Vector4[] values, SpatialIndexType type)
        {
            Values = values;
            ValueType = type;
        }

        #endregion

        #region Functions


        public bool ContainsPoint(ref SpatialIndex index, bool fully)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Position:
                    return (Values[0] == index.Values[0]);
                case SpatialIndexType.Line:
                    return PointOnLine(ref index, ref this);
                case SpatialIndexType.Sphere:
                    float dst = Values[0].XYZ.DistanceSquared(index.Values[0].XYZ);
                    if (dst <= Values[0].W * Values[0].W)
                        return true;
                    return false;
                case SpatialIndexType.Box:

                    if ((index.Values[0].X < Values[0].X) ||
                        (index.Values[0].X > Values[1].X) ||
                        (index.Values[0].Y < Values[0].Y) ||
                        (index.Values[0].Y > Values[1].Y) ||
                        (index.Values[0].Z < Values[0].Z) ||
                        (index.Values[0].Z > Values[1].Z))
                        return false;
                    return true;
                case SpatialIndexType.ConvexObject:
                    return PointInConvex(ref index, ref this);
            }
            return false;
        }

        public bool ContainsSphere(ref SpatialIndex index, bool fully)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Position:
                    if (fully)
                        return false;
                    if (Values[0].XYZ.DistanceSquared(index.Values[0].XYZ) < (index.Values[0].W * index.Values[0].W))
                        return true;
                    return false;
                case SpatialIndexType.Line:
                    if (fully)
                        return false;
                    return LineSphere(ref this, ref index);
                case SpatialIndexType.Sphere:
                    if (fully && (Values[0].W < index.Values[0].W))
                        return false;
                    float dst = Values[0].XYZ.DistanceSquared(index.Values[0].XYZ);
                    if (!fully && (dst < Values[0].W * Values[0].W - index.Values[0].W * index.Values[0].W))
                        return true;
                    if (dst <= (Values[0].W - index.Values[0].W) * (Values[0].W - index.Values[0].W))
                        return true;
                    return false;
                case SpatialIndexType.Box:
                    if (fully)
                    {
                        if ((index.Values[0].X - index.Values[0].W < Values[0].X) ||
                            (index.Values[0].X + index.Values[0].W > Values[1].X) ||
                            (index.Values[0].Y - index.Values[0].W < Values[0].Y) ||
                            (index.Values[0].Y + index.Values[0].W > Values[1].Y) ||
                            (index.Values[0].Z - index.Values[0].W < Values[0].Z) ||
                            (index.Values[0].Z + index.Values[0].W > Values[1].Z))
                            return false;
                        return true;
                    }
                    float sqdist = 0;
                    for (int i = 0; i < 3; i++)
                    {
                        float v = index.Values[0][i];
                        if (v < Values[0][i])
                            sqdist += (Values[0][i] - v) * (Values[0][i] - v);
                        if (v > Values[1][i])
                            sqdist += (v - Values[1][i]) * (v - Values[1][i]);
                    }
                    if (sqdist < index.Values[0].W * index.Values[0].W)
                        return true;
                    return false;
                case SpatialIndexType.ConvexObject:
                    if (fully)
                        return SphereInConvex(ref index, ref this);
                    return SphereConvex(ref index, ref this);
            }
            return false;
        }

        public bool ContainsBox(ref SpatialIndex index, bool fully)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Position:
                    if (fully)
                        return false;
                    for (int i = 0; i < 3; i++)
                    {
                        if ((Values[0][i] < index.Values[0][i]) || (Values[0][i] > index.Values[1][i]))
                            return false;
                    }
                    return true;
                case SpatialIndexType.Line:
                    if (fully)
                        return false;
                    return LineBox(ref this, ref index);
                case SpatialIndexType.Sphere:
                    if (fully && (Values[0].W < index.Values[0].W))
                        return false;
                    float dst = Values[0].XYZ.DistanceSquared(index.Values[0].XYZ);
                    if (!fully && (dst < Values[0].W * Values[0].W - index.Values[0].W * index.Values[0].W))
                        return true;
                    if (dst <= (Values[0].W - index.Values[0].W) * (Values[0].W - index.Values[0].W))
                        return true;
                    return false;
                case SpatialIndexType.Box:
                    if (fully)
                    {
                        for (int i = 0; i < 3; i++)
                        {
                            if ((index.Values[0][i] < Values[0][i]) || (index.Values[1][i] > Values[1][i]))
                                return false;
                        }
                        return true;
                    }
                    else return BoxBox(ref this, ref index);
                case SpatialIndexType.ConvexObject:
                    if (fully)
                        return BoxInConvex(ref index, ref this);
                    return BoxConvex(ref index, ref this);
            }
            return false;
        }

        public bool ContainsLine(ref SpatialIndex index, bool fully)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Position:
                    if (fully)
                        return false;
                    return PointOnLine(ref this, ref index);
                case SpatialIndexType.Line:
                    if (MathTools.DistanceTwoLineSegments(Values[0].XYZ, Values[1].XYZ, index.Values[0].XYZ, index.Values[1].XYZ) == 0)
                        return true;
                    return false;
                case SpatialIndexType.Sphere:
                    if (fully)
                        return false;
                    return LineSphere(ref index, ref this);
                case SpatialIndexType.Box:
                    if (fully)
                        return false;
                    return LineBox(ref index, ref this);
                case SpatialIndexType.ConvexObject:
                    if (fully)
                        return false;
                    return LineConvex(ref index, ref this);
            }
            return false;
        }

        public bool ContainsConvex(ref SpatialIndex index, bool fully)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Position:
                    if (fully)
                        return false;
                    return PointInConvex(ref this, ref index);
                case SpatialIndexType.Line:
                    if (fully)
                        return false;
                    return LineConvex(ref this, ref index);
                case SpatialIndexType.Sphere:
                    if (fully)
                    {
                        float sqrad = Values[0].W;
                        Vector3 cen = Values[0].XYZ;
                        //calculate distance of all hull points from sphere center
                        for (int i = 0; i < index.Values.Length; i++)
                        {
                            if (index.Values[i].XYZ.DistanceSquared(ref cen) > sqrad)
                                return false;
                        }
                        return true;
                    }
                    return SphereConvex(ref this, ref index);
                case SpatialIndexType.Box:
                    if (fully)
                    {
                        for (int i = 0; i < index.Values.Length; i++)
                        {
                            for (int j = 0; j < 3; i++)
                            {
                                if ((index.Values[i][j] < Values[0][j]) || (index.Values[i][j] > Values[1][j]))
                                    return false;
                            }
                        }
                        return true;
                    }
                    return BoxConvex(ref this, ref index);
                case SpatialIndexType.ConvexObject:
                    if (fully)
                    {
                        for (int i = 0; i < index.Values.Length; i++)
                        {
                            if (!PointInConvex(index.Values[i].XYZ, ref this))
                                return false;
                        }
                        return true;
                    }
                    return IntersectionTest(ref this, ref index);
            }
            return false;
        }



        /// <summary>
        /// Returns true if spatial portion contains spatial index fully
        /// </summary>
        /// <param name="index">Spatial index</param>
        public bool ContainsFully(ref SpatialIndex index)
        {
            if ((ValueType == SpatialIndexType.Invalid) || (index.ValueType == SpatialIndexType.Invalid))
                return false;
            if ((ValueType == SpatialIndexType.Infinity) || (index.ValueType == SpatialIndexType.Infinity))
                return true;

            switch (index.ValueType)
            {
                case SpatialIndexType.Position:
                    return ContainsPoint(ref index, true);
                case SpatialIndexType.Sphere:
                    return ContainsSphere(ref index, true);
                case SpatialIndexType.Box:
                    return ContainsBox(ref index, true);
                case SpatialIndexType.Line:
                    return ContainsLine(ref index, true);
                case SpatialIndexType.ConvexObject:
                    return ContainsConvex(ref index, true);
            }
            return false;
        }

        /// <summary>
        /// Returns true if spatial portion contains spatial index at least partially
        /// </summary>
        /// <param name="index">Spatial index</param>
        public bool Contains(ref SpatialIndex index)
        {
            if ((ValueType == SpatialIndexType.Invalid) || (index.ValueType == SpatialIndexType.Invalid))
                return false;
            if (ValueType == SpatialIndexType.Infinity)
                return true;
            if (index.ValueType == SpatialIndexType.Infinity)
                return false;

            switch (index.ValueType)
            {
                case SpatialIndexType.Position:
                    return ContainsPoint(ref index, false);
                case SpatialIndexType.Sphere:
                    return ContainsSphere(ref index, false);
                case SpatialIndexType.Box:
                    return ContainsBox(ref index, false);
                case SpatialIndexType.Line:
                    return ContainsLine(ref index, false);
                case SpatialIndexType.ConvexObject:
                    return ContainsConvex(ref index, false);
            }
            return false;
        }


        #endregion

        /// <summary>
        /// Check if point spatial index lies on line spatial index
        /// </summary>
        /// <param name="point">point index</param>
        /// <param name="line">line index</param>
        /// <returns>true if lies on the line</returns>
        public static bool PointOnLine(ref SpatialIndex point, ref SpatialIndex line)
        {
            return MathTools.PointOnLineSegment(line.Values[0].XYZ, line.Values[1].XYZ, point.Values[0].XYZ);
        }

        /// <summary>
        /// Checks if point lies inside convex hull
        /// </summary>
        /// <param name="point">point</param>
        /// <param name="convex">Convex hull spatial index</param>
        /// <returns>True if inside</returns>
        public static bool PointInConvex(Vector3 point, ref SpatialIndex convex)
        {
            return CollisionSolver.Containment.PointInHull(point, ref convex);
        }

        public static bool PointInConvex(ref SpatialIndex point, ref SpatialIndex convex)
        {
            return PointInConvex(point.Values[0].XYZ, ref convex);
        }

        public static bool SphereInConvex(ref SpatialIndex sphere, ref SpatialIndex convex)
        {
            return CollisionSolver.Containment.SphereInHull(sphere.Values[0].XYZ, sphere.Values[0].W, ref convex);
        }

        public static bool BoxInConvex(ref SpatialIndex box, ref SpatialIndex convex)
        {
            Vector3 min = box.Values[0].XYZ;

            if (!PointInConvex(min, ref convex))
                return false;

            Vector3 max = box.Values[1].XYZ;
            if (!PointInConvex(max, ref convex))
                return false;

            if (!PointInConvex(new Vector3(min.X, min.Y, max.Z), ref convex))
                return false;

            if (!PointInConvex(new Vector3(min.X, max.Y, min.Z), ref convex))
                return false;

            if (!PointInConvex(new Vector3(max.X, min.Y, min.Z), ref convex))
                return false;

            if (!PointInConvex(new Vector3(min.X, max.Y, max.Z), ref convex))
                return false;

            if (!PointInConvex(new Vector3(max.X, min.Y, max.Z), ref convex))
                return false;

            if (!PointInConvex(new Vector3(max.X, max.Y, min.Z), ref convex))
                return false;

            return true;
        }

        public static bool LineSphere(ref SpatialIndex line, ref SpatialIndex sphere)
        {
            if (MathTools.DistanceLineSegmentSphere(line.Values[0].XYZ, line.Values[1].XYZ, sphere.Values[0].XYZ, sphere.Values[0].W) <= 0)
                return true;
            return false;
        }

        public static bool LineBox(ref SpatialIndex line, ref SpatialIndex box)
        {
            Vector3 orig = line.Values[0].XYZ;
            Vector3 dir = line.Values[1].XYZ - orig;

            float length = dir.Length;
            dir.Divide(length);

            Vector3 min = box.Values[0].XYZ;
            Vector3 max = box.Values[1].XYZ;
            float dist;
            Vector3 point;

            if (!MathTools.IntersectionRayAABox(ref orig, ref dir, ref min, ref max, out dist, out point))
                return false;
            if ((dist < 0) || (dist > length))
                return false;
            return true;
        }

        public static bool LineConvex(ref SpatialIndex line, ref SpatialIndex convex)
        {
            return IntersectionTest(ref line, ref convex);
        }

        public static bool SphereConvex(ref SpatialIndex sphere, ref SpatialIndex convex)
        {
            return IntersectionTest(ref sphere, ref convex);
        }

        /// <summary>
        /// box / box intersection
        /// </summary>
        /// <param name="box1">Box1</param>
        /// <param name="box2">Box2</param>
        /// <returns>True if intersection</returns>
        public static bool BoxBox(ref SpatialIndex box1, ref SpatialIndex box2)
        {
            for (int i = 0; i < 3; i++)
            {
                if (box1.Values[0][i] > box2.Values[1][i])
                    return false;
                if (box1.Values[1][i] < box2.Values[0][i])
                    return false;
            }
            return true;
        }

        public static bool BoxConvex(ref SpatialIndex box, ref SpatialIndex convex)
        {
            return IntersectionTest(ref box, ref convex);
        }


        /// <summary>
        /// Generic convex test
        /// </summary>
        /// <param name="ind1">index1</param>
        /// <param name="ind2">index2</param>
        /// <returns>True if indices intersect</returns>
        public static bool IntersectionTest(ref SpatialIndex ind1, ref SpatialIndex ind2)
        {
            return CollisionSolver.QuickTest.SpatialIndices(ref ind1, ref ind2);
        }

        #region Conversion

        /// <summary>
        /// Converts spatial index to box (performs quick bounding box calculation if needed
        /// </summary>
        /// <param name="index"></param>
        public SpatialIndex ToBox()
        {
            if (IsSimpleBox)
            {
                return this;
            }

            Vector3 min = new Vector3(), max = new Vector3();

            for (int i = 0; i < Values.Length; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    float val = Values[i][j];
                    if (ValueType == SpatialIndexType.Sphere)
                        val -= Values[i].W;
                    min[j] = MathTools.Min(val, min[j]);
                    if (ValueType == SpatialIndexType.Sphere)
                        val += 2 * Values[i].W;
                    max[j] = MathTools.Max(val, max[j]);
                }
            }

            //if ((index.Values == null) || (index.Values.Length != 2)) //can't be used, c# doesn't like this

            return new SpatialIndex(ref min, ref max);
        }

        /// <summary>
        /// Converts to sphere index (note, slow and imprecisse conversion unless already sphere)
        /// </summary>
        /// <param name="index"></param>
        public SpatialIndex ToSphere()
        {
            if (IsSimpleSphere)
                return this;
            if (IsSimplePoint)
                return CreateSphere(Values[0].XYZ, 0);

            SpatialIndex box = ToBox();

            //note, optimize this at least a bit
            Vector3 cen = box.Values[0].XYZ + box.Values[1].XYZ;
            cen.Mul(0.5f);
            float rad = (box.Values[1].XYZ - box.Values[0].XYZ).Length;
            return new SpatialIndex(ref cen, rad);
        }

        /// <summary>
        /// Converts spatial index to point (returns center of this index)
        /// </summary>
        /// <param name="index"></param>
        public SpatialIndex ToPoint()
        {
            if (IsSimplePoint) return this;

            if (IsSimpleSphere) return new SpatialIndex(this.Values[0].XYZ);

            if (IsSimpleBox)
            {
                Vector3 vec = Values[0].XYZ + Values[1].XYZ;
                vec.Mul(0.5f);
                return new SpatialIndex(ref vec);
            }

            Vector3 cen = new Vector3();
            for (int i = 0; i < Values.Length; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    cen[j] += Values[i][j];
                }
            }
            cen.Divide(Values.Length);  //check if this works!
            return new SpatialIndex(ref cen);
        }

        #endregion

        #region Create new index


        public static SpatialIndex CreateInvalid()
        {
            SpatialIndex rval = new SpatialIndex();
            rval.ValueType = SpatialIndexType.Invalid;
            return rval;
        }

        public static SpatialIndex CreateInfinite()
        {
            SpatialIndex rval = new SpatialIndex();
            rval.ValueType = SpatialIndexType.Infinity;
            return rval;
        }

        /// <summary>
        /// Creates new spatial index with simple position
        /// </summary>
        /// <param name="pos">Position vector</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreatePosition(Vector3 pos)
        {
            return new SpatialIndex(ref pos);
        }

        /// <summary>
        /// Creates new spatial index with simple position
        /// </summary>
        /// <param name="pos">Position vector</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreatePosition(ref Vector3 pos)
        {
            return new SpatialIndex(ref pos);
        }

        /// <summary>
        /// Creates new spherical spatial index
        /// </summary>
        /// <param name="pos">Position of the sphere</param>
        /// <param name="radius">radius</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateSphere(Vector3 pos, float radius)
        {
            return new SpatialIndex(new Vector4(ref pos, radius));
        }

        /// <summary>
        /// Creates new spherical spatial index
        /// </summary>
        /// <param name="pos">Position of the sphere</param>
        /// <param name="radius">radius</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateSphere(ref Vector3 pos, float radius)
        {
            return new SpatialIndex(new Vector4(ref pos, radius));
        }

        /// <summary>
        /// Creates new spherical spatial index
        /// </summary>
        /// <param name="sph">Sphere</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateSphere(Sphere sph)
        {
            return new SpatialIndex(ref sph._Center, sph.Radius);
        }

        /// <summary>
        /// Creates new bounding box spatial index
        /// </summary>
        /// <param name="vec1">Minimal corner</param>
        /// <param name="vec2">Maximal corner</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateBox(ref Vector3 vec1, ref Vector3 vec2)
        {
            return new SpatialIndex(ref vec1, ref vec2);
        }

        /// <summary>
        /// Creates new bounding box spatial index
        /// </summary>
        /// <param name="vec1">Minimal corner</param>
        /// <param name="vec2">Maximal corner</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateBox(Vector3 vec1, Vector3 vec2)
        {
            return new SpatialIndex(ref vec1, ref vec2);
        }

        /// <summary>
        /// Creates new bounding box spatial index
        /// </summary>
        /// <param name="vec1">Minimal corner</param>
        /// <param name="vec2">Maximal corner</param>
        /// <returns>New spatial index</returns>
        public static SpatialIndex CreateBox(AABox box)
        {
            return new SpatialIndex(ref box.Minimum, ref box.Maximum);
        }



        #endregion

        #region IConvexObject Members

        public void GetLocalSupportingVertex(ref Vector3 direction, out Vector3 vertex)
        {
            switch (ValueType)
            {
                case SpatialIndexType.Box:
                    ///note, might not work, better check
                    if (direction.X <= 0)
                        vertex.X = Values[0].X;
                    else vertex.X = Values[1].X;
                    if (direction.Y <= 0)
                        vertex.Y = Values[0].Y;
                    else
                        vertex.Y = Values[1].Y;
                    if (direction.Z <= 0)
                        vertex.Z = Values[0].Z;
                    else
                        vertex.Z = Values[1].Z;

                    break;

                case SpatialIndexType.Position:
                    vertex.X = Values[0].X;
                    vertex.Y = Values[0].Y;
                    vertex.Z = Values[0].Z;
                    break;

                case SpatialIndexType.Sphere:

                    Vector3 vecnorm = direction;
                    if (vecnorm.LengthSquared < MathTools.Epsilon)
                        vecnorm.Assign(-1);
                    vecnorm.Normalize();
                    vertex = Values[0].XYZ;
                    vertex.AddScaled(ref vecnorm, Values[1].Z);
                    ///might not work, better check...
                    break;

                case SpatialIndexType.Line:

                    float dot1 = Values[0].XYZ.Dot(ref direction);
                    float dot2 = Values[1].XYZ.Dot(ref direction);
                    if (dot2 > dot1)
                    {
                        vertex.X = Values[1].X;
                        vertex.Y = Values[1].Y;
                        vertex.Z = Values[1].Z;
                    }
                    else
                    {
                        vertex.X = Values[0].X;
                        vertex.Y = Values[0].Y;
                        vertex.Z = Values[0].Z;
                    }

                    break;

                case SpatialIndexType.ConvexObject:
                    float max = float.NegativeInfinity;
                    vertex = new Vector3();

                    for (int i = 0; i < Values.Length; i++)
                    {
                        float dot = Values[i].XYZ.Dot(ref direction);
                        if (dot > max)
                        {
                            max = dot;
                            vertex.X = Values[i].X;
                            vertex.Y = Values[i].Y;
                            vertex.Z = Values[i].Z;
                        }
                    }

                    break;

                default:
                    vertex.X = 0; vertex.Y = 0; vertex.Z = 0;
                    break;
            }


        }

        #endregion
    } //spatial index
*/

    #endregion
}
