Started to clean up math, kdtree and simulation.
Merged