Remember that 0 degree in processing points east not north; so maybe subtract HALF_PI to the angle when using cos and sin