Optimization of C++ numerics functions