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