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

namespace VisiMath.Geometry
{
    /// <summary>
    /// Single Line segment
    /// </summary>
    public class LineSegment3D : Geometry, ISpatialIndex
    {
        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new LineSegment3D(Vertices[0], Vertices[1]);
        }


        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public LineSegment3D(System.Xml.XmlReader reader)
        {
            ReadXml(reader);
        }
        
        /// <summary>
        /// Constructs line segment from two vertices, should use same vertex format!
        /// </summary>
        /// <param name="first"></param>
        /// <param name="second"></param>
        public LineSegment3D(IVertex first, IVertex second) : base(first.GetType(), 2)
        {
            Vertices[0] = first;
            Vertices[1] = second;
        }

        /// <summary>
        /// Constructs line segment from two vertices
        /// </summary>
        /// <param name="first"></param>
        /// <param name="second"></param>
        public LineSegment3D(ref Vector3 first, ref Vector3 second) : base(typeof(PositionVertex), 2)
        {            
            Vertices[0].Position = first;
            Vertices[1].Position = second;
        }

        /// <summary>
        /// LineSegment is primitive
        /// </summary>
        public override bool IsPrimitive
        {
            get { return true; }
        }

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

        /// <summary>
        /// Distance to vector
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public override float Distance(ref Vector3 pos)
        {
            return MathTools.DistanceVectorLineSegment(this.Vertices[0].Position, this.Vertices[1].Position, pos);
        }

        /// <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)
        {
            return MathTools.DistanceRayToLineSegment(ray.Origin, ray.Direction, Vertices[0].Position, Vertices[1].Position, frontOnly);
        }


        /// <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.DistanceTwoLineSegments(Vertices[0].Position, Vertices[1].Position, pos1, pos2);
        }

        /// <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.DistanceLineSegmentSphere(Vertices[0].Position, Vertices[1].Position, center, radius);
        }

        /// <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)
        {
            Vector3 l1 = Vertices[0].Position;
            Vector3 l2 = Vertices[1].Position;
            return MathTools.DistanceLineSegmentTriangle(ref l1, ref l2, ref pos1, ref pos2, ref pos3);
        }


        /// <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.DistanceLineSegment(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)
        {
            return MathTools.PointOnLineSegment(Vertices[0].Position, Vertices[1].Position, pos);
        }


        /// <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 point;

            if (MathTools.IntersectionRayLineSegment(obj.Origin, obj.Direction, Vertices[0].Position, Vertices[1].Position, out dist, out point))
            {
                if (dist < inters.Distance)
                {
                    inters.Distance = dist;
                    inters.IntersectionPoint = point;
                    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;

            Vector3 me0 = Vertices[0].Position;
            Vector3 me1 = Vertices[1].Position;

            if (!MathTools.IntersectionSegmentCapsule(ref me0, ref me1, ref ray.Origin, ref targ, radius, out t))
                return -1;
            return t * maxdistance;
        }



        /// <summary>
        /// Prefers line list
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.LineList; }
        }

        /// <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 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.</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">
        /// 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)
        {
            return new LineList(VertexType, this.Vertices);
        }

        /// <summary>
        /// Can't convert single line to triangle, returns null.
        /// </summary>
        /// <param name="VertexType">unused</param>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        public override TriangleList ToTriangles(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }


        /// <summary>
        /// Can't convert single line to triangle, returns null.
        /// </summary>
        /// <param name="VertexType">unused</param>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        public override IndexedMesh ToIndexedMesh(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }

        #region SpatialIndex
        /// <summary>
        /// Returns spatial index as box
        /// </summary>
        public AABox AsBox
        {
            get
            {
                Vector3 vec1 = GetPosition(0);
                Vector3 vec2 = GetPosition(1);
                return new AABox(MathTools.Min(ref vec1, ref vec2), MathTools.Max(ref vec1, ref vec2));
            }
        }
        /// <summary>
        /// Returns spatial index as sphere
        /// </summary>
        public Sphere AsSphere
        {
            get
            {
                Vector3 vec1 = GetPosition(0) - GetPosition(1);

                return new Sphere(Center, 0.5f * vec1.LengthSquared);
            }
        }
        /// <summary>
        /// returns spatial index as point (center)
        /// </summary>
        public Vector3 AsPoint { get { return Center; } }

        /// <summary>
        /// return true if index contains (intersects) another index
        /// </summary>
        /// <param name="index">Second spatial index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return Contains(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return Contains(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return Contains(line);

            return Contains(index as IConvexObject);
        }

        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Vector3 index)
        {
            return Contains(ref index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(ref Vector3 index)
        {
            return ContainsPosition(index);
        }
        /// <summary>
        /// Return true if index contains position
        /// </summary>
        /// <param name="index">Position index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Point3D index)
        {
            return Contains(index.GetPosition(0));
        }

        /// <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>
        public bool ContainsLine(Vector3 line1, Vector3 line2)
        {
            return
                (MathTools.DistanceTwoLineSegments(GetPosition(0), GetPosition(1), line1, line2) == 0);
        }
        /// <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>
        public bool ContainsLine(ref Vector3 line1, ref Vector3 line2)
        {
            return
                (MathTools.DistanceTwoLineSegments(GetPosition(0), GetPosition(1), line1, line2) == 0);
        }
        /// <summary>
        /// Return true if index contains line
        /// </summary>
        /// <param name="index">line index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(LineSegment3D line)
        {
            return
                (MathTools.DistanceTwoLineSegments(GetPosition(0), GetPosition(1), line.GetPosition(0), line.GetPosition(1)) == 0);
        }

        /// <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>
        public bool Contains(Vector3 center, float radius)
        {
            Vector3 pos1 = GetPosition(0), pos2 = GetPosition(1);
            float t;
            return MathTools.IntersectionSegmentSphere(ref pos1, ref pos2, ref center, radius, out t);
        }
        /// <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>
        public bool Contains(ref Vector3 center, float radius)
        {
            Vector3 pos1 = GetPosition(0), pos2 = GetPosition(1);
            float t;
            return MathTools.IntersectionSegmentSphere(ref pos1, ref pos2, ref center, radius, out t);
        }
        /// <summary>
        /// Return true if index contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(Sphere sphere)
        {
            Vector3 pos1 = GetPosition(0), pos2 = GetPosition(1);
            float t;
            return MathTools.IntersectionSegmentSphere(ref pos1, ref pos2, ref sphere._Center, sphere.Radius, out t);
        }

        /// <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>
        public bool ContainsBox(Vector3 min, Vector3 max)
        {
            return Contains(new AABox(ref min, ref 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>
        public bool ContainsBox(ref Vector3 min, ref Vector3 max)
        {
            return Contains(new AABox(ref min, ref max));
        }
        /// <summary>
        /// Return true if index contains axis alligned box
        /// </summary>
        /// <param name="box">AABox</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(AABox box)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, box);   //optimize this?!!!
        }

        /// <summary>
        /// Return true if index contains generic convex object
        /// </summary>
        /// <param name="convex">Convex object</param>
        /// <returns>True if intersection exists</returns>
        public bool Contains(IConvexObject convex)
        {
            return CollisionSolver.QuickTest.ConvexObjects(this, 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>
        public bool ContainsFully(ref ISpatialIndex index)
        {
            AABox box = index as AABox;
            if (box != null)
                return ContainsFully(box);
            Sphere sph = index as Sphere;
            if (sph != null)
                return ContainsFully(sph);
            Point3D point = index as Point3D;
            if (point != null)
                return Contains(point);
            LineSegment3D line = index as LineSegment3D;
            if (line != null)
                return ContainsFully(line);

            return ContainsFully(index as IConvexObject);
        }

        /// <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>
        public bool ContainsFullyLine(Vector3 line1, Vector3 line2)
        {
            return ContainsFullyLine(ref line1, ref 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>
        public bool ContainsFullyLine(ref Vector3 line1, ref Vector3 line2)
        {
            return (ContainsPosition(line1) && ContainsPosition(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>
        public bool ContainsFully(LineSegment3D line)
        {
            return ContainsFullyLine(line.GetPosition(0), line.GetPosition(1));
        }

        /// <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>
        public bool ContainsFully(Vector3 center, float radius)
        {
            return false;
        }
        /// <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>
        public bool ContainsFully(ref Vector3 center, float radius)
        {
            return false;
        }
        /// <summary>
        /// Return true if index fully contains sphere
        /// </summary>
        /// <param name="sphere">sphere index</param>
        /// <returns>True if index fully contains other object</returns>
        public bool ContainsFully(Sphere sphere)
        {
            return false;
        }

        /// <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>
        public bool ContainsFullyBox(Vector3 min, Vector3 max)
        {
            return false;
        }
        /// <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>
        public bool ContainsFullyBox(ref Vector3 min, ref Vector3 max)
        {
            return false;
        }
        /// <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>
        public bool ContainsFully(AABox box)
        {
            return false;
        }

        /// <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>
        public bool ContainsFully(IConvexObject convex)
        {
            return false;
        }




        #endregion

        /// <summary>
        /// Returns implicit (math expression) or explicit (vertex based) type of convex object
        /// </summary>
        public ConvexObjectType BaseType { get { return ConvexObjectType.Explicit; } }

        /// <summary>
        /// Returns number of vertices, valid for explicit object types
        /// </summary>
        public int VertexCount { get { return 2; } }

        /// <summary>
        /// Returns vertex with specified index, valid for explicit object types
        /// </summary>
        /// <param name="index">index of vertex</param>
        /// <param name="vertex">Vector to save vertex</param>
        public void GetVertex(int index, out Vector3 vertex)
        {
            vertex = GetPosition(index);
        }

        /// <summary>
        /// Gets supporting vertex in local coordinates
        /// </summary>
        /// <param name="vertex">Save vertex</param>
        /// <param name="direction">Direction vector</param>
        public void GetLocalSupportingVertex(ref Vector3 direction, out Vector3 vertex)
        {
            Vector3 l1 = GetPosition(0);
            Vector3 l2 = GetPosition(1);
            float dot1 = l1.Dot(ref direction);
            float dot2 = l2.Dot(ref direction);

            if (dot1 > dot2)
            {
                vertex = l1;
            }
            else
                vertex = l2;
        }




    }

    /// <summary>
    /// List of line segments
    /// </summary>
    public class LineList : Geometry, IGeoContainer<LineSegment3D>
    {
        /// <summary>
        /// Returns copy of the geometric data
        /// </summary>
        /// <returns>Full copy of the geometry</returns>
        public override IGeometry GetCopy()
        {
            return new LineList(Vertices.GetCopy());
        }


        /// <summary>
        /// XML reader constructor
        /// </summary>
        /// <param name="reader">XML reader</param>
        public LineList(System.Xml.XmlReader reader)
            : base(reader)
        {
        }
        
        
        /// <summary>
        /// Constructs empty line list with specified number of vertices
        /// </summary>
        /// <param name="count"></param>
        public LineList(int count) : base(count)
        {
            if (count % 2 != 0)
            {
                throw new ArgumentException("LineList - requires even number of vertices");
            }
        }

        /// <summary>
        /// Constructs linelist from source vertex array
        /// </summary>
        /// <param name="source">Source vertex array (should have even number of vertices!)</param>
        public LineList(IVertexArray source) : base(source)
        {
            if (Vertices.Count % 2 != 0)
            {
                throw new ArgumentException("LineList - requires even number of vertices");
            }
        }

        /// <summary>
        /// Constructs linelist from source vertex array
        /// </summary>
        /// <param name="source">Source vertex array (should have even number of vertices!)</param>
        /// <param name="VertexType">Required type of the vertex</param>
        public LineList(Type VertexType, IVertexArray source)
            : base(VertexType, source)
        {
            if (Vertices.Count % 2 != 0)
            {
                throw new ArgumentException("LineList - requires even number of vertices");
            }
        }

        /// <summary>
        /// Creates new linelist with specified vertextype and count of vertices
        /// </summary>
        /// <param name="VertexType">Required vertextype</param>
        /// <param name="count">Number of vertices</param>
        public LineList(Type VertexType, int count)
            : base(VertexType, count)
        {
            if (Vertices.Count % 2 != 0)
            {
                throw new ArgumentException("LineList - requires even number of vertices");
            }
        }
        

        /// <summary>
        /// LineList is primitive
        /// </summary>
        public override bool IsPrimitive
        {
            get { return false; }
        }

        /// <summary>
        /// LineList can work as a list of LineSegments
        /// </summary>
        public override bool IsContainer
        {
            get { return true; }
        }

        /// <summary>
        /// Returns closest distance from pos to one of the segments
        /// </summary>
        /// <param name="pos">position vector</param>
        /// <returns>closest distance</returns>
        public override float Distance(ref Vector3 pos)
        {
            float dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count /2; i++)
            {
                Vector3 line1 = Vertices[i*2].Position;
                Vector3 line2 = Vertices[i*2+1].Position;
                float newdst = MathTools.DistanceVectorLineSegment(ref line1, ref line2, ref pos);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <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 dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count / 2; i++)
            {
                Vector3 line1 = Vertices[i * 2].Position;
                Vector3 line2 = Vertices[i * 2 + 1].Position;
                float newdst = MathTools.DistanceTwoLineSegments(ref line1, ref line2, ref pos1, ref pos2);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <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 dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count / 2; i++)
            {
                Vector3 line1 = Vertices[i * 2].Position;
                Vector3 line2 = Vertices[i * 2 + 1].Position;
                float newdst = MathTools.DistanceLineSegmentSphere(ref line1, ref line2, ref center, radius);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <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 dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count / 2; i++)
            {
                Vector3 line1 = Vertices[i * 2].Position;
                Vector3 line2 = Vertices[i * 2 + 1].Position;
                float newdst = MathTools.DistanceLineSegmentTriangle(ref line1, ref line2, ref pos1, ref pos2, ref pos3);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <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 dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count / 2; i++)
            {
                Vector3 line1 = Vertices[i * 2].Position;
                Vector3 line2 = Vertices[i * 2 + 1].Position;
                float newdst = ray.DistanceToLineSegment(ref line1, ref line2, frontOnly);
                if (newdst != float.NaN)
                {
                    if (newdst < dst)
                        dst = newdst;
                }
            }
            return dst;
        }

        /// <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.DistanceLines(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)
        {

            for (int i = 0; i < Count; i++)
            {
                if (MathTools.PointOnLineSegment(Vertices[i*2].Position, Vertices[i*2 + 1].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 point;
            bool rval = false;

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

        /// <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 * 2);
                Vector3 me1 = GetPosition(i * 2 + 1);

                if (MathTools.IntersectionSegmentCapsule(ref me0, ref me1, 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 Conversions

        /// <summary>
        /// Linelist of course prefers linelist
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.LineList; }
        }

        /// <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 - returns self unles different vertex type is specified
        /// </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)
        {
            if (VertexType == this.PreferredVertexType)
                return this;

            return new LineList(VertexType, this.Vertices);
        }

        /// <summary>
        /// Linelist can't be converted to triangles, returns null
        /// </summary>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        /// <param name="VertexType">unused</param>
        public override TriangleList ToTriangles(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }

        /// <summary>
        /// Linelist can't be converted to triangles, returns null
        /// </summary>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        /// <param name="VertexType">unused</param>
        public override IndexedMesh ToIndexedMesh(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }

        #endregion

        #region IGeoContainer<LineSegment3D> Members

        public LineSegment3D this[int index]
        {
            get
            {
                return new LineSegment3D(this.Vertices[index * 2], this.Vertices[index * 2 + 1]);
            }
            set
            {
                this.Vertices[index * 2] = value.Vertices[0];
                this.Vertices[index * 2 + 1] = value.Vertices[1];
            }
        }

        public int Count
        {
            get { return Vertices.Count / 2; }
        }

        /// <summary>
        /// LineList is fixed array, can't add, doesn't do anything
        /// </summary>
        /// <param name="geoobject"></param>
        public void Add(LineSegment3D geoobject)
        {            
        }

        #endregion

        #region IEnumerable<IGeometry> Members

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

        #endregion

        #region IEnumerable Members

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

        #endregion
    }

    /// <summary>
    /// One long line of connected line segments
    /// </summary>
    public class PolyLine : Geometry, IGeoContainer<LineSegment3D>
    {
        #region IGeoContainer<LineSegment3D> Members

        public LineSegment3D this[int index]
        {
            get
            {
                return new LineSegment3D(Vertices[index], Vertices[index + 1]);
            }
            set
            {
                Vertices[index] = value.Vertices[0];
                Vertices[index] = value.Vertices[1];
            }
        }

        public int Count
        {
            get { return Vertices.Count-1; }
        }

        /// <summary>
        /// Not dynamic, can't add, doesn't do anything
        /// </summary>
        public void Add(LineSegment3D geoobject)
        {            
        }

        #endregion

        #region IEnumerable<IGeometry> Members

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

        #endregion

        #region IEnumerable Members

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

        #endregion

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

        /// <summary>
        /// Constructor from array of vertices
        /// </summary>
        /// <param name="varray">Array of connected vertices</param>
        public PolyLine(IVertexArray varray)
            : base(varray)
        {
        }

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

        /// <summary>
        /// Polyline is primitive
        /// </summary>
        public override bool IsPrimitive
        {
            get { return true; }
        }

        /// <summary>
        /// Polyline is container of connected line segments
        /// </summary>
        public override bool IsContainer
        {
            get { return true; }
        }
        
        /// <summary>
        /// Returns closest distance of the polyline from one specified point
        /// </summary>
        /// <param name="pos"></param>
        /// <returns>Distance</returns>
        public override float Distance(ref Vector3 pos)
        {
            float dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count -1; i++)
            {
                Vector3 line1 = Vertices[i].Position;
                Vector3 line2 = Vertices[i+1].Position;
                float newdst = MathTools.DistanceVectorLineSegment(ref line1, ref line2, ref pos);
                if (newdst < dst)
                    dst = newdst;
            }
            return dst;
        }

        /// <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 dst = float.MaxValue;
            for (int i = 0; i < Vertices.Count / 2; i++)
            {
                Vector3 line1 = Vertices[i].Position;
                Vector3 line2 = Vertices[i + 1].Position;
                float newdst = ray.DistanceToLineSegment(ref line1, ref line2, frontOnly);
                if (newdst != float.NaN)
                {
                    if (newdst < dst)
                        dst = newdst;
                }
            }
            return dst;
        }


        /// <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)
        {
            float maxd = float.MaxValue;
            for (int i = 0; i < Count-1; i++)
            {
                float newd = obj.DistanceLineSegment(GetPosition(i), GetPosition(i + 1));
                if (newd < maxd)
                    maxd = newd;
            }
            return maxd;
        }

        /// <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 maxd = float.MaxValue;
            for (int i = 0; i < Count-1; i++)
            {
                Vector3 l1 = GetPosition(i);
                Vector3 l2 = GetPosition(i + 1);
                float newd = MathTools.DistanceTwoLineSegments(ref l1, ref l2, ref pos1, ref pos2);
                if (newd < maxd)
                    maxd = newd;
            }
            return maxd;
        }
        /// <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 maxd = float.MaxValue;
            for (int i = 0; i < Count - 1; i++)
            {
                Vector3 l1 = GetPosition(i);
                Vector3 l2 = GetPosition(i + 1);
                float newd = MathTools.DistanceLineSegmentSphere(ref l1, ref l2, ref center, radius);
                if (newd < maxd)
                    maxd = newd;
            }
            return maxd;
        }

        /// <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 maxd = float.MaxValue;
            for (int i = 0; i < Count - 1; i++)
            {
                Vector3 l1 = GetPosition(i);
                Vector3 l2 = GetPosition(i + 1);
                float newd = MathTools.DistanceLineSegmentTriangle(ref l1, ref l2, ref pos1, ref pos2, ref pos3);
                if (newd < maxd)
                    maxd = newd;
            }
            return maxd;
        }

        /// <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)
        {

            for (int i = 0; i < Vertices.Count -1; i++)
            {
                if (MathTools.PointOnLineSegment(Vertices[i].Position, Vertices[i +1].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 point;
            bool rval = false;

            for (int i = 0; i < Vertices.Count - 1; i++)
            {
                if (MathTools.IntersectionRayLineSegment(obj.Origin, obj.Direction, Vertices[i].Position, Vertices[i + 1].Position, out dist, out point))
                {
                    if (dist < inters.Distance)
                    {
                        inters.Distance = dist;
                        inters.IntersectionPoint = point;
                        inters.GeoObject = this;
                        rval = true;
                    }
                }
            }
            return rval;
        }

        /// <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 < this.Vertices.Count-1; i++)
            {
                Vector3 me0 = Vertices[i].Position;
                Vector3 me1 = Vertices[i+1].Position;

                if (MathTools.IntersectionSegmentCapsule(ref me0, ref me1, 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 Conversions

        /// <summary>
        /// Prefers line list
        /// </summary>
        public override PrimitiveTypes PreferredPrimitive
        {
            get { return PrimitiveTypes.PolyLine; }
        }

        /// <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">
        /// 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)
        {
            //return new LineList(this.Vertices);
            IVertexArray varray = VertexFactory.CreateArray(VertexType, (Vertices.Count - 1) * 2);
            int idx = 0;
            for (int i = 0; i < Vertices.Count - 1; i++)
            {
                varray[idx] = VertexFactory.CopyVertex(Vertices[i], VertexType);
                varray[idx + 1] = VertexFactory.CopyVertex(Vertices[i+1], VertexType);
                idx += 2;
            }
            return new LineList(varray);
        }

        /// <summary>
        /// Can't convert line to triangle, returns null.
        /// </summary>
        /// <param name="VertexType">unused</param>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        public override TriangleList ToTriangles(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }


        /// <summary>
        /// Can't convert line to triangle, returns null.
        /// </summary>
        /// <param name="VertexType">unused</param>
        /// <param name="quality">unused</param>
        /// <param name="vertexlimit">unused</param>
        public override IndexedMesh ToIndexedMesh(Type VertexType, float quality = 0, int vertexlimit = 0)
        {
            return null;
        }

        #endregion
    }

}
