add FloatTools
MotionPlanner: use doubles for speed and acceleration, and do computations in doubles