FMTX for asymptotically optimal dynamic replanning

FMTX extends the FMT* algorithm to dynamic environments by using a lazy wavefront search and a cost-improvement test. This allows the system to revisit nodes for optimal parent selection and implicitly rewire paths when obstacles change without full recalculation.