small changes in mergewaypoints.cpp
Merge branch 'cuong' of https://github.com/rdiankov/openrave into cuong