Updated math section

* added quaternion math functions
* updated matrix code