Matrix work

Realized the math in the Matrix functions were not written right. So I
went into those and re-did the math in each function. Should produce
proper results now. Speaking of results, changed what the functions take
in and output. The functions now take in two dimensional vectors and
output two dimensional vectors. That's a new trick that was learned
today. Not sure as of yet if it will work but from research thus far it
should function correctly. Fingers crossed it works when I start testing
as well.