Update algebralgorithms.h

Fixed an issue from "is_diagonally_dominant" function. Added new function that tries to fix the matrix when it is not diagonally dominant.