]> git.mxchange.org Git - flightgear.git/blob - src/WeatherCM/linintp2.cpp
Don't start paused for in air starts.
[flightgear.git] / src / WeatherCM / linintp2.cpp
1 /*
2   WARNING - Do not remove this header.
3
4   This code is a templated version of the 'magic-software' spherical
5   interpolation code by Dave Eberly. The original (un-hacked) code can be
6   obtained from here: http://www.magic-software.com/gr_appr.htm
7   This code is derived from linintp2.h/cpp and sphrintp.h/cpp.
8
9   Dave Eberly says that the conditions for use are:
10
11   * You may distribute the original source code to others at no charge.
12
13   * You may modify the original source code and distribute it to others at
14     no charge. The modified code must be documented to indicate that it is
15     not part of the original package.
16
17   * You may use this code for non-commercial purposes. You may also
18     incorporate this code into commercial packages. However, you may not
19     sell any of your source code which contains my original and/or modified
20     source code. In such a case, you need to factor out my code and freely
21     distribute it.
22
23   * The original code comes with absolutely no warranty and no guarantee is
24     made that the code is bug-free.
25
26   This does not seem incompatible with GPL - so this modified version
27   is hereby placed under GPL along with the rest of FlightGear.
28
29                               Christian Mayer
30 */
31
32 #include <simgear/compiler.h>
33 #include STL_IOSTREAM
34 #include <simgear/debug/logstream.hxx>
35
36 #include <float.h>
37 #include <math.h>
38 #include <stdlib.h>
39 #include "linintp2.h"
40
41 SG_USING_NAMESPACE(std);
42 SG_USING_STD(cout);
43
44 //---------------------------------------------------------------------------
45 mgcLinInterp2D::mgcLinInterp2D (int _numPoints, double* x, double* y, 
46                                    unsigned int* _f)
47 {
48     if ( (numPoints = _numPoints) < 3 )
49     {
50         point = 0;
51         edge = 0;
52         triangle = 0;
53         numTriangles = 0;
54         return;
55     }
56
57     SG_LOG(SG_MATH, SG_DEBUG, "[ 20%] allocating memory");
58
59     point = new double*[numPoints];
60     tmppoint = new double*[numPoints+3];
61     f = new unsigned int[numPoints];
62     int i;
63     for (i = 0; i < numPoints; i++)
64         point[i] = new double[2];
65     for (i = 0; i < numPoints+3; i++)
66         tmppoint[i] = new double[2];
67     for (i = 0; i < numPoints; i++)
68     {
69         point[i][0] = tmppoint[i][0] = x[i];
70         point[i][1] = tmppoint[i][1] = y[i];
71
72         f[i] = _f[i];
73     }
74
75     SG_LOG(SG_MATH, SG_DEBUG, "[ 30%] creating delaunay diagram");
76
77     Delaunay2D();
78 }
79 //---------------------------------------------------------------------------
80 mgcLinInterp2D::~mgcLinInterp2D ()
81 {
82     if ( numPoints < 3 )
83         return;
84
85     int i;
86
87     if ( point )
88     {
89         for (i = 0; i < numPoints; i++)
90             delete[] point[i];
91         delete[] point;
92     }
93     if ( tmppoint )
94     {
95         for (i = 0; i < numPoints+3; i++)
96             delete[] tmppoint[i];
97         delete[] tmppoint;
98     }
99
100     delete[] f;
101     delete[] edge;
102     delete[] triangle;
103 }
104 //---------------------------------------------------------------------------
105 void mgcLinInterp2D::ComputeBarycenter (Vertex& v0, Vertex& v1, Vertex& v2, 
106                                            Vertex& ver, double c[3])
107 {
108     double a0 = v0.x-v2.x;
109     double b0 = v0.y-v2.y;
110     double a1 = v1.x-v2.x;
111     double b1 = v1.y-v2.y;
112     double a2 = ver.x-v2.x;
113     double b2 = ver.y-v2.y;
114
115     double m00 = a0*a0+b0*b0;
116     double m01 = a0*a1+b0*b1;
117     double m11 = a1*a1+b1*b1;
118     double r0 = a2*a0+b2*b0;
119     double r1 = a2*a1+b2*b1;
120     double det = m00*m11-m01*m01;
121
122     c[0] = (m11*r0-m01*r1)/det;
123     c[1] = (m00*r1-m01*r0)/det;
124     c[2] = 1-c[0]-c[1];
125 }
126 //---------------------------------------------------------------------------
127 int mgcLinInterp2D::InTriangle (Vertex& v0, Vertex& v1, Vertex& v2, 
128                                    Vertex& test)
129 {
130     const double eps = 1e-08;
131     double tx, ty, nx, ny;
132
133     // test against normal to first edge
134     tx = test.x - v0.x;
135     ty = test.y - v0.y;
136     nx = v0.y - v1.y;
137     ny = v1.x - v0.x;
138     if ( tx*nx + ty*ny < -eps )
139         return 0;
140
141     // test against normal to second edge
142     tx = test.x - v1.x;
143     ty = test.y - v1.y;
144     nx = v1.y - v2.y;
145     ny = v2.x - v1.x;
146     if ( tx*nx + ty*ny < -eps )
147         return 0;
148
149     // test against normal to third edge
150     tx = test.x - v2.x;
151     ty = test.y - v2.y;
152     nx = v2.y - v0.y;
153     ny = v0.x - v2.x;
154     if ( tx*nx + ty*ny < -eps )
155         return 0;
156
157     return 1;
158 }
159 //---------------------------------------------------------------------------
160 int mgcLinInterp2D::Evaluate (double x, double y, EvaluateData& F)
161 {
162     Vertex ver = { x, y };
163     // determine which triangle contains the target point
164
165     int i; 
166     Vertex v0, v1, v2;
167     for (i = 0; i < numTriangles; i++)
168     {
169         Triangle& t = triangle[i];
170         v0.x = point[t.vertex[0]][0];
171         v0.y = point[t.vertex[0]][1];
172         v1.x = point[t.vertex[1]][0];
173         v1.y = point[t.vertex[1]][1];
174         v2.x = point[t.vertex[2]][0];
175         v2.y = point[t.vertex[2]][1];
176
177         if ( InTriangle(v0,v1,v2,ver) )
178             break;
179     }
180
181     if ( i == numTriangles )  // point is outside interpolation region
182     {
183         return 0;
184     }
185
186     Triangle& t = triangle[i];  // (x,y) is in this triangle
187
188     // compute barycentric coordinates with respect to subtriangle
189     double bary[3];
190     ComputeBarycenter(v0,v1,v2,ver,bary);
191
192     // compute barycentric combination of function values at vertices
193     F.index[0] = f[t.vertex[0]];
194     F.index[1] = f[t.vertex[1]];
195     F.index[2] = f[t.vertex[2]];
196     F.percentage[0] = bary[0];
197     F.percentage[1] = bary[1];
198     F.percentage[2] = bary[2];
199
200     return 1;
201 }
202 //---------------------------------------------------------------------------
203 int mgcLinInterp2D::Delaunay2D ()
204 {
205     int result;
206
207     const double EPSILON = 1e-12;
208     const int TSIZE = 75;
209     const double RANGE = 10.0;
210
211     xmin = tmppoint[0][0];
212     xmax = xmin;
213     ymin = tmppoint[0][1];
214     ymax = ymin;
215
216     int i;
217     for (i = 0; i < numPoints; i++)
218     {
219         double value = tmppoint[i][0];
220         if ( xmax < value )
221             xmax = value;
222         if ( xmin > value )
223             xmin = value;
224
225         value = tmppoint[i][1];
226         if ( ymax < value )
227             ymax = value;
228         if ( ymin > value )
229             ymin = value;
230     }
231
232     double xrange = xmax-xmin, yrange = ymax-ymin;
233     double maxrange = xrange;
234     if ( maxrange < yrange )
235         maxrange = yrange;
236
237     // need to scale the data later to do a correct triangle count
238     double maxrange2 = maxrange*maxrange;
239
240     // tweak the points by very small random numbers
241     double bgs = EPSILON*maxrange;
242     srand(367);   
243     for (i = 0; i < numPoints; i++) 
244     {
245         tmppoint[i][0] += bgs*(0.5 - rand()/double(RAND_MAX));
246         tmppoint[i][1] += bgs*(0.5 - rand()/double(RAND_MAX));
247     }
248
249     double wrk[2][3] =
250     {
251         { 5*RANGE, -RANGE, -RANGE },
252         { -RANGE, 5*RANGE, -RANGE }
253     };
254     for (i = 0; i < 3; i++)
255     {
256         tmppoint[numPoints+i][0] = xmin+xrange*wrk[0][i];
257         tmppoint[numPoints+i][1] = ymin+yrange*wrk[1][i];
258     }
259
260     int i0, i1, i2, i3, i4, i5, i6, i7, i8, i9, i11;
261     int nts, ii[3];
262     double xx;
263
264     int tsz = 2*TSIZE;
265     int** tmp = new int*[tsz+1];
266     tmp[0] = new int[2*(tsz+1)];
267     for (i0 = 1; i0 < tsz+1; i0++)
268         tmp[i0] = tmp[0] + 2*i0;
269     i1 = 2*(numPoints + 2);
270
271     int* id = new int[i1];
272     for (i0 = 0; i0 < i1; i0++) 
273         id[i0] = i0; 
274
275     int** a3s = new int*[i1];
276     a3s[0] = new int[3*i1];
277     for (i0 = 1; i0 < i1; i0++)
278         a3s[i0] = a3s[0] + 3*i0;
279     a3s[0][0] = numPoints;
280     a3s[0][1] = numPoints+1;
281     a3s[0][2] = numPoints+2;
282
283     double** ccr = new double*[i1];  // circumscribed centers and radii
284     ccr[0] = new double[3*i1];
285     for (i0 = 1; i0 < i1; i0++)
286         ccr[i0] = ccr[0] + 3*i0;
287     ccr[0][0] = 0.0;
288     ccr[0][1] = 0.0;
289     ccr[0][2] = FLT_MAX;
290
291     nts = 1;  // number of triangles
292     i4 = 1;
293
294     SG_LOG(SG_MATH, SG_DEBUG, "[ 40%] create triangulation");
295
296     // compute triangulation
297     for (i0 = 0; i0 < numPoints; i0++)
298     {  
299         i1 = i7 = -1;
300         i9 = 0;
301         for (i11 = 0; i11 < nts; i11++)
302         {
303             i1++;
304             while ( a3s[i1][0] < 0 ) 
305                 i1++;
306             xx = ccr[i1][2];
307             for (i2 = 0; i2 < 2; i2++)
308             {  
309                 double z = tmppoint[i0][i2]-ccr[i1][i2];
310                 xx -= z*z;
311                 if ( xx < 0 ) 
312                     goto Corner3;
313             }
314             i9--;
315             i4--;
316             id[i4] = i1;
317             for (i2 = 0; i2 < 3; i2++)
318             {  
319                 ii[0] = 0;
320                 if (ii[0] == i2) 
321                     ii[0]++;
322                 for (i3 = 1; i3 < 2; i3++)
323                 {  
324                     ii[i3] = ii[i3-1] + 1;
325                     if (ii[i3] == i2) 
326                         ii[i3]++;
327                 }
328                 if ( i7 > 1 )
329                 {  
330                     i8 = i7;
331                     for (i3 = 0; i3 <= i8; i3++)
332                     {  
333                         for (i5 = 0; i5 < 2; i5++) 
334                             if ( a3s[i1][ii[i5]] != tmp[i3][i5] ) 
335                                 goto Corner1;
336                         for (i6 = 0; i6 < 2; i6++) 
337                             tmp[i3][i6] = tmp[i8][i6];
338                         i7--;
339                         goto Corner2;
340 Corner1:;
341                     }
342                 }
343                 if ( ++i7 > tsz )
344                 {
345                     // temporary storage exceeded, increase TSIZE
346                     result = 0;
347                     goto ExitDelaunay;
348                 }
349                 for (i3 = 0; i3 < 2; i3++) 
350                     tmp[i7][i3] = a3s[i1][ii[i3]];
351 Corner2:;
352             }
353             a3s[i1][0] = -1;
354 Corner3:;
355         }
356
357         for (i1 = 0; i1 <= i7; i1++)
358         {  
359             for (i2 = 0; i2 < 2; i2++)
360                 for (wrk[i2][2] = 0, i3 = 0; i3 < 2; i3++)
361                 {  
362                     wrk[i2][i3] = tmppoint[tmp[i1][i2]][i3]-tmppoint[i0][i3];
363                     wrk[i2][2] += 
364                         0.5*wrk[i2][i3]*(tmppoint[tmp[i1][i2]][i3]+
365                         tmppoint[i0][i3]);
366                 }
367
368             xx = wrk[0][0]*wrk[1][1]-wrk[1][0]*wrk[0][1];
369             ccr[id[i4]][0] = (wrk[0][2]*wrk[1][1]-wrk[1][2]*wrk[0][1])/xx;
370             ccr[id[i4]][1] = (wrk[0][0]*wrk[1][2]-wrk[1][0]*wrk[0][2])/xx;
371
372             for (ccr[id[i4]][2] = 0, i2 = 0; i2 < 2; i2++) 
373             {  
374                 double z = tmppoint[i0][i2]-ccr[id[i4]][i2];
375                 ccr[id[i4]][2] += z*z;
376                 a3s[id[i4]][i2] = tmp[i1][i2];
377             }
378
379             a3s[id[i4]][2] = i0;
380             i4++;
381             i9++;
382         }
383         nts += i9;
384     }
385
386     // count the number of triangles
387     SG_LOG(SG_MATH, SG_DEBUG, "[ 50%] count the number of triangles");
388
389     numTriangles = 0;
390     i0 = -1;
391     for (i11 = 0; i11 < nts; i11++)
392     {  
393         i0++;
394         while ( a3s[i0][0] < 0 ) 
395             i0++;
396         if ( a3s[i0][0] < numPoints )
397         {  
398             for (i1 = 0; i1 < 2; i1++) 
399                 for (i2 = 0; i2 < 2; i2++) 
400                     wrk[i1][i2] = 
401                         tmppoint[a3s[i0][i1]][i2]-tmppoint[a3s[i0][2]][i2];
402
403             if ( fabs(wrk[0][0]*wrk[1][1]-wrk[0][1]*wrk[1][0]) > EPSILON*maxrange2 )
404                 numTriangles++;
405         }
406     }
407
408     // create the triangles
409     SG_LOG(SG_MATH, SG_DEBUG, "[ 60%] create the triangles");
410
411     triangle = new Triangle[numTriangles];
412
413     numTriangles = 0;
414     i0 = -1;
415     for (i11 = 0; i11 < nts; i11++)
416     {  
417         i0++;
418         while ( a3s[i0][0] < 0 ) 
419             i0++;
420         if ( a3s[i0][0] < numPoints )
421         {  
422             for (i1 = 0; i1 < 2; i1++) 
423                 for (i2 = 0; i2 < 2; i2++) 
424                     wrk[i1][i2] = 
425                         tmppoint[a3s[i0][i1]][i2]-tmppoint[a3s[i0][2]][i2];
426             xx = wrk[0][0]*wrk[1][1]-wrk[0][1]*wrk[1][0];
427             if ( fabs(xx) > EPSILON*maxrange2 )
428             {  
429                 int delta = xx < 0 ? 1 : 0;
430                 Triangle& tri = triangle[numTriangles];
431                 tri.vertex[0] = a3s[i0][0];
432                 tri.vertex[1] = a3s[i0][1+delta];
433                 tri.vertex[2] = a3s[i0][2-delta];
434                 tri.adj[0] = -1;
435                 tri.adj[1] = -1;
436                 tri.adj[2] = -1;
437                 numTriangles++;
438             }
439         }
440     }
441
442     // build edge table
443     SG_LOG(SG_MATH, SG_DEBUG, "[ 70%] build the edge table");
444
445     numEdges = 0;
446     edge = new Edge[3*numTriangles];
447
448     int j, j0, j1;
449     for (i = 0; i < numTriangles; i++)
450     {
451         if ( (i%500) == 0)
452             SG_LOG(SG_MATH, SG_BULK, "[ 7" << 10*i/numTriangles << "%] build the edge table");
453
454         Triangle& t = triangle[i];
455
456         for (j0 = 0, j1 = 1; j0 < 3; j0++, j1 = (j1+1)%3)
457         {
458             for (j = 0; j < numEdges; j++)
459             {
460                 Edge& e = edge[j];
461                 if ( (t.vertex[j0] == e.vertex[0] 
462                    && t.vertex[j1] == e.vertex[1])
463                 ||   (t.vertex[j0] == e.vertex[1] 
464                    && t.vertex[j1] == e.vertex[0]) )
465                     break;
466             }
467             if ( j == numEdges )  // add edge to table
468             {
469                 edge[j].vertex[0] = t.vertex[j0];
470                 edge[j].vertex[1] = t.vertex[j1];
471                 edge[j].triangle[0] = i;
472                 edge[j].index[0] = j0;
473                 edge[j].triangle[1] = -1;
474                 numEdges++;
475             }
476             else  // edge already exists, add triangle to table
477             {
478                 edge[j].triangle[1] = i;
479                 edge[j].index[1] = j0;
480             }
481         }
482     }
483
484     // establish links between adjacent triangles
485     SG_LOG(SG_MATH, SG_DEBUG, "[ 80%] establishing links between adjacent triangles");
486
487     for (i = 0; i < numEdges; i++)
488     {
489         if ( edge[i].triangle[1] != -1 )
490         {
491             j0 = edge[i].triangle[0];
492             j1 = edge[i].triangle[1];
493             triangle[j0].adj[edge[i].index[0]] = j1;
494             triangle[j1].adj[edge[i].index[1]] = j0;
495         }
496     }
497
498     result = 1;
499
500 ExitDelaunay:;
501     delete[] tmp[0];
502     delete[] tmp;
503     delete[] id;
504     delete[] a3s[0];
505     delete[] a3s;
506     delete[] ccr[0];
507     delete[] ccr;
508
509     SG_LOG(SG_MATH, SG_DEBUG, "[ 90%] finsishes delauney triangulation");
510
511     return result;
512 }
513 //---------------------------------------------------------------------------
514 void mgcLinInterp2D::GetPoint (int i, double& x, double& y)
515 {
516     // assumes i is valid [can use PointCount() before passing i]
517     x = point[i][0];
518     y = point[i][1];
519 }
520 //---------------------------------------------------------------------------
521 void mgcLinInterp2D::GetEdge (int i, double& x0, double& y0, double& x1,
522                                  double& y1)
523 {
524     // assumes i is valid [can use EdgeCount() before passing i]
525     int v0 = edge[i].vertex[0], v1 = edge[i].vertex[1];
526
527     x0 = point[v0][0];
528     y0 = point[v0][1];
529     x1 = point[v1][0];
530     y1 = point[v1][1];
531 }
532 //---------------------------------------------------------------------------
533 void mgcLinInterp2D::GetTriangle (int i, double& x0, double& y0, double& x1,
534                                      double& y1, double& x2, double& y2)
535 {
536     // assumes i is valid [can use TriangleCount() before passing i]
537     int v0 = triangle[i].vertex[0];
538     int v1 = triangle[i].vertex[1];
539     int v2 = triangle[i].vertex[2];
540
541     x0 = point[v0][0];
542     y0 = point[v0][1];
543     x1 = point[v1][0];
544     y1 = point[v1][1];
545     x2 = point[v2][0];
546     y2 = point[v2][1];
547 }
548 //---------------------------------------------------------------------------
549