root/core/gps_math.c

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. fac
  2. arctan
  3. arctan2
  4. floor
  5. ceil
  6. Round
  7. min
  8. formatDouble
  9. regressionInit
  10. regressionReset
  11. regressionAdd
  12. regressionCompute
  13. regressionActual
  14. regressionForecast
  15. regressionReverse
  16. regressionChange
  17. regressionQuality
  18. _sin
  19. _cos
  20. sin
  21. cos

   1 #ifndef __CHDK_GPS_MATH_H
   2 #define __CHDK_GPS_MATH_H
   3 
   4 #include "camera_info.h"
   5 #include "stdlib.h"
   6 #include "math.h"
   7 #include "gps_math.h"
   8 
   9 int fac(int n){
  10 
  11     int i;
  12     int f=1;
  13     for (i=2; i<=n; i++) f*=i;
  14     return(f);
  15 }
  16 
  17 double arctan(double x, int n){
  18 
  19     double result=0.0;
  20     int sign;
  21     int i;
  22 
  23     if (abs(x) < 1 )
  24     {
  25         result = x;
  26         sign = 0;
  27         i = 3;
  28 
  29         do
  30         {
  31             sign ^= 1;
  32             result += ((pow(x, i)/i) * ((sign) ? -1.0 : +1.0));
  33             i+=2;
  34         } while( (--n) ); // as long as member number is not zero
  35     }
  36 
  37     if (abs(x) >= 1 )
  38     {
  39         if (x<-1) {result = 3.141592653589793* -0.5; }
  40         if (x>=1) {result = 3.141592653589793*0.5; }
  41         sign = 0;
  42         i = 1;
  43 
  44         do
  45         {
  46             sign ^= 1;
  47             result += ((1/(i*pow(x,i))) * ((sign) ? -1.0 : +1.0));
  48             i+=2;
  49         } while( (--n) ); // as long as member number is not zero
  50 
  51     }
  52     return result;
  53 }
  54 
  55 double arctan2(double y, double x){
  56     double result;
  57     result = 2.0 * arctan((y / (sqrt(x*x+y*y)+x)),30);
  58     return result;
  59 }
  60 
  61 
  62 /*----------------------------------------------------------
  63 **  get first integer less than or equal to the argument
  64 *---------------------------------------------------------*/
  65 double floor (double arg) {
  66     long long trunc = (double) (long long) arg;
  67     return arg>=0 || trunc==arg ? trunc : trunc - 1.0;
  68 }
  69 
  70 /*----------------------------------------------------------
  71 **  get firstinteger greater thanthe argument
  72 *---------------------------------------------------------*/
  73 double ceil (double arg) {
  74     long long trunc = (double) (long long) arg;
  75     return arg<=0 || trunc==arg ? trunc : trunc + 1.0;
  76 }
  77 
  78 #define MAXDIGITS  (18)
  79 #define quad    long long
  80 
  81 #undef  USE_LLI
  82 
  83 static double doubleExpTable [MAXDIGITS+1] = {
  84     1.0,
  85     10.0,
  86     100.0,
  87     1000.0,
  88     10000.0,
  89     100000.0,
  90     1000000.0,
  91     10000000.0,
  92     100000000.0,
  93     1000000000.0,
  94     10000000000.0,
  95     100000000000.0,
  96     1000000000000.0,
  97     10000000000000.0,
  98     100000000000000.0,
  99     1000000000000000.0,
 100     10000000000000000.0,
 101     100000000000000000.0,
 102     1000000000000000000.0
 103 };
 104 
 105 double Round(double number, int places){
 106     return floor(number * doubleExpTable[places] + 0.5) / doubleExpTable[places];
 107 }
 108 
 109 static quad quadExpTable[MAXDIGITS+1] = {
 110     1LL,
 111     10LL,
 112     100LL,
 113     1000LL,
 114     10000LL,
 115     100000LL,
 116     1000000LL,
 117     10000000LL,
 118     100000000LL,
 119     1000000000LL,
 120     10000000000LL,
 121     100000000000LL,
 122     1000000000000LL,
 123     10000000000000LL,
 124     100000000000000LL,
 125     1000000000000000LL,
 126     10000000000000000LL,
 127     100000000000000000LL,
 128     1000000000000000000LL
 129 };
 130 
 131 static t_format_result staticBuffer;
 132 
 133 static int min (int a, int b) {
 134     return a<b ? a : b;
 135 }
 136 
 137 const char* formatDouble (t_format_result result, double value, unsigned length, unsigned fractDigits) {
 138 
 139     char* sign = "";
 140     quad expanded;
 141     quad integer, fract;
 142     unsigned integerDigits;
 143     char* p;
 144     int shift = 0;
 145     int origLength = length;
 146 
 147 #ifndef USE_LLI
 148     char fractbuffer [20];
 149 #endif
 150 
 151     /*-----------------------
 152     *   default buffer
 153     *----------------------*/
 154 
 155     if (!result) result = staticBuffer;
 156 
 157     /*-----------------------
 158     *   sanity: length
 159     *----------------------*/
 160 
 161     if (length ==0          ) length = sizeof(t_format_result)-1;
 162     if (length < 3          ) length = 3;
 163     if (length > sizeof(t_format_result)-1) length = sizeof(t_format_result)-1;
 164 
 165     /*-----------------------
 166     *   sanity: fract
 167     *----------------------*/
 168 
 169     if (fractDigits > MAXDIGITS) fractDigits = MAXDIGITS;
 170     if (fractDigits > length-3 ) fractDigits = length - 3;
 171     integerDigits = length - fractDigits - 1;
 172 
 173     /*-----------------------
 174     *   abs
 175     *----------------------*/
 176 
 177     if (value < 0) {
 178         value = -value;
 179         sign  = "-";
 180     }
 181 
 182     /*-----------------------
 183     *   scale
 184     *----------------------*/
 185 
 186     value = value * (double) quadExpTable[fractDigits] + 0.5;
 187 
 188     while (value >= 9223372036854775807.0 && shift < fractDigits) {
 189         shift++;
 190         value *= 0.1;
 191     }
 192 
 193     /*-----------------------
 194     *   general overflow?
 195     *----------------------*/
 196 
 197     if (value >= 9223372036854775807.0) {
 198         sprintf (result, "%*s", length, "***");
 199         return result;
 200     }
 201 
 202     /*-----------------------
 203     *   int+fract
 204     *----------------------*/
 205 
 206     expanded = (quad) value;
 207     integer = expanded / quadExpTable[fractDigits-shift];
 208     fract   = expanded % quadExpTable[fractDigits-shift] * quadExpTable[shift];
 209 
 210     /*-----------------------
 211     *   fixed overflow
 212     *----------------------*/
 213 
 214     if (integer >= quadExpTable[min(*sign ? integerDigits-1 : integerDigits, MAXDIGITS)]) {
 215         sprintf (result, "%*s", length, "***");
 216         return result;
 217     }
 218 
 219     /*-----------------------
 220     *   format
 221     *----------------------*/
 222 
 223 #ifdef USE_LLI
 224     if (!origLength) {
 225         sprintf (result, "%s%lli.%0*lli", sign, integer, fractDigits, fract);
 226         return result;
 227     }
 228     sprintf (result, "%*lli.%0*lli", integerDigits, integer, fractDigits, fract);
 229 #else
 230     if (fractDigits >= 10) {
 231         sprintf (fractbuffer, "%0*li%09li", fractDigits-9,
 232                  (long) (fract/1000000000),
 233                  (long) (fract%1000000000));
 234     } else {
 235         sprintf (fractbuffer, "%0*li", fractDigits, (long) fract);
 236     }
 237 
 238     if (integer >= 1000000000ll) {
 239         if (!origLength) {
 240             sprintf (result, "%s%li%09li.%s", sign,
 241                      (long) (integer/1000000000),
 242                      (long) (integer%1000000000),
 243                      fractbuffer);
 244             return result;
 245         }
 246         sprintf (result, "%*li%09li.%s", integerDigits-9,
 247                  (long) (integer/1000000000),
 248                  (long) (integer%1000000000),
 249                  fractbuffer);
 250     } else {
 251         if (!origLength) {
 252             sprintf (result, "%s%li.%s", sign, (long) integer, fractbuffer);
 253             return result;
 254         }
 255         sprintf (result, "%*li.%s", integerDigits, (long) integer, fractbuffer);
 256     }
 257 #endif
 258 
 259     if (*sign) {
 260         p = result;
 261         while (p[1]==' ') p++;
 262         p[0] = *sign;
 263     }
 264 
 265     return result;
 266 }
 267 
 268 
 269 /*-----------------------------------------------------------------------------------
 270 *      Initialize the control block
 271 *----------------------------------------------------------------------------------*/
 272 
 273 void regressionInit (t_regression *rcb, int size, t_regression_xy buffer[]) {
 274     rcb->size       = size;
 275     rcb->values     = buffer;
 276     regressionReset (rcb);
 277 }
 278 
 279 /*-----------------------------------------------------------------------------------
 280 *       Reset the control block
 281 *----------------------------------------------------------------------------------*/
 282 
 283 void regressionReset (t_regression *rcb) {
 284     int i;
 285     rcb->n          = 0;
 286     rcb->sx         = 0.0;
 287     rcb->sy         = 0.0;
 288     rcb->sxx        = 0.0;
 289     rcb->sxy        = 0.0;
 290     rcb->last_x     = 0.0;
 291     rcb->valid      = 0;
 292     rcb->s          = 0.0;
 293     rcb->t          = 0.0;
 294     rcb->index      = 0;
 295     for (i=0; i<rcb->size; i++) {
 296         rcb->values[i].x = 0;
 297         rcb->values[i].y = 0;
 298     }
 299 }
 300 
 301 /*-----------------------------------------------------------------------------------
 302 *       Incorporate new data pair
 303 *----------------------------------------------------------------------------------*/
 304 
 305 void regressionAdd (t_regression *rcb, double x, double y) {
 306 
 307     /* remove old value */
 308 
 309     if (rcb->n >= rcb->size) {
 310         double old_x   = rcb->values[rcb->index].x;
 311         double old_y   = rcb->values[rcb->index].y;
 312         rcb->values[rcb->index].x = 0;
 313         rcb->values[rcb->index].y = 0;
 314         rcb->n          -= 1;
 315         rcb->sx         -= old_x;
 316         rcb->sy         -= old_y;
 317         rcb->sxx        -= old_x*old_x;
 318         rcb->sxy        -= old_x*old_y;
 319         rcb->valid      = 0;
 320         rcb->s          = 0.0;
 321         rcb->t          = 0.0;
 322     }
 323 
 324     if (rcb->n < rcb->size) {
 325         rcb->n          += 1;
 326         rcb->sx         += x;
 327         rcb->sy         += y;
 328         rcb->sxx        += x*x;
 329         rcb->sxy        += x*y;
 330         rcb->valid      = 0;
 331         rcb->s          = 0.0;
 332         rcb->t          = 0.0;
 333         rcb->last_x     = x;
 334         rcb->values[rcb->index].x = x;
 335         rcb->values[rcb->index].y = y;
 336         rcb->index = (rcb->index+1) % rcb->size;
 337     }
 338 }
 339 
 340 /*-----------------------------------------------------------------------------------
 341 *       calculate regression
 342 *----------------------------------------------------------------------------------*/
 343 
 344 static void regressionCompute (t_regression *rcb) {
 345 
 346     double det;
 347 
 348     if (rcb->valid) return;
 349     rcb->valid = -1;
 350 
 351     det = rcb->n*rcb->sxx - rcb->sx*rcb->sx;
 352 
 353     /* TODO: possibly epsilon to abs (det)  */
 354     if (det==0.0) return;
 355 
 356     rcb->s = (rcb->n * rcb->sxy - rcb->sx * rcb->sy ) / det;
 357     rcb->t = (rcb->sxx*rcb->sy  - rcb->sx * rcb->sxy) / det;
 358     rcb->valid = 1;
 359 }
 360 
 361 /*-----------------------------------------------------------------------------------
 362 *      Over regression calculated is y - value for the load value x
 363 *----------------------------------------------------------------------------------*/
 364 
 365 double regressionActual (t_regression *rcb) {
 366     regressionCompute (rcb);
 367     return rcb->last_x * rcb->s + rcb->t;
 368 }
 369 
 370 /*-----------------------------------------------------------------------------------
 371 *       Over regression calculated is y - value for givenName x value
 372 *----------------------------------------------------------------------------------*/
 373 
 374 double regressionForecast (t_regression *rcb, double x) {
 375     regressionCompute (rcb);
 376     return x * rcb->s + rcb->t;
 377 }
 378 
 379 /*-----------------------------------------------------------------------------------
 380 *       Over regression calculated is x value for y value
 381 *----------------------------------------------------------------------------------*/
 382 
 383 double regressionReverse (t_regression *rcb, double y) {
 384     regressionCompute (rcb);
 385     return rcb->s != 0.0 ? (y - rcb->t) / rcb->s : 1e9;
 386 }
 387 
 388 /*-----------------------------------------------------------------------------------
 389 *       Slope of the regression line, then? Change of y at? Change of xt
 390 *----------------------------------------------------------------------------------*/
 391 
 392 double regressionChange (t_regression *rcb) {
 393     regressionCompute (rcb);
 394     return rcb->s;
 395 }
 396 
 397 /*-----------------------------------------------------------------------------------
 398 *   Quality of Regression (0..1)
 399 *   (simplified implementation, returns 1 if available regression, 0 otherwise)
 400 *   (full implementation on backorder)
 401 *----------------------------------------------------------------------------------*/
 402 
 403 double regressionQuality (t_regression *rcb) {
 404     regressionCompute (rcb);
 405     return rcb->valid==1 ? 1.0 : 0.0;
 406 }
 407 
 408 extern double floor (double arg);
 409 extern double sqrt (double arg);
 410 
 411 /*---------------------------------------------------------------
 412 **  Toolbox with Restricted Range
 413 **  -pi/4 .. +pi/4
 414 **-------------------------------------------------------------*/
 415 
 416 static double _sin (double arg) {
 417     double sum     = arg;
 418     double msqarg  = -arg*arg;
 419     double term    = arg;
 420     int i;
 421 
 422     for (i=2; i<=14; i+=2) {   // accuracy reverse order here!
 423         term *= msqarg / (i*(i+1));
 424         sum  += term;
 425     }
 426     return sum;
 427 }
 428 
 429 static double _cos (double arg) {
 430     double s = _sin (arg);
 431     return sqrt (1.0 - s*s);
 432 }
 433 
 434 /*---------------------------------------------------------------
 435 ** has 52 bit double precision,
 436 **  therefore is Pi with 16 decimal points
 437 **-------------------------------------------------------------*/
 438 
 439 #define PIBY2   (3.141592653589793 / 2.0)
 440 
 441 /*---------------------------------------------------------------
 442 **  Functions with range approximately -10^9 to +10^9
 443 **-------------------------------------------------------------*/
 444 
 445 double sin (double phi) {
 446     long periode = (long) floor (phi / PIBY2 + 0.5);
 447     double reduced = phi - PIBY2 * periode;
 448     switch (periode&3) {
 449     default:
 450         return _sin (reduced);
 451     case 1:
 452         return _cos (reduced);
 453     case 2:
 454         return -_sin (reduced);
 455     case 3:
 456         return -_cos (reduced);
 457     }
 458 }
 459 
 460 double cos (double phi) {
 461     long periode = floor (phi / PIBY2 + 0.125);
 462     double reduced = phi - PIBY2 * periode;
 463     switch (periode&3) {
 464     default:
 465         return _cos (reduced);
 466     case 1:
 467         return -_sin (reduced);
 468     case 2:
 469         return -_cos (reduced);
 470     case 3:
 471         return _sin (reduced);
 472     }
 473 }
 474 
 475 #endif

/* [<][>][^][v][top][bottom][index][help] */