Optimize grids removing many push_back
moved include and changed to C++ (cmath instead of math.h)
Merge branch 'optimize-grid' into v2.0

After Max validation I feel I can merge on release branch
Merge branch 'v2.0'