Faster calculation

Avoid recalculating expensive calls such as cos,sin and divisions.