Fixed trig error causing incorrect vector calculations