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