Added collision (not tested) and more math utility. 
Also renamed some packages and classes.