change angle calc to cos calc
decrease sqrt calls