Moved all relevant code from prrts.c to pRRTstar.cpp.

-- All functions as defined in prrts.c ported to pRRTstar.cpp
-- Next getPlannerData and solve need to be completed.