Wiselib: wiselib.testing/util/standalone

Wiselib
00001 // vim: set noexpandtab ts=4 sw=4:
00002 
00003 #ifndef STANDALONE_MATH_H
00004 #define STANDALONE_MATH_H
00005 
00006 #include <stdint.h>
00007 
00008 namespace wiselib {
00009 
00010 template<typename OsModel_P>
00011 class StandaloneMath {
00012    public:
00013       typedef OsModel_P OsModel;
00014       typedef double real_t;
00015       typedef int32_t integer_t;
00016       
00017       static const real_t PI, PI_2, PI_3, PI_4, PI_6;
00018       
00019       static real_t sqrt(real_t);
00020       
00021       // ---- Trigonometry
00022       
00023       static real_t radians_to_degrees(real_t);
00024       static real_t degrees_to_radians(real_t);
00025       
00026       static real_t sin(real_t);
00027       static real_t cos(real_t);
00028       static real_t tan(real_t);
00029       
00030       static real_t asin(real_t);
00031       static real_t acos(real_t);
00032       
00033       // ---- Rounding & signs
00034       
00035       static real_t fabs(real_t);
00036       
00037       template<typename T>
00038       static T sgn(T x) {
00039          if(x > 0) { return 1; }
00040          else if(x < 0) { return -1; }
00041          return 0;
00042       }
00043       
00045       static integer_t trunc(real_t);
00047       static integer_t floor(real_t);
00049       static integer_t ceil(real_t);
00051       static integer_t round(real_t);
00052       
00053    private:
00054       static real_t sin_degrees(integer_t);
00055       static real_t cos_degrees(integer_t);
00056       static integer_t asin_degrees(real_t);
00057       
00058       static real_t sqrt_eps(real_t, real_t);
00059       
00060       static const real_t sin_table_[91];
00061 };
00062 
00063 template<typename OsModel_P>
00064 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::PI = 3.14159265358979323846; /* pi */
00065 
00066 template<typename OsModel_P>
00067 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::PI_2 = 1.57079632679489661923; /* pi/2 */
00068 
00069 template<typename OsModel_P>
00070 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::PI_3 = 1.04719755119659763132; /* pi/3 */
00071 
00072 template<typename OsModel_P>
00073 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::PI_4 = 0.78539816339744830962; /* pi/4 */
00074 
00075 template<typename OsModel_P>
00076 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::PI_6 = 0.52359877559829881566; /* pi/6 */
00077 
00078 template<typename OsModel_P>
00079 const typename StandaloneMath<OsModel_P>::real_t StandaloneMath<OsModel_P>::sin_table_[91] = {
00080    0.000000000000000000000000000000,
00081    0.017452406437283511653202339176,
00082    0.034899496702500969191884649945,
00083    0.052335956242943834637593170100,
00084    0.069756473744125302438590097154,
00085    0.087155742747658165869850677154,
00086    0.104528463267653470847307062286,
00087    0.121869343405147476100403025612,
00088    0.139173100960065437847745783984,
00089    0.156434465040230868959625354364,
00090    0.173648177666930331186634361984,
00091    0.190808995376544804356555573577,
00092    0.207911690817759342575499204031,
00093    0.224951054343865003426472526371,
00094    0.241921895599667730047954705697,
00095    0.258819045102520739476403832668,
00096    0.275637355816999163327096766807,
00097    0.292371704722736769355151409400,
00098    0.309016994374947395751718204338,
00099    0.325568154457156699876918537484,
00100    0.342020143325668712908083080038,
00101    0.358367949545300268354708350671,
00102    0.374606593415912014766178117497,
00103    0.390731128489273771275946955939,
00104    0.406736643075800208269043878317,
00105    0.422618261740699441286750470681,
00106    0.438371146789077403838064128649,
00107    0.453990499739546748969587497413,
00108    0.469471562785890805802324621254,
00109    0.484809620246337058535601727272,
00110    0.499999999999999944488848768742,
00111    0.515038074910054155530758634995,
00112    0.529919264233204900804707904172,
00113    0.544639035015027084263294909761,
00114    0.559192903470746904837085367035,
00115    0.573576436351046048400803556433,
00116    0.587785252292473137103456792829,
00117    0.601815023152048267363056766044,
00118    0.615661475325658291701813595864,
00119    0.629320391049837390973209494405,
00120    0.642787609686539251896419955301,
00121    0.656059028990507275835852851742,
00122    0.669130606358858237570075289113,
00123    0.681998360062498476530379321048,
00124    0.694658370458997254104360763449,
00125    0.707106781186547461715008466854,
00126    0.719339800338651080835461470997,
00127    0.731353701619170459835572728480,
00128    0.743144825477394244117590460519,
00129    0.754709580222772014046483946004,
00130    0.766044443118978013451680908474,
00131    0.777145961456970901792828954058,
00132    0.788010753606722014197316639184,
00133    0.798635510047292829227671973058,
00134    0.809016994374947451262869435595,
00135    0.819152044288991798559607104835,
00136    0.829037572555041735178349426860,
00137    0.838670567945424050293468098971,
00138    0.848048096156425956770874563517,
00139    0.857167300702112333610216410307,
00140    0.866025403784438596588302061718,
00141    0.874619707139395741180010190874,
00142    0.882947592858926877390501886111,
00143    0.891006524188367787786546614370,
00144    0.898794046299167037616939524014,
00145    0.906307787036649936673882166360,
00146    0.913545457642600866599025266623,
00147    0.920504853452440374717014037742,
00148    0.927183854566787424289486807538,
00149    0.933580426497201742996878692793,
00150    0.939692620785908316882739654829,
00151    0.945518575599316735136312672694,
00152    0.951056516295153531181938433292,
00153    0.956304755963035435506469639222,
00154    0.961261695938318894150143023580,
00155    0.965925826289068312213714762038,
00156    0.970295726275996472942608761514,
00157    0.974370064785235245885530730447,
00158    0.978147600733805577810642262193,
00159    0.981627183447663975712771389226,
00160    0.984807753012208020315654266597,
00161    0.987688340595137770350220307591,
00162    0.990268068741570361979142944620,
00163    0.992546151641321983127852490725,
00164    0.994521895368273289861349439889,
00165    0.996194698091745545198705258372,
00166    0.997564050259824197652847033169,
00167    0.998629534754573833232882407174,
00168    0.999390827019095762118183756684,
00169    1.000000000000000000000000000000
00170 };
00171 
00172 template<typename OsModel_P>
00173 typename StandaloneMath<OsModel_P>::real_t
00174 StandaloneMath<OsModel_P>::
00175 fabs(typename StandaloneMath<OsModel_P>::real_t x) {
00176    if(x < 0) { return -x; }
00177    return x;
00178 }
00179 
00180 template<typename OsModel_P>
00181 typename StandaloneMath<OsModel_P>::real_t
00182 StandaloneMath<OsModel_P>::
00183 sqrt_eps(typename StandaloneMath<OsModel_P>::real_t x, typename StandaloneMath<OsModel_P>::real_t epsilon) {
00184    //static const integer_t max_iterations = 1000;
00185    typename StandaloneMath<OsModel_P>::integer_t iterations = 0;
00186    
00187    if(x < 0) { // || x <= (2.0*epsilon*epsilon)) {
00188       return -1.0;
00189    }
00190    
00191    typename StandaloneMath<OsModel_P>::real_t y = x;
00192    while(fabs((y*y) - x) > epsilon /*&& iterations < max_iterations*/) {
00193       y = (y + (x / y)) / 2.0;
00194       iterations++;
00195    }
00196    return y;
00197 }
00198 
00199 template<typename OsModel_P>
00200 typename StandaloneMath<OsModel_P>::real_t
00201 StandaloneMath<OsModel_P>::
00202 sqrt(typename StandaloneMath<OsModel_P>::real_t x) {
00203    return sqrt_eps(x, 0.00000001);
00204 }
00205 
00206 // Degrees <-> Radians conversion
00207 template<typename OsModel_P>
00208 typename StandaloneMath<OsModel_P>::real_t
00209 StandaloneMath<OsModel_P>::
00210 radians_to_degrees(typename StandaloneMath<OsModel_P>::real_t x) { return 180.0 * x / PI; }
00211 
00212 template<typename OsModel_P>
00213 typename StandaloneMath<OsModel_P>::real_t
00214 StandaloneMath<OsModel_P>::
00215 degrees_to_radians(typename StandaloneMath<OsModel_P>::real_t x) { return PI * x / 180.0; }
00216 
00217 // Basic trigonometric functions
00218 template<typename OsModel_P>
00219 typename StandaloneMath<OsModel_P>::real_t
00220 StandaloneMath<OsModel_P>::
00221 sin(typename StandaloneMath<OsModel_P>::real_t x) {
00222    return sin_degrees(round(radians_to_degrees(x)));
00223 }
00224 
00225 template<typename OsModel_P>
00226 typename StandaloneMath<OsModel_P>::real_t
00227 StandaloneMath<OsModel_P>::
00228 cos(typename StandaloneMath<OsModel_P>::real_t x) {
00229    return sin(x + PI_2);
00230 }
00231 
00232 template<typename OsModel_P>
00233 typename StandaloneMath<OsModel_P>::real_t
00234 StandaloneMath<OsModel_P>::
00235 tan(typename StandaloneMath<OsModel_P>::real_t x) {
00236    return sin(x) / cos(x);
00237 }
00238 
00239 template<typename OsModel_P>
00240 typename StandaloneMath<OsModel_P>::real_t
00241 StandaloneMath<OsModel_P>::
00242 sin_degrees(typename StandaloneMath<OsModel_P>::integer_t x) {
00243    int y = x % 360;
00244    if(y < 0) { y += 360; }
00245    
00246    if(y < 90) { return sin_table_[y]; }
00247    else if(y < 180) { return sin_table_[180 - y]; }
00248    else if(y < 270) { return -sin_table_[y - 180]; }
00249    else { return -sin_table_[360 - y]; }
00250 }
00251 
00252 template<typename OsModel_P>
00253 typename StandaloneMath<OsModel_P>::real_t
00254 StandaloneMath<OsModel_P>::
00255 cos_degrees(typename StandaloneMath<OsModel_P>::integer_t x) {
00256    return sin_degrees(x + 90);
00257 }
00258 
00259 template<typename OsModel_P>
00260 typename StandaloneMath<OsModel_P>::real_t
00261 StandaloneMath<OsModel_P>::
00262 asin(typename StandaloneMath<OsModel_P>::real_t x) {
00263    return degrees_to_radians(asin_degrees(x));
00264 }
00265 
00266 template<typename OsModel_P>
00267 typename StandaloneMath<OsModel_P>::real_t
00268 StandaloneMath<OsModel_P>::
00269 acos(typename StandaloneMath<OsModel_P>::real_t x) {
00270    return degrees_to_radians(180 + asin_degrees(-x));
00271 }
00272 
00273 template<typename OsModel_P>
00274 typename StandaloneMath<OsModel_P>::integer_t
00275 StandaloneMath<OsModel_P>::
00276 asin_degrees(typename StandaloneMath<OsModel_P>::real_t x) {
00277    if(x < 0) {
00278       return -asin_degrees(-x);
00279    }
00280    
00281    // sin_table_ is monotone, conduct a binary search
00282    integer_t top = 90, bottom = 0, idx = 0;
00283    while(top - bottom > 1) {
00284       idx = (top + bottom) / 2;
00285       if(sin_table_[idx] > x) {
00286          top = idx;
00287       }
00288       else {
00289          bottom = idx;
00290       }
00291    }
00292    
00293    idx = bottom;
00294    if(
00295       fabs(sin_table_[top] - x) <
00296       fabs(sin_table_[bottom] - x)
00297    ) {
00298       idx = top;
00299    }
00300    return idx;
00301 }
00302 
00303 
00304 // Rounding
00305 template<typename OsModel_P>
00306 typename StandaloneMath<OsModel_P>::integer_t
00307 StandaloneMath<OsModel_P>::
00308 trunc(typename StandaloneMath<OsModel_P>::real_t x) {
00309    return static_cast<integer_t>(x);
00310 }
00311 
00312 template<typename OsModel_P>
00313 typename StandaloneMath<OsModel_P>::integer_t
00314 StandaloneMath<OsModel_P>::
00315 floor(typename StandaloneMath<OsModel_P>::real_t x) {
00316    if(x > 0) { return trunc(x); }
00317    else {
00318       //   floor(-12.3456) = -13
00319       //   trunc(-12.3456) = -12
00320       if(static_cast<real_t>(trunc(x)) != x) {
00321          return trunc(x) - 1;
00322       }
00323       else {
00324          return trunc(x);
00325       }
00326    }
00327 }
00328 
00329 template<typename OsModel_P>
00330 typename StandaloneMath<OsModel_P>::integer_t
00331 StandaloneMath<OsModel_P>::
00332 ceil(typename StandaloneMath<OsModel_P>::real_t x) {
00333    return -floor(-x);
00334 }
00335 
00336 template<typename OsModel_P>
00337 typename StandaloneMath<OsModel_P>::integer_t
00338 StandaloneMath<OsModel_P>::
00339 round(typename StandaloneMath<OsModel_P>::real_t x) {
00340    return floor(x + 0.5);
00341 }
00342 
00343 } // namespace wiselib
00344 
00345 #endif // STANDALONE_MATH_H
00346