Maths Code Refactoring

This pull request represents the completion of the first deliverable of my Google Summer of Code project.
I managed to refactor all the maths code in a c++ style while mantaining the same level of performance.
If you have any questions or doubts please ask.
Turns out it was an ordinary matrix multiplication but since some of the code had a matrix transposed the code seemed to do something different at the time. I removed the comment as I fixed everything now.