Switched from simplify to shortcutPath.
Removed an outdated TODO.
Merge pull request #14 from personalrobotics/bugfix/Shortcutting

Switched from simplify to shortcutPath.