## root/core/gps_math.c

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

## DEFINITIONS

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

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