public int CostToEnd; //The H cost public SubWaypoint(Vector2 pos, SubWaypoint parent, Vector2 endPos) { this.Pos = pos; this.Parent = parent; this.calculateCosts(endPos); }
private void addToOpenList(SubWaypoint parent, Entity entity) { var adjacent = World.getAdjacentCoords(MathHelp.floor(parent.Pos.X), MathHelp.floor(parent.Pos.Y), false); foreach (var coord in adjacent) { if (entity.World.isWalkableExcept(MathHelp.floor(coord.X), MathHelp.floor(coord.Y), entity)) { var subWaypoint = new SubWaypoint(coord, parent, this.Goal); if (getContainedSubWaypoint(this.closedList, subWaypoint) == null) { var contained = getContainedSubWaypoint(this.openList, subWaypoint); if (contained == null) { this.openList.Add(subWaypoint); } else { if (parent.CostFromStart + 10 < contained.CostFromStart) { contained.Parent = parent; contained.calculateCosts(this.Goal); } } } } } }
private void savePath(SubWaypoint end) { var point = end; while (point != null) { if (point.Parent != null) { point.Pos = Vector2.Add(point.Pos, new Vector2(0.5F, 0.5F)); } this.pointsToGoTo.Add(point.Pos); point = point.Parent; } this.currentPointAt = this.pointsToGoTo.Count - 1; }
private SubWaypoint getLowestFFromOpenList() { SubWaypoint lowestWaypoint = null; var lowestF = int.MaxValue; foreach (var waypoint in this.openList) { var f = waypoint.CostFromStart + waypoint.CostToEnd; if (f <= lowestF) { lowestF = f; lowestWaypoint = waypoint; } } return(lowestWaypoint); }
public void updateCalcing(Vector2 startingPoint, Entity entity, Path path) { if (!this.IsCalced && !this.Failed) { if (!this.IsCurrentlyCalcing) { var start = new SubWaypoint(startingPoint, null, this.Goal); this.openList.Add(start); this.IsCurrentlyCalcing = true; } if (this.openList.Count > 0) { var lowestF = this.getLowestFFromOpenList(); if (lowestF != null) { this.openList.Remove(lowestF); this.closedList.Add(lowestF); //Path is complete if (lowestF.Pos == this.goalRounded) { this.savePath(lowestF); this.IsCalced = true; this.IsCurrentlyCalcing = false; return; } this.addToOpenList(lowestF, entity); } } else { this.Failed = true; this.IsCurrentlyCalcing = false; } } }
private static SubWaypoint getContainedSubWaypoint(IEnumerable <SubWaypoint> list, SubWaypoint waypoint) { foreach (var point in list) { if (point.Pos == waypoint.Pos) { return(point); } } return(null); }