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.
Basically my intention was to fix it, but I ended up just leaving a note because the refactoring isn't supposed to fix bugs (Maybe I should post a bug report somewhere though)