protected List <Vector4D> FindRefinedPath(MyNavigationTriangle start, MyNavigationTriangle end, ref Vector3 startPoint, ref Vector3 endPoint)
        {
            MyPath <MyNavigationPrimitive> triPath = FindPath(start, end);

            if (triPath == null)
            {
                return(null);
            }

            // Path made of triangle centers
            //List<Vector3> path = new List<Vector3>();
            //path.Add(startPoint);
            //for (int j = 1; j < triPath.Count - 1; ++j)
            //{
            //    path.Add((triPath[j].Vertex as MyNavigationTriangle).Center);
            //}
            //path.Add(endPoint);
            //m_path2 = path;
            //return path;

            // Refined path smoothed by the funnel algorithm
            List <Vector4D> refinedPath = new List <Vector4D>();

            refinedPath.Add(new Vector4D(startPoint, 1.0f));

            Funnel funnel = new Funnel();

            funnel.Calculate(triPath, refinedPath, ref startPoint, ref endPoint, 0, triPath.Count - 1);

            m_path.Clear();
            foreach (var p in refinedPath)
            {
                m_path.Add(new Vector3D(p));
            }

            return(refinedPath);
        }
        // Output is Vector4D, because the first three coords specify path node center and the fourth defines the radius
        public void RefinePath(MyPath <MyNavigationPrimitive> path, List <Vector4D> output, ref Vector3 startPoint, ref Vector3 endPoint, int begin, int end)
        {
            Funnel funnel = new Funnel();

            funnel.Calculate(path, output, ref startPoint, ref endPoint, begin, end);
        }