Path smoothing while path following

I can't find a nice way to do path smoothing while i'm following a path.

I have tried this piece of code but aparently it doesn't work as suspected. The idea of how it should work is:

• Edge 1 (the first location of the path)
• Edge 2 (the 2nd location of the path)
• while edge2 is not the last edge in the path, step through the edges checking to see if the agent can move without obstruction from the source node of edge1 to the destination node of edge2. If the agent can move between those positions then the two edges are replaced with a single edge.

public static LinkedList<Vector2> SmoothPathEdgesQuick(LinkedList<Vector2> path, Player player)
{
List<Vector2> toBeDeleted = new List<Vector2>();

var edge1 = path.First;
var edge2 = path.First.Next;
while (edge2 != null && edge2.Next != null)
{
if (player.CanWalkBetween(edge1.Value, edge2.Next.Value))
{
edge1 = edge2.Next;
if (edge1.Next == null)
{
break;
}
else
{
edge2 = edge1.Next;
}
}
else
{
edge1 = edge2;
edge2 = edge2.Next;
}
}
if (edge1.Next != null)
{
if (edge2 != null && player.CanWalkBetween(edge1.Value, edge2.Value))
{
}
}
foreach (var vector2 in path)
{
if (toBeDeleted.Contains(vector2))
continue;
}
}


As when i go from 250,400 to 500,400 (which is just a line) it should get just 2 waypoints (so the newLinkedList should just have a count of 2). But instead of 2 it does gets more waypoints, which is strange as the example in the book doesn't get more than 2 waypoints for a straight line.

I've used the code below which is provided by the the book Programming Game AI by Example. And based my code on the logic in there.

void Raven_PathPlanner::SmoothPathEdgesQuick(Path& path)
{
//create a couple of iterators and point them at the front of the path
Path::iterator e1(path.begin()), e2(path.begin());

//increment e2 so it points to the edge following e1.
++e2;

//while e2 is not the last edge in the path, step through the edges checking
//to see if the agent can move without obstruction from the source node of
//e1 to the destination node of e2. If the agent can move between those
//positions then the two edges are replaced with a single edge.
while (e2 != path.end())
{
//check for obstruction, adjust and remove the edges accordingly
if ( (e2->Behavior() == EdgeType::normal) &&
m_pOwner->canWalkBetween(e1->Source(), e2->Destination()) )
{
e1->SetDestination(e2->Destination());
e2 = path.erase(e2);
}

else
{
e1 = e2;
++e2;
}
}
}


Your code only makes a single test with edge1 before advancing. This skips over certain nodes that are in a straight line. For example, if you have 5 points (A->B->C->D->E) all in a straight. The smoothed path should only contain two points (A->E). In your logic you test if you can travel between A->C and if you can then B is eliminated. You then check to see if you can get from C->E, and if you can then D is eliminated. Here you reach the end of the path and you are left with an extra waypoint (A->C->E).

Because you are moving your edge1 further down the path at ever iteration you are not eliminating all possible waypoints. You should check to see if you can walk between edge1.value and edge2.next.value, if so, mark edge2 for deletion and advance edge2 to edge2.next. If you cannot walk between then advance edge1 and edge2.

while (edge2 != null && edge2.Next != null)
{
if (player.CanWalkBetween(edge1.Value, edge2.Next.Value))
{