fixed box issue, fixed replanning collision checking, integrated naive cc