Addition of Custom Waypoints

Now you can spell out funny words with your paths ;P
Maybe I misunderstand the logic at the moment.  But for me, it looks like "wp" contains some way points that always start at queuedMoves.waypoints.back (i. e., this is always the first entry in wp) and then continue onward.  If this is correct, I'd suggest to:

1) assert() this is the case for the first entry.
2) Get rid of the "if" in the loop.  You could even consider using std::vector::insert to insert the range at once instead of the loop.