Math tweaks, contains bug fix

[cheako]

Fix:

1. No accounting for movement beyond boarders, resulting in lost horizontal movement.

Optimisations:

1. Stop using degrees, doing everything in radians makes things faster at runtime.
2. Removes one Sqrt(), replanced by two power of 2.
3. Removes conditional mess, replaces with sort and array index.