00001 // mdcWrapAng.h 00002 /* Handle wrap-around problems in phi: phi1 is the reference angle, and phi2 00003 is adjusted, if necessary, so that it is close to phi1. The assumption is 00004 that phi1 and phi2 are in fact supposed to be close to one another. */ 00005 00006 #ifndef MDCWRAPANG_H 00007 #define MDCWRAPANG_H 00008 00009 #include "MdcGeom/Constants.h" 00010 static const double &twpi = Constants::twoPi; 00011 00012 inline double mdcWrapAng( double phi1, double phi2) { 00013 00014 double diff; 00015 00016 diff = phi2 - phi1; 00017 if (diff > 0.5*twpi) phi2 -= twpi; 00018 else if (diff < -0.5*twpi) phi2 += twpi; 00019 00020 return phi2; 00021 } 00022 #endif