src/Scenery/hitlist.cxx

   1 // hitlist.cxx -
   2 // Height Over Terrain and Assosciated Routines for FlightGear based Scenery
   3 // Written by Norman Vine, started 2000.
   4
   5 #ifdef HAVE_CONFIG_H
   6 #  include <config.h>
   7 #endif
   8
   9 #ifdef HAVE_WINDOWS_H
  10 #  include <windows.h>
  11 #endif
  12
  13 #include <float.h>
  14 #include <math.h>
  15
  16 #include <GL/glut.h>
  17 #include <GL/gl.h>
  18
  19 #include <plib/sg.h>
  20
  21 #include <simgear/constants.h>
  22 #include <simgear/sg_inlines.h>
  23 #include <simgear/debug/logstream.hxx>
  24 #include <simgear/math/point3d.hxx>
  25 #include <simgear/math/sg_geodesy.hxx>
  26 #include <simgear/math/vector.hxx>
  27
  28 #include <Main/globals.hxx>
  29 #include <Main/viewer.hxx>
  30
  31 #include "hitlist.hxx"
  32
  33 extern ssgBranch *terrain_branch;
  34
  35 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
  36                                  const sgdVec3 l_vec, const sgdVec4 plane )
  37 {
  38     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
  39
  40   /* Is line parallel to plane? */
  41
  42     if ( fabs ( tmp ) < FLT_EPSILON )
  43         return false ;
  44
  45     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
  46                                  + plane[3] ) / tmp ) ;
  47     sgdAddVec3  ( dst, l_org ) ;
  48
  49     return true ;
  50 }
  51
  52
  53 /*
  54  * Given a point and a triangle lying on the same plane
  55  * check to see if the point is inside the triangle
  56  */
  57 static bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
  58 {
  59     sgdVec3 dif;
  60
  61     SGDfloat min, max;
  62     // punt if outside bouding cube
  63     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
  64     if( (point[0] < min) || (point[0] > max) )
  65         return false;
  66     dif[0] = max - min;
  67
  68     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
  69     if( (point[1] < min) || (point[1] > max) )
  70         return false;
  71     dif[1] = max - min;
  72
  73     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
  74     if( (point[2] < min) || (point[2] > max) )
  75         return false;
  76     dif[2] = max - min;
  77
  78     // drop the smallest dimension so we only have to work in 2d.
  79     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
  80     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
  81     if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
  82         // x is the smallest dimension
  83         x1 = point[1];
  84         y1 = point[2];
  85         x2 = tri[0][1];
  86         y2 = tri[0][2];
  87         x3 = tri[1][1];
  88         y3 = tri[1][2];
  89         rx = tri[2][1];
  90         ry = tri[2][2];
  91     } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
  92         // y is the smallest dimension
  93         x1 = point[0];
  94         y1 = point[2];
  95         x2 = tri[0][0];
  96         y2 = tri[0][2];
  97         x3 = tri[1][0];
  98         y3 = tri[1][2];
  99         rx = tri[2][0];
 100         ry = tri[2][2];
 101     } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
 102         // z is the smallest dimension
 103         x1 = point[0];
 104         y1 = point[1];
 105         x2 = tri[0][0];
 106         y2 = tri[0][1];
 107         x3 = tri[1][0];
 108         y3 = tri[1][1];
 109         rx = tri[2][0];
 110         ry = tri[2][1];
 111     } else {
 112         // all dimensions are really small so lets call it close
 113         // enough and return a successful match
 114         return true;
 115     }
 116
 117     // check if intersection point is on the same side of p1 <-> p2 as p3
 118     SGDfloat tmp = (y2 - y3) / (x2 - x3);
 119     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
 120     int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
 121     if ( side1 != side2 ) {
 122         // printf("failed side 1 check\n");
 123         return false;
 124     }
 125
 126     // check if intersection point is on correct side of p2 <-> p3 as p1
 127     tmp = (y3 - ry) / (x3 - rx);
 128     side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
 129     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 130     if ( side1 != side2 ) {
 131         // printf("failed side 2 check\n");
 132         return false;
 133     }
 134
 135     // check if intersection point is on correct side of p1 <-> p3 as p2
 136     tmp = (y2 - ry) / (x2 - rx);
 137     side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
 138     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 139     if ( side1 != side2 ) {
 140         // printf("failed side 3  check\n");
 141         return false;
 142     }
 143
 144     return true;
 145 }
 146
 147
 148 inline static int isZeroAreaTri( sgdVec3 tri[3] )
 149 {
 150     return( sgdEqualVec3(tri[0], tri[1]) ||
 151             sgdEqualVec3(tri[1], tri[2]) ||
 152             sgdEqualVec3(tri[2], tri[0]) );
 153 }
 154
 155 /*
 156  * Given a point and a triangle lying on the same plane
 157  * check to see if the point is inside the triangle
 158  */
 159
 160 #if 0
 161 static bool PointInTri( sgdVec3 P, sgdVec3 V[3] )
 162 {
 163     sgdVec3 X,W1,W2;
 164     sgdSubVec3(X,P,V[0]);
 165     sgdSubVec3(W1,V[1],V[2]);
 166     sgdSubVec3(W2,V[2],V[0]);
 167
 168     sgdVec3 C;
 169     sgdVectorProductVec3(C, W1, W2);
 170
 171     double d = sgdScalarProductVec3(X,C);
 172
 173     // Test not needed if you KNOW point is on plane of triangle
 174     // and triangle is not degenerate
 175     if( d > -FLT_EPSILON && d < FLT_EPSILON )
 176         return false;
 177
 178     double u11 = sgdScalarProductVec3(W1,W1);
 179     double u22 = sgdScalarProductVec3(W2,W2);
 180     double u12 = sgdScalarProductVec3(W1,W2);
 181
 182     double y1 = sgdScalarProductVec3(X,W1);
 183     double y2 = sgdScalarProductVec3(X,W2);
 184     double z1 = (y1*u22 - y2*u12)/d;
 185
 186     if( z1>0 || z1>1 )
 187         return false;
 188
 189     double z2 = (y2*u11 - y1*u12)/d;
 190
 191     if( z2>0 || z2>1 )
 192         return false;
 193
 194     return true;
 195 }
 196 #endif
 197
 198 /*
 199    Find the intersection of an infinite line with a leaf
 200    the line being defined by a point and direction.
 201
 202    Variables
 203     In:
 204      ssgLeaf pointer  -- leaf
 205      qualified matrix -- m
 206      line origin      -- orig
 207      line direction   -- dir
 208     Out:
 209      result  -- intersection point
 210      normal  -- intersected tri's normal
 211
 212    Returns:
 213     true if intersection found
 214     false otherwise
 215
 216    !!! WARNING !!!
 217     If you need an exhaustive list of hitpoints YOU MUST use
 218     the generic version of this function as the specialized
 219     versions will do an early out of expensive tests if the point
 220     can not be the closest one found
 221    !!! WARNING !!!
 222 */
 223 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 224                               sgdVec3 orig, sgdVec3 dir )
 225 {
 226     int num_hits = 0;
 227     int i = 0;
 228
 229     for ( ; i < leaf->getNumTriangles(); ++i ) {
 230         short i1, i2, i3;
 231         leaf->getTriangle( i, &i1, &i2, &i3 );
 232
 233         sgdVec3 tri[3];
 234         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
 235         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
 236         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
 237
 238         // avoid division by zero when two points are the same
 239         if( isZeroAreaTri( tri ) )
 240             continue;
 241
 242         sgdVec4 plane;
 243         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 244
 245         sgdVec3 point;
 246         if( sgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 247             if( sgdPointInTriangle( point, tri ) ) {
 248                                 // transform point into passed into desired coordinate frame
 249                 sgdXformPnt3( point, point, m );
 250                 add(leaf,i,point,plane);
 251                 num_hits++;
 252             }
 253         }
 254     }
 255     return num_hits;
 256 }
 257
 258
 259 //=================
 260 int FGHitList::IntersectPolyOrFanLeaf( ssgLeaf *leaf, sgdMat4 m,
 261                                        sgdVec3 orig, sgdVec3 dir )
 262 {
 263     double tmp_dist;
 264
 265     // number of hits but there could be more that
 266     // were not found because of short circut switch !
 267     // so you may want to use the unspecialized IntersectLeaf()
 268     int num_hits = 0;
 269
 270     int ntri = leaf->getNumTriangles();
 271     for ( int n = 0; n < ntri; ++n ) {
 272         sgdVec3 tri[3];
 273
 274         if ( !n ) {
 275             sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 276             sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 277             sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 278         } else {
 279             sgdCopyVec3( tri[1], tri[2] );
 280             sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 281         }
 282
 283         if( isZeroAreaTri( tri ) )
 284             continue;
 285
 286         sgdVec4 plane;
 287         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 288
 289         sgdVec3 point;
 290
 291         //inlined IsectInfLinePlane( point dst, orig, dir, plane )
 292         {
 293             SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 294
 295             /* Is line parallel to plane? */
 296             if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 297                 continue ;
 298
 299             sgdScaleVec3 ( point, dir,
 300                            -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 301                            / tmp ) ;
 302
 303             sgdAddVec3  ( point, orig ) ;
 304         }  // end of inlined intersection routine
 305
 306         // short circut if this point is further away then a previous hit
 307         tmp_dist = sgdScalarProductVec3(point,point);
 308         if( tmp_dist > test_dist )
 309             continue;
 310
 311         if( sgdPointInTriangle( point, tri ) ) {
 312             // transform point into passed coordinate frame
 313             sgdXformPnt3( point, point, m );
 314             add(leaf,n,point,plane);
 315             test_dist = tmp_dist;
 316             num_hits++;
 317         }
 318     }
 319     return num_hits;
 320 }
 321
 322
 323 //===============
 324
 325 int FGHitList::IntersectTriLeaf( ssgLeaf *leaf, sgdMat4 m,
 326                                  sgdVec3 orig, sgdVec3 dir )
 327 {
 328     double tmp_dist;
 329
 330     // number of hits but there could be more that
 331     // were not found because of short circut switch !
 332     // so you may want to use the unspecialized IntersectLeaf()
 333     int num_hits = 0;
 334
 335     int ntri = leaf->getNumTriangles();
 336     for ( int n = 0; n < ntri; ++n ) {
 337         sgdVec3 tri[3];
 338         sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
 339         sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
 340         sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
 341
 342         // avoid division by zero when two points are the same
 343         if( isZeroAreaTri( tri ) )
 344             continue;
 345
 346         sgdVec4 plane;
 347         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 348
 349         sgdVec3 point;
 350         //inlined IsectInfLinePlane( point dst, orig, dir, plane )
 351         {
 352             SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 353
 354             /* Is line parallel to plane? */
 355             if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 356                 continue ;
 357
 358             sgdScaleVec3 ( point, dir,
 359                            -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 360                            / tmp ) ;
 361
 362             sgdAddVec3  ( point, orig ) ;
 363         }  // end of inlined intersection routine
 364
 365         // short circut if this point is further away then a previous hit
 366         tmp_dist = sgdScalarProductVec3(point,point);
 367         if( tmp_dist > test_dist )
 368             continue;
 369
 370         if( sgdPointInTriangle( point, tri ) ) {
 371             // transform point into passed coordinate frame
 372             sgdXformPnt3( point, point, m );
 373             add(leaf,n,point,plane);
 374             test_dist = tmp_dist;
 375             num_hits++;
 376         }
 377     }
 378     return num_hits;
 379 }
 380
 381 //============================
 382
 383 int FGHitList::IntersectStripLeaf( ssgLeaf *leaf, sgdMat4 m,
 384                                    sgdVec3 orig, sgdVec3 dir )
 385 {
 386     double tmp_dist;
 387
 388     // number of hits but there could be more that
 389     // were not found because of short circut switch !
 390     // so you may want to use the unspecialized IntersectLeaf()
 391     int num_hits = 0;
 392
 393     int ntri = leaf->getNumTriangles();
 394     for ( int n = 0; n < ntri; ++n ) {
 395         sgdVec3 tri[3];
 396
 397         if ( !n ) {
 398             sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 399             sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 400             sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 401         } else {
 402             if ( n & 1 ) {
 403                 sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
 404                 sgdCopyVec3( tri[1], tri[2] );
 405                 sgdCopyVec3( tri[2], tri[1] );
 406
 407             } else {
 408                 sgdCopyVec3( tri[0], tri[1] );
 409                 sgdCopyVec3( tri[1], tri[2] );
 410                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 411             }
 412         }
 413
 414         if( isZeroAreaTri( tri ) )
 415             continue;
 416
 417         sgdVec4 plane;
 418         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 419
 420         sgdVec3 point;
 421
 422         //inlined IsectInfLinePlane( point dst, orig, dir, plane )
 423         {
 424             SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 425
 426             /* Is line parallel to plane? */
 427             if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 428                 continue ;
 429
 430             sgdScaleVec3 ( point, dir,
 431                            -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 432                            / tmp ) ;
 433
 434             sgdAddVec3  ( point, orig ) ;
 435         }  // end of inlined intersection routine
 436
 437         // short circut if this point is further away then a previous hit
 438         tmp_dist = sgdScalarProductVec3(point,point);
 439         if( tmp_dist > test_dist )
 440             continue;
 441
 442         if( sgdPointInTriangle( point, tri ) ) {
 443             // transform point into passed coordinate frame
 444             sgdXformPnt3( point, point, m );
 445             add(leaf,n,point,plane);
 446             test_dist = tmp_dist;
 447             num_hits++;
 448         }
 449     }
 450     return num_hits;
 451 }
 452
 453 // ======================
 454
 455 int FGHitList::IntersectQuadsLeaf( ssgLeaf *leaf, sgdMat4 m,
 456                                    sgdVec3 orig, sgdVec3 dir )
 457 {
 458     double tmp_dist;
 459
 460     // number of hits but there could be more that
 461     // were not found because of short circut switch !
 462     // so you may want to use the unspecialized IntersectLeaf()
 463     int num_hits = 0;
 464
 465     int ntri = leaf->getNumTriangles();
 466     for ( int n = 0; n < ntri; ++n ) {
 467         sgdVec3 tri[3];
 468
 469         sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
 470         sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
 471         sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
 472
 473         if( isZeroAreaTri( tri ) )
 474             continue;
 475
 476         sgdVec4 plane;
 477         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 478
 479         sgdVec3 point;
 480
 481         //inlined IsectInfLinePlane( point dst, orig, dir, plane )
 482         {
 483             SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 484
 485             /* Is line parallel to plane? */
 486             if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 487                 continue ;
 488
 489             sgdScaleVec3 ( point, dir,
 490                            -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 491                            / tmp ) ;
 492
 493             sgdAddVec3  ( point, orig ) ;
 494         }  // end of inlined intersection routine
 495
 496         // short circut if this point is further away then a previous hit
 497         tmp_dist = sgdScalarProductVec3(point,point);
 498         if( tmp_dist > test_dist )
 499             continue;
 500
 501         if( sgdPointInTriangle( point, tri ) ) {
 502             // transform point into passed coordinate frame
 503             sgdXformPnt3( point, point, m );
 504             add(leaf,n,point,plane);
 505             test_dist = tmp_dist;
 506             num_hits++;
 507         }
 508     }
 509     return num_hits;
 510 }
 511
 512
 513
 514 // Need these because of mixed matrix types
 515 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
 516 {
 517     for ( int j = 0 ; j < 4 ; j++ ) {
 518         dst[0][j] = m2[0][0] * m1[0][j] +
 519             m2[0][1] * m1[1][j] +
 520             m2[0][2] * m1[2][j] +
 521             m2[0][3] * m1[3][j] ;
 522
 523         dst[1][j] = m2[1][0] * m1[0][j] +
 524             m2[1][1] * m1[1][j] +
 525             m2[1][2] * m1[2][j] +
 526             m2[1][3] * m1[3][j] ;
 527
 528         dst[2][j] = m2[2][0] * m1[0][j] +
 529             m2[2][1] * m1[1][j] +
 530             m2[2][2] * m1[2][j] +
 531             m2[2][3] * m1[3][j] ;
 532
 533         dst[3][j] = m2[3][0] * m1[0][j] +
 534             m2[3][1] * m1[1][j] +
 535             m2[3][2] * m1[2][j] +
 536             m2[3][3] * m1[3][j] ;
 537     }
 538 }
 539
 540 /*
 541  *
 542  */
 543
 544 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 545                                  sgdVec3 orig, sgdVec3 dir )
 546 {
 547     /* the lookat vector and matrix in branch's coordinate frame
 548      * but we won't determine these unless needed,
 549      * This 'lazy evaluation' is a result of profiling data */
 550     sgdVec3 orig_leaf, dir_leaf;
 551     sgdMat4 m_leaf;
 552
 553
 554     // 'lazy evaluation' flag
 555     int first_time = 1;
 556
 557     for ( ssgEntity *kid = branch->getKid( 0 );
 558           kid != NULL;
 559           kid = branch->getNextKid() )
 560     {
 561         if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
 562             sgdVec3 center;
 563             sgdSetVec3( center,
 564                         kid->getBSphere()->getCenter()[0],
 565                         kid->getBSphere()->getCenter()[1],
 566                         kid->getBSphere()->getCenter()[2] );
 567             sgdXformPnt3( center, m ) ;
 568
 569             // sgdClosestPointToLineDistSquared( center, orig, dir )
 570             // inlined here because because of profiling results
 571             sgdVec3 u, u1, v;
 572             sgdSubVec3(u, center, orig);
 573             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
 574                           / sgdScalarProductVec3(dir,dir) );
 575             sgdSubVec3(v, u, u1);
 576
 577             // doubles because of possible overflow
 578 #define SQUARE(x) (x*x)
 579             if( sgdScalarProductVec3(v, v)
 580                 < SQUARE( double(kid->getBSphere()->getRadius()) ) )
 581             {
 582                 // possible intersections
 583                 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
 584                     sgdMat4 m_new;
 585                     if ( kid->isA( ssgTypeTransform() ) )
 586                     {
 587                         sgMat4 fxform;
 588                         ((ssgTransform *)kid)->getTransform( fxform );
 589                         sgMultMat4(m_new, m, fxform);
 590                     } else {
 591                         sgdCopyMat4(m_new, m);
 592                     }
 593                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 594                 } else if ( kid->isAKindOf( ssgTypeLeaf() ) ) {
 595                     if ( first_time ) {
 596                         // OK we need these
 597                         sgdTransposeNegateMat4( m_leaf, m);
 598                         sgdXformPnt3( orig_leaf, orig, m_leaf );
 599                         sgdXformPnt3( dir_leaf,  dir,  m_leaf );
 600                         first_time = 0;
 601                     }
 602
 603                     GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
 604
 605                     switch ( primType ) {
 606                     case GL_POLYGON :
 607                     case GL_TRIANGLE_FAN :
 608                         IntersectPolyOrFanLeaf( (ssgLeaf *)kid, m,
 609                                                 orig_leaf, dir_leaf );
 610                         break;
 611                     case GL_TRIANGLES :
 612                         IntersectTriLeaf( (ssgLeaf *)kid, m,
 613                                           orig_leaf, dir_leaf );
 614                         break;
 615                     case GL_TRIANGLE_STRIP :
 616                     case GL_QUAD_STRIP :
 617                         IntersectStripLeaf( (ssgLeaf *)kid, m,
 618                                             orig_leaf, dir_leaf );
 619                         break;
 620                     case GL_QUADS :
 621                         IntersectQuadsLeaf( (ssgLeaf *)kid, m,
 622                                             orig_leaf, dir_leaf );
 623                         break;
 624                     default:
 625                         IntersectLeaf( (ssgLeaf *)kid, m,
 626                                        orig_leaf, dir_leaf );
 627                     }
 628                 }
 629             } else {
 630                 // end of the line for this branch
 631             }
 632         } else {
 633             // branch requested not to be traversed
 634         }
 635     }
 636 }
 637
 638 void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
 639 {
 640     /*
 641       Walk backwards up the tree, transforming the
 642       vertex by all the matrices along the way.
 643
 644       Upwards recursion hurts my head.
 645     */
 646
 647     sgMat4 mat ;
 648
 649     /*
 650       If this node has a parent - get the composite
 651       matrix for the parent.
 652     */
 653
 654     if ( entity->getNumParents() > 0 )
 655         ssgGetEntityTransform ( entity->getParent(0), mat ) ;
 656     else
 657         sgMakeIdentMat4 ( mat ) ;
 658
 659   /*
 660   If this node has a transform - then concatenate it.
 661   */
 662
 663     if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
 664         sgMat4 this_mat ;
 665         ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
 666         sgPostMultMat4 ( mat, this_mat ) ;
 667     }
 668
 669     sgCopyMat4 ( m, mat ) ;
 670 }
 671 #if 0
 672 // previously used method
 673 // This expects the inital m to be the identity transform
 674 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
 675 {
 676     for ( ssgEntity *parent = branch->getParent(0);
 677           parent != NULL;
 678           parent = parent->getParent(0) )
 679     {
 680         // recurse till we are at the scene root
 681         // then just unwinding the stack should
 682         // give us our cumulative transform :-)  NHV
 683         ssgGetEntityTransform( parent, m );
 684         if ( parent->isA( ssgTypeTransform() ) ) {
 685             sgMat4 xform;
 686             ((ssgTransform *)parent)->getTransform( xform );
 687             sgPreMultMat4( m, xform );
 688         }
 689     }
 690 }
 691 #endif // 0
 692
 693 // return the passed entitity's bsphere's center point radius and
 694 // fully formed current model matrix for entity
 695 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
 696                            float *radius, sgMat4 m )
 697 {
 698     sgSphere *bsphere = entity->getBSphere();
 699     *radius = (double)bsphere->getRadius();
 700     sgCopyVec3( center, bsphere->getCenter() );
 701     sgMakeIdentMat4 ( m ) ;
 702     ssgGetEntityTransform( entity, m );
 703 }
 704
 705
 706 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
 707     sgdMat4 m;
 708     clear();
 709     sgdMakeIdentMat4 ( m ) ;
 710     IntersectBranch( scene, m, orig, dir);
 711 }
 712
 713
 714 static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
 715     sgVec3 tmp;
 716     sgSetVec3(tmp, src[0], src[1], src[2] );
 717     sgMat4 TMP;
 718     sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
 719     sgXformVec3(tmp, tmp, TMP);
 720     sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
 721 }
 722
 723
 724 // a temporary hack until we get everything rewritten with sgdVec3
 725 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 726 {
 727     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 728 }
 729
 730
 731 /*
 732  * This method is faster
 733  */
 734
 735 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig,
 736                            sgdVec3 dir )
 737 {
 738     clear();
 739     IntersectBranch( scene, m, orig, dir);
 740 }
 741
 742
 743 // Determine scenery altitude via ssg.
 744 // returned results are in meters
 745 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
 746                     FGHitList *hit_list,
 747                     double *terrain_elev, double *radius, double *normal)
 748 {
 749     sgdVec3 view_pos;
 750     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 751
 752     sgdVec3 orig, dir;
 753     sgdCopyVec3(orig, view_pos );
 754     sgdCopyVec3(dir, abs_view_pos );
 755     // sgdNormalizeVec3(dir);
 756
 757     // !! why is terrain not globals->get_terrain()
 758     hit_list->Intersect( terrain_branch, orig, dir );
 759
 760     int this_hit=0;
 761     Point3D geoc;
 762     double result = -9999;
 763     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 764
 765     // cout << "hits = ";
 766     int hitcount = hit_list->num_hits();
 767     for ( int i = 0; i < hitcount; ++i ) {
 768         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
 769         double lat_geod, alt, sea_level_r;
 770         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
 771                      &alt, &sea_level_r);
 772         // cout << alt << " ";
 773         if ( alt > result && alt < 10000 ) {
 774             result = alt;
 775             this_hit = i;
 776         }
 777     }
 778     // cout << endl;
 779
 780     if ( result > -9000 ) {
 781         *terrain_elev = result;
 782         *radius = geoc.radius();
 783         sgVec3 tmp;
 784         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 785         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
 786         //      << tmp[2] << endl;
 787         /* ssgState *IntersectedLeafState =
 788            ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 789         CurrentNormalInLocalPlane(tmp, tmp);
 790         sgdSetVec3( normal, tmp );
 791         // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
 792         return true;
 793     } else {
 794         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
 795         *terrain_elev = 0.0;
 796         float *up = globals->get_current_view()->get_world_up();
 797         sgdSetVec3(normal, up[0], up[1], up[2]);
 798         return false;
 799     }
 800 }
 801
 802
 803 // Determine scenery altitude via ssg.
 804 // returned results are in meters
 805 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
 806                     ssgTransform *terra_transform,
 807                     FGHitList *hit_list,
 808                     double *terrain_elev, double *radius, double *normal)
 809 {
 810 #ifndef FAST_HITLIST__TEST
 811     return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
 812                           terrain_elev,radius,normal);
 813 #else
 814     sgdVec3 view_pos;
 815     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 816
 817     sgdVec3 orig, dir;
 818     sgdCopyVec3(orig, view_pos );
 819
 820     sgdCopyVec3(dir, abs_view_pos );
 821     sgdNormalizeVec3(dir);
 822
 823     sgMat4 fxform;
 824     sgMakeIdentMat4 ( fxform ) ;
 825     ssgGetEntityTransform( terra_transform, fxform );
 826
 827     sgdMat4 xform;
 828     sgdSetMat4(xform,fxform);
 829     hit_list->Intersect( terra_transform, xform, orig, dir );
 830
 831     int this_hit=0;
 832     Point3D geoc;
 833     double result = -9999;
 834     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 835
 836     int hitcount = hit_list->num_hits();
 837     for ( int i = 0; i < hitcount; ++i ) {
 838         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
 839         double lat_geod, alt, sea_level_r;
 840         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
 841                      &alt, &sea_level_r);
 842         if ( alt > result && alt < 20000 ) {
 843             result = alt;
 844             this_hit = i;
 845         }
 846     }
 847
 848     if ( result > -9000 ) {
 849         *terrain_elev = result;
 850         *radius = geoc.radius();
 851         sgVec3 tmp;
 852         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 853         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
 854         //      << tmp[2] << endl;
 855         /* ssgState *IntersectedLeafState =
 856            ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 857         CurrentNormalInLocalPlane(tmp, tmp);
 858         sgdSetVec3( normal, tmp );
 859         // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
 860         return true;
 861     } else {
 862         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
 863         return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
 864                               terrain_elev,radius,normal);
 865     }
 866 #endif // !FAST_HITLIST__TEST
 867 }
 868