Improved accuracy of diagonal moves by oversampling the path discretization. Accelerated the planner by rewriting time critical routines from floating point to fixed point arithmetics.