Add navigation
autocenter camera during navigation mode
separate math function
improve readability of code
Merge pull request #134 from hammer814/navi

Add navigation