Optimisation and simplification
More memory, but less calculations
Merge pull request #16 from Vuzi/master

Optimisation and simplification