Matrix Manipulations C++