Updated math for logMap routines when close to theta=pi (singularity)
Updated math for logMap routines when close to theta=pi (singularity)

This math is a little cleaner and should give better numerical results (it doesn't rely on as many sqrt() calculations)
OpenFAST/OpenFAST,OpenFAST/OpenFAST,OpenFAST/OpenFAST