Matrix inversion function in C++