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