Code cleanup:
-Changed calculations to use std::pow rather than multiplying by itself
-Changed matrix multiplications to A(B-C) rather than AB-AC (reduces number of matrix multiplications)