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

namespace VisiMath.Geometry
{
    /// <summary>
    /// Simple 3d quad (two triangles)
    /// </summary>
    /// <remarks>Note, for most test uses two triangles (slow, needs to be optimized)</remarks>
    public class Quad : Geometry, ISurfaceGeo
    {

        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new Quad(Vertices.GetCopy());
        }


        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public Quad(System.Xml.XmlReader reader)
            : base(reader)
        {
        }

        /// <summary>
        /// Constructs quad with standard vertex format (no values set)
        /// </summary>
        public Quad()
            : base(4)
        {
        }

        /// <summary>
        /// Constructs quad of specified vertex type
        /// </summary>
        /// <param name="vertextype"></param>
        public Quad(Type vertextype)
            : base(vertextype, 4)
        {
        }

        /// <summary>
        /// Constructs quad from three vertices. Note, all vertices should be of same format!
        /// </summary>
        /// <param name="first">first vertex</param>
        /// <param name="second">second vertex</param>
        /// <param name="third">third vertex</param>
        /// <param name="fourth">fourth corner</param>
        public Quad(IVertex first, IVertex second, IVertex third, IVertex fourth)
            : this(first.GetType())
        {
            Vertices[0] = first;
            Vertices[1] = second;
            Vertices[2] = third;
            Vertices[3] = fourth;
        }

        /// <summary>
        /// Constructs quad  from three position vectors
        /// </summary>
        /// <param name="first">first corner</param>
        /// <param name="second">second corner</param>
        /// <param name="third">third corner</param>
        /// <param name="fourth">fourth corner</param>
        public Quad(Vector3 first, Vector3 second, Vector3 third, Vector3 fourth)
            : this()
        {
            Vertices[0].Position = first;
            Vertices[1].Position = second;
            Vertices[2].Position = third;
        }

        /// <summary>
        /// Constructs quad from the vertex array
        /// </summary>
        /// <param name="vert">vertex array, should contain 3 vertex</param>
        public Quad(IVertexArray vert) :
            base(vert)
        {
            if (vert.Count < 4)
            {
                throw new ArgumentException("Quad - specified vertex array doesn't contain enought vertices for triangle!");
            }
            //note, add debug here if more vertices?
        }

        /// <summary>
        /// Constructs quad from specified vertices in array
        /// </summary>
        /// <param name="vert">Array holding vertices</param>
        /// <param name="startingindex">Index of the first quadvertex</param>
        /// <exception cref="ArgumentException">Thrown if starting index is too high</exception>
        public Quad(IVertexArray vert, int startingindex) : base(vert[0].GetType(), 3)
        {
            if ((startingindex + 4) >= vert.Count)
                throw new ArgumentException("Quad : Not enougth vertices in the source vertex array");
            for (int i = 0; i < 4; i++)
            {
                Vertices[i] = vert[startingindex + i];
            }
        }

        /// <summary>
        /// Is object geometric renderable primitive from enumaration PrimitiveTypes?
        /// </summary>
        public override bool IsPrimitive
        {
            get { return true; }
        }


        /// <summary>
        /// Preferred primitive type for rendering - quad likes quad
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.Quad; }
        }



        /// <summary>
        /// quad is not container
        /// </summary>
        public override bool IsContainer
        {
            get { return false; }
        }

        /// <summary>
        /// Returns closest distance to the ray
        /// </summary>
        /// <param name="ray">Ray to check distance to</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public override float Distance(Ray ray, bool frontOnly = false)
        {
            float dst1 = MathTools.DistanceRayTriangle(ray.Origin, ray.Direction, Vertices[0].Position, Vertices[1].Position, Vertices[2].Position, frontOnly);
            float dst2 = MathTools.DistanceRayTriangle(ray.Origin, ray.Direction, Vertices[2].Position, Vertices[3].Position, Vertices[0].Position, frontOnly);
            if (dst1 == float.NaN)
                return dst2;
            if (dst2 == float.NaN)
                return dst1;
            return (dst1<dst2)?dst1:dst2;
        }

        /// <summary>
        /// Returns distance from the vector
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public override float Distance(ref Vector3 pos)
        {
            return MathTools.DistanceVectorQuad(GetPosition(0), GetPosition(1), GetPosition(2), GetPosition(3), pos);
        }

        /// <summary>
        /// Returns closest distance to the line segment. Generic, from center, OVERRIDE!
        /// </summary>
        ///<param name="pos1">Start of the line</param>
        ///<param name="pos2">end of the line</param>
        /// <returns>Closest distance</returns>
        public override float DistanceLineSegment(ref Vector3 pos1, ref Vector3 pos2)
        {
            return MathTools.DistanceLineSegmentQuad(pos1, pos2, GetPosition(0), GetPosition(1), GetPosition(2), GetPosition(3));
        }

        /// <summary>
        /// Returns closest distance to the sphere
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">radius of the sphere</param>
        /// <returns>Closest distance</returns>
        public override float DistanceSphere(ref Vector3 center, float radius)
        {
            return MathTools.DistanceSphereQuad(center, radius, GetPosition(0), GetPosition(1), GetPosition(2), GetPosition(3));
        }

        /// <summary>
        /// Returns closest distance to the triangle.
        /// </summary>
        /// <param name="pos1">First corner</param>
        /// <param name="pos2">Second corner</param>
        /// <param name="pos3">Third corner</param>
        /// <returns>Distance</returns>
        public override float DistanceTriangle(ref Vector3 pos1, ref Vector3 pos2, ref Vector3 pos3)
        {
            return MathTools.DistanceTriangleQuad(pos1, pos2, pos2, GetPosition(0), GetPosition(1), GetPosition(2), GetPosition(3));
        }


        #region Conversions

        /// <summary>
        /// Convert object to list of points
        /// </summary>
        /// <returns>New geometry object containing point based definition of the object</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        public override Point3DList ToPoints(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return new Point3DList(VertexType, Vertices);
        }

        /// <summary>
        /// Convert object to list of lines
        /// </summary>
        /// <returns>New geometry object containing edges of the object</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        public override LineList ToLines(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray;
            VertexFactory.CopyVertexArray(VertexType, this.Vertices, out varray);
            varray[0] = Vertices[0];
            varray[1] = Vertices[1];
            varray[2] = Vertices[1];
            varray[3] = Vertices[2];
            varray[4] = Vertices[2];
            varray[5] = Vertices[3];
            varray[6] = Vertices[3];
            varray[7] = Vertices[0];

            return new LineList(varray);
        }

        /// <summary>
        /// Convert object to list of triangles. Valid only for surface based objects, otherwise returns null.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface.</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        public override TriangleList ToTriangles(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(VertexType, 6);
            varray[0] = VertexFactory.CopyVertex(Vertices[0], VertexType);
            varray[1] = VertexFactory.CopyVertex(Vertices[1], VertexType);
            varray[2] = VertexFactory.CopyVertex(Vertices[2], VertexType);
            varray[3] = VertexFactory.CopyVertex(Vertices[2], VertexType);
            varray[4] = VertexFactory.CopyVertex(Vertices[3], VertexType);
            varray[5] = VertexFactory.CopyVertex(Vertices[0], VertexType);
            
            return new TriangleList(varray);
        }

        /// <summary>
        /// Convert object to indexed triangle mesh. Valid only for surface based objects, otherwise returns null.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        public override IndexedMesh ToIndexedMesh(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(VertexType, 6);
            varray[0] = VertexFactory.CopyVertex(Vertices[0], VertexType);
            varray[1] = VertexFactory.CopyVertex(Vertices[1], VertexType);
            varray[2] = VertexFactory.CopyVertex(Vertices[2], VertexType);
            varray[3] = VertexFactory.CopyVertex(Vertices[2], VertexType);
            varray[4] = VertexFactory.CopyVertex(Vertices[3], VertexType);
            varray[5] = VertexFactory.CopyVertex(Vertices[0], VertexType);

            int[] indices = new int[6]{0,1,2,2,3,0};

            return new IndexedMesh(varray, indices);
        }


        #endregion

        /// <summary>
        /// Returns closest distance from the generic geometric object
        /// </summary>
        /// <param name="obj">Object to check distance with</param>
        /// <returns>Closest distance to the object</returns>
        public override float Distance(IGeometry obj)
        {
            return obj.DistanceQuad(this);
        }


        /// <summary>
        /// Checks if specified position is inside the object
        /// </summary>
        /// <param name="pos">Position to check</param>
        /// <returns>True if vector is inside the object</returns>
        public override bool ContainsPosition(Vector3 pos)
        {
            if (MathTools.PointInTriangle(Vertices[0].Position, Vertices[1].Position, Vertices[2].Position, pos)) return true;
            if (MathTools.PointInTriangle(Vertices[2].Position, Vertices[3].Position, Vertices[0].Position, pos)) return true;
            return false;
        }


        /// <summary>
        /// Check for intersection and fills intersection info if intersection is closer to the origin of the ray then value before
        /// </summary>
        /// <param name="inters">Data structure with intersection info</param>
        /// <param name="obj">Ray object</param>
        /// <returns>True if there was an intersection and it was closer then previous stored</returns>
        public override bool GetIntersectionInfo(Ray obj, ref IIntersectionInfo inters)
        {
            float dist;
            Vector3 ver;

            if (MathTools.IntersectionRayTriangle(obj.Origin, obj.Direction, Vertices[0].Position, Vertices[1].Position, Vertices[2].Position, out dist, out ver, false))
            {
                if (dist < inters.Distance)
                {
                    inters.Distance = dist;
                    inters.IntersectionPoint = ver;
                    inters.GeoObject = this;
                    return true;
                }
            }
            if (MathTools.IntersectionRayTriangle(obj.Origin, obj.Direction, Vertices[2].Position, Vertices[3].Position, Vertices[0].Position, out dist, out ver, false))
            {
                if (dist < inters.Distance)
                {
                    inters.Distance = dist;
                    inters.IntersectionPoint = ver;
                    inters.GeoObject = this;
                    return true;
                }
            }
            return false;
        }

        /// <summary>
        /// Checks for intersection between object and moving sphere
        /// </summary>
        ///<param name="radius">Radius of the sphere</param>
        /// <param name="maxdistance">Maximal distance to travel</param>
        /// <param name="ray">Ray describing movement of the sphere</param>
        /// <returns>Distance to the intersection or -1 if no intersection exists</returns>
        public override float IntersectsMoving(float radius, Ray ray, float maxdistance)
        {
            Vector3 targ = Vector3.MoveInDirection(ref ray.Origin, ref ray.Direction, maxdistance);
            float t;

            float mindist = float.MaxValue;

            for (int i = 0; i < 1; i++)
            {
                Vector3 me0 = GetPosition(i * 4);
                Vector3 me1 = GetPosition(i * 4 + 1);
                Vector3 me2 = GetPosition(i * 4 + 2);

                if (MathTools.IntersectionTriangleCapsule(ref me0, ref me1, ref me2, ref ray.Origin, ref targ, radius, out t))
                {
                    float newdst = t * maxdistance;
                    mindist = MathTools.Min(mindist, newdst);
                }

                me0 = GetPosition(i * 4 + 2);
                me1 = GetPosition(i * 4 + 3);
                me2 = GetPosition(i * 4 + 0);

                if (MathTools.IntersectionTriangleCapsule(ref me0, ref me1, ref me2, ref ray.Origin, ref targ, radius, out t))
                {
                    float newdst = t * maxdistance;
                    mindist = MathTools.Min(mindist, newdst);
                }

            }
            if (mindist == float.MaxValue)
                return -1;
            return mindist;
        }


        #region ISurfaceGeo Members

        /// <summary>
        /// Recalculates normal of the quad
        /// </summary>
        public void RecalculateNormals()
        {
            Vector3 normal = MathTools.CalcTriNormal(Vertices[0].Position, Vertices[1].Position, Vertices[2].Position);
            for (int i = 0; i < 4; i++)
                Vertices[i].Normal = normal;
        }

        #endregion

    }

    /// <summary>
    /// List of quads
    /// </summary>
    public class QuadList : Geometry, ISurfaceGeo, IGeoContainer<Quad>
    {

        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new QuadList(Vertices.GetCopy());
        }

        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public QuadList(System.Xml.XmlReader reader) : base(reader)
        {
        }

        /// <summary>
        /// Construct QuadList from the vertex array source
        /// </summary>
        /// <param name="source">Source vertex array, must contain 4*quad number vertices!</param>
        /// <exception cref="System.ArgumentException">Throw for inccorect number of vertices</exception>
        public QuadList(IVertexArray source) : base(source)
        {
            if (source.Count % 4 != 0)
                throw new ArgumentException("QuadList requires source array with tripplets of vertices for each triangle!");
        }

        /// <summary>
        /// Constructs quad list with default vertex type and with place for storing count quads
        /// </summary>
        /// <param name="count">Number of triangles</param>
        public QuadList(int count)
            : base(count * 4)
        {
        }

        /// <summary>
        /// Constructs quad list with required vertex type and with place for storing count quads
        /// </summary>
        /// <param name="count">Number of triangles</param>
        /// <param name="vertextype">Required type of vertex (IVertex type)</param>
        public QuadList(Type vertextype, int count)
            : base(vertextype, count * 4)
        {
        }

        /// <summary>
        /// Is object geometric renderable primitive from enumaration PrimitiveTypes?
        /// </summary>
        public override bool IsPrimitive
        {
            get { return true; }
        }

        /// <summary>
        /// Preferred primitive type for rendering
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.QuadList; }
        }

        /// <summary>
        /// Quad List works as a list of quads, returns true
        /// </summary>
        public override bool IsContainer
        {
            get { return true; }
        }


        /// <summary>
        /// Convert object to list of points
        /// </summary>
        /// <returns>New geometry object containing point based definition of the object</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        public override Point3DList ToPoints(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return new Point3DList(VertexType, this.Vertices);
        }

        /// <summary>
        /// Convert object to list of lines
        /// </summary>
        /// <returns>New geometry object containing edges of the object</returns>
        /// <param name="quality">
        /// Not used
        /// </param>
        /// <param name="VertexType">Required vertex type</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.
        /// NOT USED!</param>
        public override LineList ToLines(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(VertexType, (Vertices.Count / 3) * 8);

            int targetidx = 0;

            for (int i = 0; i < Count; i++)
            {
                varray[targetidx + 0] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);
                varray[targetidx + 1] = VertexFactory.CopyVertex(Vertices[i * 4 + 1], VertexType);
                varray[targetidx + 2] = VertexFactory.CopyVertex(Vertices[i * 4 + 1], VertexType);
                varray[targetidx + 3] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 4] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 5] = VertexFactory.CopyVertex(Vertices[i * 4 + 3], VertexType);
                varray[targetidx + 6] = VertexFactory.CopyVertex(Vertices[i * 4 + 3], VertexType);
                varray[targetidx + 7] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);

                targetidx += 8;
            }
            return new LineList(varray);
        }

        /// <summary>
        /// Convert object to list of triangles - returns self.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface.</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        /// <remarks>Triangle list returns pointer to self.</remarks>
        public override TriangleList ToTriangles(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(VertexType, 6 * Count);

            int targetidx = 0;

            for (int i = 0; i < Count; i++)
            {
                varray[targetidx + 0] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);
                varray[targetidx + 1] = VertexFactory.CopyVertex(Vertices[i * 4 + 1], VertexType);
                varray[targetidx + 2] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 3] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 4] = VertexFactory.CopyVertex(Vertices[i * 4 + 3], VertexType);
                varray[targetidx + 5] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);

                targetidx += 6;
            }
            return new TriangleList(varray);
        }

        /// <summary>
        /// Convert object to indexed triangle mesh. Valid only for surface based objects, otherwise returns null.
        /// </summary>
        /// <returns>New geometry object containing triangle faces of the object surface</returns>
        /// <param name="quality">
        /// Quality for parametric objects (0-1). Zero is simples possible, 1 maximal for the current dimension of the object. 
        /// If vertexlimit parameter is used, quality will be reduced to fullfill maximal vertex budget
        /// </param>
        /// <param name="VertexType">Required type of the vertex</param>
        /// <param name="vertexlimit">Maximal number of allowed vertices. If set to zero, there's no limit. 
        /// Used for fully parametric objects or to reduce number of the vertices in complicated objects.</param>
        /// <remarks>Doesn't check for same vertices</remarks>
        public override IndexedMesh ToIndexedMesh(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            IVertexArray varray = VertexFactory.CreateArray(VertexType, 6 * Count);
            int[] indices = new int[6 * Count];

            int targetidx = 0;


            for (int i = 0; i < Count; i++)
            {
                varray[targetidx + 0] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);
                varray[targetidx + 1] = VertexFactory.CopyVertex(Vertices[i * 4 + 1], VertexType);
                varray[targetidx + 2] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 3] = VertexFactory.CopyVertex(Vertices[i * 4 + 2], VertexType);
                varray[targetidx + 4] = VertexFactory.CopyVertex(Vertices[i * 4 + 3], VertexType);
                varray[targetidx + 5] = VertexFactory.CopyVertex(Vertices[i * 4 + 0], VertexType);
                
                for (int j = 0; j < 6; j++)
                    indices[targetidx + j] = targetidx + j;                               
                targetidx += 6;

            }

            return new IndexedMesh(varray, indices);
        }

        /// <summary>
        /// Returns closest distance from the generic geometric object
        /// </summary>
        /// <param name="obj">Object to check distance with</param>
        /// <returns>Closest distance to the object</returns>
        public override float Distance(IGeometry obj)
        {
            return obj.DistanceQuads(this);
        }

        /// <summary>
        /// Checks if specified position is inside the object - point lies on any triangle
        /// </summary>
        /// <param name="pos">Position to check</param>
        /// <returns>True if vector is inside the object</returns>
        public override bool ContainsPosition(Vector3 pos)
        {
            for (int i = 0; i < Count; i++)
            {
                if (MathTools.PointInTriangle(Vertices[i * 4].Position, Vertices[i * 4 + 1].Position, Vertices[i * 4 + 2].Position, pos))
                    return true;
                if (MathTools.PointInTriangle(Vertices[i * 4+2].Position, Vertices[i * 4 + 3].Position, Vertices[i * 4 + 0].Position, pos))
                    return true;
            }
            return false;
        }


        /// <summary>
        /// Check for intersection and fills intersection info if intersection is closer to the origin of the ray then value before
        /// </summary>
        /// <param name="inters">Data structure with intersection info</param>
        /// <param name="obj">Ray object</param>
        /// <returns>True if there was an intersection and it was closer then previous stored</returns>
        public override bool GetIntersectionInfo(Ray obj, ref IIntersectionInfo inters)
        {

            float dist;
            Vector3 ver;
            bool rval = false;

            for (int i = 0; i < Vertices.Count / 3; i++)
            {
                if (MathTools.IntersectionRayTriangle(obj.Origin, obj.Direction, Vertices[i * 4].Position, Vertices[i * 4 + 1].Position, Vertices[i * 4+2].Position, out dist, out ver, false))
                {
                    if (dist < inters.Distance)
                    {
                        inters.Distance = dist;
                        inters.IntersectionPoint = ver;
                        inters.GeoObject = this;
                        rval = true;
                    }
                }
                if (MathTools.IntersectionRayTriangle(obj.Origin, obj.Direction, Vertices[i * 4 + 2].Position, Vertices[i * 4 + 3].Position, Vertices[i * 4].Position, out dist, out ver, false))
                {
                    if (dist < inters.Distance)
                    {
                        inters.Distance = dist;
                        inters.IntersectionPoint = ver;
                        inters.GeoObject = this;
                        rval = true;
                    }
                }
            }
            return rval;
        }

        /// <summary>
        /// Check for intersection and adds intersection info if there was any intersection
        /// </summary>
        /// <param name="ilist">List for intersections</param>
        /// <param name="obj">Ray object</param>
        /// <returns>True if there was an intersection</returns>        
        /// <remarks>For now using base version and returns only hit of the closest encountered triangle. Might need reworking to check all tris?</remarks>
        public override bool GetIntersectionInfo(Ray obj, IntersectionList ilist)
        {
            return base.GetIntersectionInfo(obj, ilist);
        }


        /// <summary>
        /// Returns closest distance to the ray
        /// </summary>
        /// <param name="ray">Ray to check distance to</param>
        /// <param name="frontOnly">Check only objects in front of the origin</param>
        /// <returns>Closest distance to the ray, if frontOnly and object is behind the ray, returns NaN</returns>
        public override float Distance(Ray ray, bool frontOnly = false)
        {
            float dist = float.MaxValue;
            for (int i = 0; i < Count; i++)
            {
                float newdst = MathTools.DistanceRayTriangle(ray.Origin, ray.Direction, Vertices[i * 4 + 0].Position, Vertices[i * 4 + 1].Position, Vertices[i * 4 + 2].Position, frontOnly);
                if (newdst != float.NaN)
                {
                    if (newdst < dist)
                    {
                        dist = newdst;
                    }
                }
                newdst = MathTools.DistanceRayTriangle(ray.Origin, ray.Direction, Vertices[i * 4 + 2].Position, Vertices[i * 4 + 3].Position, Vertices[i * 4].Position, frontOnly);
                if (newdst != float.NaN)
                {
                    if (newdst < dist)
                    {
                        dist = newdst;
                    }
                }
            }
            if (dist == float.MaxValue)
                return float.NaN;
            return dist;
        }

        /// <summary>
        /// Returns distance from the vector
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public override float Distance(ref Vector3 pos)
        {
            float dist = float.MaxValue;
            for (int i = 0; i < Count; i++)
            {
                float newdst = MathTools.DistanceVectorQuad(GetPosition(i*4), GetPosition(i*4+1), GetPosition(i*4+2), GetPosition(i*4+3), pos);
                if (newdst < dist)
                {
                    dist = newdst;
                }
            }
            return dist;
        }


        /// <summary>
        /// Returns closest distance to the line segment. Generic, from center, OVERRIDE!
        /// </summary>
        ///<param name="pos1">Start of the line</param>
        ///<param name="pos2">end of the line</param>
        /// <returns>Closest distance</returns>
        public override float DistanceLineSegment(ref Vector3 pos1, ref Vector3 pos2)
        {
            float dist = float.MaxValue;
            for (int i = 0; i < Count; i++)
            {
                float newdst = MathTools.DistanceLineSegmentQuad(pos1, pos2, GetPosition(i * 4), GetPosition(i * 4 + 1), GetPosition(i * 4 + 2), GetPosition(i * 4 + 3));
                if (newdst < dist)
                {
                    dist = newdst;
                }
            }
            return dist;
        }

        /// <summary>
        /// Returns closest distance to the sphere
        /// </summary>
        /// <param name="center">center of the sphere</param>
        /// <param name="radius">radius of the sphere</param>
        /// <returns>Closest distance</returns>
        public override float DistanceSphere(ref Vector3 center, float radius)
        {
            float dist = float.MaxValue;
            for (int i = 0; i < Count; i++)
            {
                float newdst = MathTools.DistanceSphereQuad(center, radius, GetPosition(i * 4), GetPosition(i * 4 + 1), GetPosition(i * 4 + 2), GetPosition(i * 4 + 3));
                if (newdst < dist)
                {
                    dist = newdst;
                }
            }
            return dist;
        }

        /// <summary>
        /// Returns closest distance to the triangle.
        /// </summary>
        /// <param name="pos1">First corner</param>
        /// <param name="pos2">Second corner</param>
        /// <param name="pos3">Third corner</param>
        /// <returns>Distance</returns>
        public override float DistanceTriangle(ref Vector3 pos1, ref Vector3 pos2, ref Vector3 pos3)
        {
            float dist = float.MaxValue;
            for (int i = 0; i < Count; i++)
            {
                float newdst = MathTools.DistanceTriangleQuad(pos1, pos2, pos3, GetPosition(i * 4), GetPosition(i * 4 + 1), GetPosition(i * 4 + 2), GetPosition(i * 4 + 3));
                if (newdst < dist)
                {
                    dist = newdst;
                }
            }
            return dist;
        }

        /// <summary>
        /// Checks for intersection between object and moving sphere
        /// </summary>
        ///<param name="radius">Radius of the sphere</param>
        /// <param name="maxdistance">Maximal distance to travel</param>
        /// <param name="ray">Ray describing movement of the sphere</param>
        /// <returns>Distance to the intersection or -1 if no intersection exists</returns>
        public override float IntersectsMoving(float radius, Ray ray, float maxdistance)
        {
            Vector3 targ = Vector3.MoveInDirection(ref ray.Origin, ref ray.Direction, maxdistance);
            float t;

            float mindist = float.MaxValue;

            for (int i = 0; i < Count; i++)
            {
                Vector3 me0 = GetPosition(i * 4);
                Vector3 me1 = GetPosition(i * 4 + 1);
                Vector3 me2 = GetPosition(i * 4 + 2);

                if (MathTools.IntersectionTriangleCapsule(ref me0, ref me1, ref me2, ref ray.Origin, ref targ, radius, out t))
                {
                    float newdst = t * maxdistance;
                    mindist = MathTools.Min(mindist, newdst);
                }

                me0 = GetPosition(i * 4 + 2);
                me1 = GetPosition(i * 4 + 3);
                me2 = GetPosition(i * 4 + 0);

                if (MathTools.IntersectionTriangleCapsule(ref me0, ref me1, ref me2, ref ray.Origin, ref targ, radius, out t))
                {
                    float newdst = t * maxdistance;
                    mindist = MathTools.Min(mindist, newdst);
                }

            }
            if (mindist == float.MaxValue)
                return -1;
            return mindist;
        }
        


        #region IGeoContainer<Quad> Members

        /// <summary>
        /// Returns or sets triangles in list
        /// </summary>
        /// <param name="index">index of the triangle</param>
        /// <returns>New triangle object</returns>
        public Quad this[int index]
        {
            get
            {
                return new Quad(this.Vertices, index * 4);
            }
            set
            {
                for (int i = 0; i < 4; i++)
                    Vertices[index * 4 + i] = value.Vertices[i];
            }
        }

        /// <summary>
        /// Returns number of the triangles in list
        /// </summary>
        public int Count
        {
            get { return Vertices.Count/4; }
        }

        /// <summary>
        /// Not dynamic, cannot add! Does nothing
        /// </summary>
        /// <param name="geoobject"></param>
        public void Add(Quad geoobject)
        {            
        }

        #endregion

        #region IEnumerable<IGeometry> Members

        public IEnumerator<IGeometry> GetEnumerator()
        {
            return new GeoEnumerator<Quad>(this);
        }

        #endregion

        #region IEnumerable Members

        System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
        {
            return Vertices.GetEnumerator(); //????
        }

        #endregion

        #region ISurfaceGeo Members

        /// <summary>
        /// Recalculates normals for all triangles
        /// </summary>
        public void RecalculateNormals()
        {
            for (int i = 0; i < this.Count; i++)
            {
                Vector3 normal = MathTools.CalcTriNormal(Vertices[i * 4 + 0].Position, Vertices[i * 4 + 1].Position, Vertices[i * 4 + 2].Position);
                for (int j = 0; j < 4; i++)
                    Vertices[i * 4 + j].Normal = normal;
            }
        }

        #endregion
    }
}
