FixedPoint: simplify some code, remove some commented code
MotionPlanner: optimizations
MotionPlanner: optimization