straight copy matlab to c++