This source file includes following definitions.
- fac
- arctan
- arctan2
- floor
- ceil
- Round
- min
- formatDouble
- regressionInit
- regressionReset
- regressionAdd
- regressionCompute
- regressionActual
- regressionForecast
- regressionReverse
- regressionChange
- regressionQuality
- _sin
- _cos
- sin
- cos
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) );
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) );
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
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
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)
78 #define quad long long
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
108 static quad quadExpTable[MAXDIGITS+1] = {
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 = "";
139 quad expanded;
140 quad integer, fract;
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
152
153
154 if (!result) result = staticBuffer;
155
156
157
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
166
167
168 if (fractDigits > MAXDIGITS) fractDigits = MAXDIGITS;
169 if (fractDigits > length-3 ) fractDigits = length - 3;
170 integerDigits = length - fractDigits - 1;
171
172
173
174
175
176 if (value < 0) {
177 value = -value;
178 sign = "-";
179 }
180
181
182
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
194
195
196 if (value >= 9223372036854775807.0) {
197 sprintf (result, "%*s", length, "***");
198 return result;
199 }
200
201
202
203
204
205 expanded = (quad) value;
206 integer = expanded / quadExpTable[fractDigits-shift];
207 fract = expanded % quadExpTable[fractDigits-shift] * quadExpTable[shift];
208
209
210
211
212
213 if (integer >= quadExpTable[min(*sign ? integerDigits-1 : integerDigits, MAXDIGITS)]) {
214 sprintf (result, "%*s", length, "***");
215 return result;
216 }
217
218
219
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
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
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
302
303
304 void regressionAdd (t_regression *rcb, double x, double y) {
305
306
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
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
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
362
363
364 double regressionActual (t_regression *rcb) {
365 regressionCompute (rcb);
366 return rcb->last_x * rcb->s + rcb->t;
367 }
368
369
370
371
372
373 double regressionForecast (t_regression *rcb, double x) {
374 regressionCompute (rcb);
375 return x * rcb->s + rcb->t;
376 }
377
378
379
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
389
390
391 double regressionChange (t_regression *rcb) {
392 regressionCompute (rcb);
393 return rcb->s;
394 }
395
396
397
398
399
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
412
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) {
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
435
436
437
438 #define PIBY2 (3.141592653589793 / 2.0)
439
440
441
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