Addition of Custom Waypoints

Now you can spell out funny words with your paths ;P
Just very nit-picky:  I prefer to add "const" here, since wp is in fact constant.  I think it is always good to explicity state/enforce this.  The same is true for start, but for me it is more obvious that start is not modified in error anywhere (since it is just used in the line below) than it is that the "complicated" operations below don't mess up cwp vs wp or something.