Swapped to glMatrix, optimizations

- Swapped the old Closure-based 3D math functions to glMatrix.
- Removed most of the heap allocations which lets the garbage collector
work less hard.
- Other small optimizations.