merged with fast planner lib