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