Make variables local
Reduce calls to sin and cos
Fix cosmetics