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