public List <Vertex2D> findPath(Vector2 vStart, Vector2 vEnd)
{
var start = gridConnectionProvider.GetVertex(vStart);
var end = gridConnectionProvider.GetVertex(vEnd);
var wA = findClosestWaypoint(start);
var wB = findClosestWaypoint(end);
var wPath = findPathWaypoints(wA, wB);
return(findPathAlongWaypoints(start, end, wPath));
}
public void Simulate()
{
if (!first)
{
return;
}
//first = false;
grid = TW.Data.GetSingleton <NavigableGrid2DData>().Grid;
if (grid == null)
{
return;
}
var gridConnectionProvider = new GridConnectionProvider()
{
Grid = grid
};
p.ConnectionProvider = gridConnectionProvider;
var start = gridConnectionProvider.GetVertex(Data.Start / grid.NodeSize);
var goal = gridConnectionProvider.GetVertex(Data.End / grid.NodeSize);
TW.Graphics.LineManager3D.AddCenteredBox(new Vector3(start.Position.X + 0.5f, 0, start.Position.Y + 0.5f) * grid.NodeSize, grid.NodeSize * 0.5f, new Color4(0, 1, 0));
TW.Graphics.LineManager3D.AddCenteredBox(new Vector3(goal.Position.X + 0.5f, 0, goal.Position.Y + 0.5f) * grid.NodeSize, grid.NodeSize * 0.5f, new Color4(1, 0, 0));
var path = p.FindPath(start, goal);
Vertex2D prev = null;
//viz.Render();
if (path == null)
{
return;
}
foreach (var node in path)
{
if (prev != null)
{
TW.Graphics.LineManager3D.AddLine(
new Vector3(prev.Position.X + 0.5f, 0, prev.Position.Y + 0.5f) * grid.NodeSize,
new Vector3(node.Position.X + 0.5f, 0, node.Position.Y + 0.5f) * grid.NodeSize,
new Color4(1, 0, 0));
}
prev = node;
}
}
private void processWaypoints()
{
foreach (Waypoint wp in TW.Data.GetChangesOfType <Waypoint>().Where(c => c.Change == ModelChange.Added).Select(c => c.ModelObject))
{
var start = provider.GetVertex(wp.Position);
foreach (Waypoint wpTo in getWaypoints())
{
if (wpTo == wp)
{
continue;
}
var goal = provider.GetVertex(wpTo.Position);
var finder = new PathFinder2D <Vertex2D> {
ConnectionProvider = provider
};
finder.StopCondition = n => finder.GetCurrentCost(n) > WaypointDistance * 2f;
var path = finder.FindPath(start, goal);
if (path == null)
{
continue;
}
if (path.Last() != goal)
{
continue;
}
var edge = new Waypoint.Edge {
Target = wpTo, Distance = finder.GetCurrentCost(path[path.Count - 2])
};
wp.Edges.Add(edge);
}
}
}