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);
}