Fixed bugs in GetMeshRouteFromPoints and started Funnel Algorithm.