FFRRT*

Implemented optimizations on collisions, and introduced heuristics to obtain massive speed boosts over the original algorithm (>1000x). Also extended the algorithm to 3D. Implemented a dynamic smoothing algorithm allowing for fast path replanning. Currently drafting the paper to be submitted soon to an IEEE journal.