Clamp cos between -1 and 1
cos
static double acosClamp(double cos ) { if(cos>1){return 0;} else if(cos<-1){return math.pi;} else{ return math.acos(cos);} }