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