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