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 #include <float.h>
  10 #include <math.h>
  11
  12 #include <plib/sg.h>
  13 #include <plib/ssg.h>
  14
  15 #include <simgear/sg_inlines.h>
  16 #include <simgear/debug/logstream.hxx>
  17 #include <simgear/math/point3d.hxx>
  18 #include <simgear/math/polar3d.hxx>
  19 #include <simgear/math/sg_geodesy.hxx>
  20 #include <simgear/math/vector.hxx>
  21 #include <simgear/timing/timestamp.hxx>
  22
  23 #include <Main/globals.hxx>
  24 #include <Main/viewer.hxx>
  25 #include <Scenery/scenery.hxx>
  26
  27 #include "hitlist.hxx"
  28
  29 // Specialized version of sgMultMat4 needed because of mixed matrix
  30 // types
  31 static inline void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
  32     for ( int j = 0 ; j < 4 ; j++ ) {
  33         dst[0][j] = m2[0][0] * m1[0][j] +
  34                     m2[0][1] * m1[1][j] +
  35                     m2[0][2] * m1[2][j] +
  36                     m2[0][3] * m1[3][j] ;
  37
  38         dst[1][j] = m2[1][0] * m1[0][j] +
  39                     m2[1][1] * m1[1][j] +
  40                     m2[1][2] * m1[2][j] +
  41                     m2[1][3] * m1[3][j] ;
  42
  43         dst[2][j] = m2[2][0] * m1[0][j] +
  44                     m2[2][1] * m1[1][j] +
  45                     m2[2][2] * m1[2][j] +
  46                     m2[2][3] * m1[3][j] ;
  47
  48         dst[3][j] = m2[3][0] * m1[0][j] +
  49                     m2[3][1] * m1[1][j] +
  50                     m2[3][2] * m1[2][j] +
  51                     m2[3][3] * m1[3][j] ;
  52     }
  53 }
  54
  55
  56 /*
  57  * Walk backwards up the tree, transforming the vertex by all the
  58  * matrices along the way.
  59  *
  60  * Upwards recursion hurts my head.
  61  */
  62 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m ) {
  63     sgMat4 mat ;
  64
  65     // If this node has a parent - get the composite matrix for the
  66     // parent.
  67     if ( entity->getNumParents() > 0 )
  68         ssgGetEntityTransform ( entity->getParent(0), mat ) ;
  69     else
  70         sgMakeIdentMat4 ( mat ) ;
  71
  72     // If this node has a transform - then concatenate it.
  73     if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
  74         sgMat4 this_mat ;
  75         ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
  76         sgPostMultMat4 ( mat, this_mat ) ;
  77     }
  78
  79     sgCopyMat4 ( m, mat ) ;
  80 }
  81
  82
  83 // return the passed entitity's bsphere's center point radius and
  84 // fully formed current model matrix for entity
  85 static inline void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
  86                                   float *radius, sgMat4 m )
  87 {
  88     sgSphere *bsphere = entity->getBSphere();
  89     *radius = (double)bsphere->getRadius();
  90     sgCopyVec3( center, bsphere->getCenter() );
  91     sgMakeIdentMat4 ( m ) ;
  92     ssgGetEntityTransform( entity, m );
  93 }
  94
  95
  96 // This is same as PLib's sgdIsectInfLinePlane() and can be replaced
  97 // by it after the next PLib release
  98 static inline bool fgdIsectInfLinePlane( sgdVec3 dst,
  99                                          const sgdVec3 l_org,
 100                                          const sgdVec3 l_vec,
 101                                          const sgdVec4 plane )
 102 {
 103     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
 104
 105     /* Is line parallel to plane? */
 106
 107     if ( fabs ( tmp ) < FLT_EPSILON )
 108         return false ;
 109
 110     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
 111                                   + plane[3] ) / tmp ) ;
 112     sgdAddVec3  ( dst, l_org ) ;
 113
 114     return true ;
 115 }
 116
 117 /*
 118  * Given a point and a triangle lying on the same plane check to see
 119  * if the point is inside the triangle
 120  *
 121  * This is same as PLib's sgdPointInTriangle() and can be replaced by
 122  * it after the next PLib release
 123  */
 124 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
 125 {
 126     sgdVec3 dif;
 127
 128     // Some tolerance in meters we accept a point to be outside of the triangle
 129     // and still return that it is inside.
 130     SGDfloat eps = 1e-4;
 131     SGDfloat min, max;
 132     // punt if outside bouding cube
 133     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
 134     if( (point[0] < min - eps) || (point[0] > max + eps) )
 135         return false;
 136     dif[0] = max - min;
 137
 138     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
 139     if( (point[1] < min - eps) || (point[1] > max + eps) )
 140         return false;
 141     dif[1] = max - min;
 142
 143     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
 144     if( (point[2] < min - eps) || (point[2] > max + eps) )
 145         return false;
 146     dif[2] = max - min;
 147
 148     // drop the smallest dimension so we only have to work in 2d.
 149     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
 150     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
 151     if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
 152         // x is the smallest dimension
 153         x1 = point[1];
 154         y1 = point[2];
 155         x2 = tri[0][1];
 156         y2 = tri[0][2];
 157         x3 = tri[1][1];
 158         y3 = tri[1][2];
 159         rx = tri[2][1];
 160         ry = tri[2][2];
 161     } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
 162         // y is the smallest dimension
 163         x1 = point[0];
 164         y1 = point[2];
 165         x2 = tri[0][0];
 166         y2 = tri[0][2];
 167         x3 = tri[1][0];
 168         y3 = tri[1][2];
 169         rx = tri[2][0];
 170         ry = tri[2][2];
 171     } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
 172         // z is the smallest dimension
 173         x1 = point[0];
 174         y1 = point[1];
 175         x2 = tri[0][0];
 176         y2 = tri[0][1];
 177         x3 = tri[1][0];
 178         y3 = tri[1][1];
 179         rx = tri[2][0];
 180         ry = tri[2][1];
 181     } else {
 182         // all dimensions are really small so lets call it close
 183         // enough and return a successful match
 184         return true;
 185     }
 186
 187     // check if intersection point is on the same side of p1 <-> p2 as p3
 188     SGDfloat tmp = (y2 - y3);
 189     SGDfloat tmpn = (x2 - x3);
 190     int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
 191     int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
 192                          + side1 * eps * fabs(tmpn));
 193     if ( side1 != side2 ) {
 194         // printf("failed side 1 check\n");
 195         return false;
 196     }
 197
 198     // check if intersection point is on correct side of p2 <-> p3 as p1
 199     tmp = (y3 - ry);
 200     tmpn = (x3 - rx);
 201     side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
 202     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 203                      + side1 * eps * fabs(tmpn));
 204     if ( side1 != side2 ) {
 205         // printf("failed side 2 check\n");
 206         return false;
 207     }
 208
 209     // check if intersection point is on correct side of p1 <-> p3 as p2
 210     tmp = (y2 - ry);
 211     tmpn = (x2 - rx);
 212     side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
 213     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 214                      + side1 * eps * fabs(tmpn));
 215     if ( side1 != side2 ) {
 216         // printf("failed side 3  check\n");
 217         return false;
 218     }
 219
 220     return true;
 221 }
 222
 223
 224 // Check if all three vertices are the same point (or close enough)
 225 static inline int isZeroAreaTri( sgdVec3 tri[3] )
 226 {
 227     return( sgdEqualVec3(tri[0], tri[1]) ||
 228             sgdEqualVec3(tri[1], tri[2]) ||
 229             sgdEqualVec3(tri[2], tri[0]) );
 230 }
 231
 232
 233 // Constructor
 234 FGHitList::FGHitList() :
 235     last(NULL), test_dist(DBL_MAX)
 236 {
 237 }
 238
 239
 240 // Destructor
 241 FGHitList::~FGHitList() {}
 242
 243
 244 // http://www.cs.lth.se/home/Tomas_Akenine_Moller/raytri/raytri.c
 245 // http://little3d.free.fr/ressources/jgt%20Fast,%20Minumum%20Storage%20Ray-Triangle%20Intersection.htm
 246 // http://www.acm.org/jgt/papers/MollerTrumbore97/
 247
 248 /* Ray-Triangle Intersection Test Routines          */
 249 /* Different optimizations of my and Ben Trumbore's */
 250 /* code from journals of graphics tools (JGT)       */
 251 /* http://www.acm.org/jgt/                          */
 252 /* by Tomas Moller, May 2000                        */
 253
 254 /* code rewritten to do tests on the sign of the determinant */
 255 /* the division is at the end in the code                    */
 256 // cosmetics change by H.J :
 257 // make u & v locals since we don't use them, use sg functions
 258 static bool intersect_triangle(const double orig[3], const double dir[3],
 259                         const double vert0[3], const double vert1[3], const double vert2[3],
 260                         double *t)
 261 {
 262    double u, v;
 263    double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
 264
 265    const SGDfloat eps = 1e-4;
 266
 267    /* find vectors for two edges sharing vert0 */
 268    sgdSubVec3(edge1, vert1, vert0);
 269    sgdSubVec3(edge2, vert2, vert0);
 270
 271    /* begin calculating determinant - also used to calculate U parameter */
 272    sgdVectorProductVec3(pvec, dir, edge2);
 273
 274    /* if determinant is near zero, ray lies in plane of triangle */
 275    double det = sgdScalarProductVec3(edge1, pvec);
 276
 277    if (det > eps)
 278    {
 279       /* calculate distance from vert0 to ray origin */
 280       sgdSubVec3(tvec, orig, vert0);
 281
 282       /* calculate U parameter and test bounds */
 283       u = sgdScalarProductVec3(tvec, pvec);
 284       if (u < 0.0 || u > det)
 285         return false;
 286
 287       /* prepare to test V parameter */
 288       sgdVectorProductVec3(qvec, tvec, edge1);
 289
 290       /* calculate V parameter and test bounds */
 291       v = sgdScalarProductVec3(dir, qvec);
 292       if (v < 0.0 || u + v > det)
 293         return false;
 294
 295    }
 296    else if(det < -eps)
 297    {
 298       /* calculate distance from vert0 to ray origin */
 299       sgdSubVec3(tvec, orig, vert0);
 300
 301       /* calculate U parameter and test bounds */
 302       u = sgdScalarProductVec3(tvec, pvec);
 303       if (u > 0.0 || u < det)
 304         return false;
 305
 306       /* prepare to test V parameter */
 307       sgdVectorProductVec3(qvec, tvec, edge1);
 308
 309       /* calculate V parameter and test bounds */
 310       v = sgdScalarProductVec3(dir, qvec) ;
 311       if (v > 0.0 || u + v < det)
 312         return false;
 313    }
 314    else return false;  /* ray is parallell to the plane of the triangle */
 315
 316    /* calculate t, ray intersects triangle */
 317    *t = sgdScalarProductVec3(edge2, qvec) / det;
 318
 319    return true;
 320 }
 321
 322
 323 /*
 324 Find the intersection of an infinite line with a leaf the line being
 325 defined by a point and direction.
 326
 327 Variables
 328 In:
 329 ssgLeaf pointer  -- leaf
 330 qualified matrix -- m
 331 line origin      -- orig
 332 line direction   -- dir
 333 Out:
 334 result  -- intersection point
 335 normal  -- intersected tri's normal
 336
 337 Returns:
 338 true if intersection found
 339 false otherwise
 340
 341 !!! WARNING !!!
 342
 343 If you need an exhaustive list of hitpoints YOU MUST use the generic
 344 version of this function as the specialized versions will do an early
 345 out of expensive tests if the point can not be the closest one found
 346
 347 !!! WARNING !!!
 348 */
 349 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 350                               sgdVec3 orig, sgdVec3 dir )
 351 {
 352     int num_hits = 0;
 353     int i = 0;
 354
 355     for ( ; i < leaf->getNumTriangles(); ++i ) {
 356         short i1, i2, i3;
 357         leaf->getTriangle( i, &i1, &i2, &i3 );
 358
 359         sgdVec3 tri[3];
 360         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
 361         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
 362         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
 363 #if 1
 364         sgdFloat t;
 365         if( intersect_triangle( orig, dir, tri[0], tri[1], tri[2], &t) ) {
 366             sgdVec4 plane;
 367             sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 368             // t is the distance to the triangle plane
 369             // so P = Orig + t*dir
 370             sgdVec3 point;
 371             sgdAddScaledVec3( point, orig, dir, t );
 372             sgdXformPnt3( point, point, m );
 373             sgdXformPnt4(plane,plane,m);
 374             add(leaf,i,point,plane);
 375             num_hits++;
 376         }
 377 #else
 378         if( isZeroAreaTri( tri ) )
 379             continue;
 380
 381         sgdVec4 plane;
 382         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 383
 384         sgdVec3 point;
 385         if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 386             if( fgdPointInTriangle( point, tri ) ) {
 387                 // transform point into passed into desired coordinate frame
 388                 sgdXformPnt3( point, point, m );
 389                 sgdXformPnt4(plane,plane,m);
 390                 add(leaf,i,point,plane);
 391                 num_hits++;
 392             }
 393         }
 394 #endif
 395     }
 396     return num_hits;
 397 }
 398
 399
 400 // Short circuit/slightly optimized version of the full IntersectLeaf()
 401 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 402                               sgdVec3 orig, sgdVec3 dir,
 403                               GLenum primType )
 404 {
 405     double tmp_dist;
 406
 407     // number of hits but there could be more that
 408     // were not found because of short circut switch !
 409     // so you may want to use the unspecialized IntersectLeaf()
 410     int n, num_hits = 0;
 411
 412     int ntri = leaf->getNumTriangles();
 413     for ( n = 0; n < ntri; ++n )
 414     {
 415         sgdVec3 tri[3];
 416
 417         switch ( primType )
 418         {
 419         case GL_POLYGON :
 420             SG_LOG( SG_TERRAIN, SG_ALERT,
 421                     "WARNING: dubiously handled GL_POLYGON" );
 422         case GL_TRIANGLE_FAN :
 423             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 424                "IntersectLeaf: GL_TRIANGLE_FAN" ); */
 425             if ( !n ) {
 426                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 427                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 428                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 429             } else {
 430                 sgdCopyVec3( tri[1], tri[2] );
 431                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 432             }
 433             break;
 434         case GL_TRIANGLES :
 435             /* SG_LOG( SG_TERRAIN, SG_DEBUG,
 436                "IntersectLeaf: GL_TRIANGLES" ); */
 437             sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
 438             sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
 439             sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
 440             break;
 441         case GL_QUAD_STRIP :
 442             SG_LOG( SG_TERRAIN, SG_ALERT,
 443                     "WARNING: dubiously handled GL_QUAD_STRIP" );
 444         case GL_TRIANGLE_STRIP :
 445             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 446                "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
 447             if ( !n ) {
 448                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 449                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 450                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 451             } else {
 452                 if ( n & 1 ) {
 453                     sgdCopyVec3( tri[0], tri[2] );
 454                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 455                 } else {
 456                     sgdCopyVec3( tri[1], tri[2] );
 457                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 458                 }
 459             }
 460             break;
 461         case GL_QUADS :
 462             SG_LOG( SG_TERRAIN, SG_ALERT,
 463                     "WARNING: dubiously handled GL_QUADS" );
 464             sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
 465             sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
 466             sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
 467             break;
 468         default:
 469             SG_LOG( SG_TERRAIN, SG_ALERT,
 470                     "WARNING: not-handled structure: " << primType );
 471             return IntersectLeaf( leaf, m, orig, dir);
 472         }
 473
 474         if( isZeroAreaTri( tri ) )
 475             continue;
 476
 477         sgdVec4 plane;
 478         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 479
 480         sgdVec3 point;
 481
 482         // find point of intersection of line from point org
 483         // in direction dir with triangle's plane
 484         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 485         /* Is line parallel to plane? */
 486         if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 487             continue ;
 488
 489         // find parametric point
 490         sgdScaleVec3 ( point, dir,
 491                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 492                        / tmp ) ;
 493
 494         // short circut if this point is further away then a previous hit
 495         tmp_dist = sgdDistanceSquaredVec3(point, orig );
 496         if( tmp_dist > test_dist )
 497             continue;
 498
 499         // place parametric point in world
 500         sgdAddVec3 ( point, orig ) ;
 501
 502         if( fgdPointInTriangle( point, tri ) ) {
 503             // transform point into passed coordinate frame
 504             sgdXformPnt3( point, point, m );
 505             sgdXformPnt4(plane,plane,m);
 506             add(leaf,n,point,plane);
 507             test_dist = tmp_dist;
 508             num_hits++;
 509         }
 510     }
 511     return num_hits;
 512 }
 513
 514
 515
 516 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
 517     return ( sgdScalarProductVec3(v, v) < (radius*radius) );
 518 }
 519
 520
 521 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 522                                  sgdVec3 orig, sgdVec3 dir )
 523 {
 524     /* the lookat vector and matrix in branch's coordinate frame but
 525      * we won't determine these unless needed, This 'lazy evaluation'
 526      * is a result of profiling data */
 527     sgdVec3 orig_leaf, dir_leaf;
 528     sgdMat4 m_leaf;
 529
 530     // 'lazy evaluation' flag
 531     int first_time = 1;
 532
 533     for ( ssgEntity *kid = branch->getKid( 0 );
 534             kid != NULL;
 535             kid = branch->getNextKid() )
 536     {
 537         if ( kid->getTraversalMask() & SSGTRAV_HOT
 538              && !kid->getBSphere()->isEmpty() )
 539         {
 540             sgdVec3 center;
 541             const sgFloat *BSCenter = kid->getBSphere()->getCenter();
 542             sgdSetVec3( center,
 543                         BSCenter[0],
 544                         BSCenter[1],
 545                         BSCenter[2] );
 546             sgdXformPnt3( center, m ) ;
 547
 548             // sgdClosestPointToLineDistSquared( center, orig, dir )
 549             // inlined here because because of profiling results
 550             sgdVec3 u, u1, v;
 551             sgdSubVec3(u, center, orig);
 552             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)  );
 553             sgdSubVec3(v, u, u1);
 554
 555             // double because of possible overflow
 556             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
 557             {
 558                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
 559                 {
 560                     sgdMat4 m_new;
 561                     if ( kid->isA( ssgTypeTransform() ) )
 562                     {
 563                         sgMat4 fxform;
 564                         ((ssgTransform *)kid)->getTransform( fxform );
 565                         sgMultMat4(m_new, m, fxform);
 566                     } else {
 567                         sgdCopyMat4(m_new, m);
 568                     }
 569                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 570                 }
 571                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
 572                 {
 573                     if ( first_time ) {
 574                         sgdTransposeNegateMat4( m_leaf, m );
 575                         sgdXformPnt3( orig_leaf, orig, m_leaf );
 576                         sgdXformVec3( dir_leaf,  dir,  m_leaf );
 577                         first_time = 0;
 578                     }
 579                     // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
 580                     // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
 581                     //                primType );
 582                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
 583                 }
 584             }  // Out of range
 585         } // branch not requested to be traversed
 586     } // end for loop
 587 }
 588
 589
 590 // a temporary hack until we get everything rewritten with sgdVec3
 591 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 592 {
 593     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 594 }
 595
 596
 597 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
 598     sgdMat4 m;
 599     clear();
 600     sgdMakeIdentMat4 ( m ) ;
 601     IntersectBranch( scene, m, orig, dir );
 602 }
 603
 604
 605 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
 606 {
 607     clear();
 608     IntersectBranch( scene, m, orig, dir );
 609 }
 610
 611
 612 // Determine scenery altitude via ssg.
 613 // returned results are in meters
 614 // static double hitlist1_time = 0.0;
 615
 616 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
 617                     sgdVec3 scenery_center,
 618                     FGHitList *hit_list,
 619                     double *terrain_elev, double *radius, double *normal)
 620 {
 621     // SGTimeStamp start; start.stamp();
 622
 623     bool result;
 624     sgdVec3 view_pos;
 625     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 626
 627     sgdVec3 orig, dir;
 628     sgdCopyVec3(orig, view_pos );
 629     sgdCopyVec3(dir, abs_view_pos );
 630
 631     sgdNormaliseVec3( dir );
 632     hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
 633                          orig, dir );
 634
 635     int this_hit = -1;
 636     int max_hit = -1;
 637     double hit_elev = -9999;
 638     double max_elev = -9999;
 639     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 640
 641     int hitcount = hit_list->num_hits();
 642     // cout << "hits = " << hitcount << endl;
 643     for ( int i = 0; i < hitcount; ++i ) {
 644         // FIXME: sgCartToGeod is slow.  Call it just once for the
 645         // "sc" point, and then handle the rest with a geodetic "up"
 646         // vector approximation.  Across one tile, this will be
 647         // acceptable.
 648         double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
 649         // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
 650         //      << lat_geod << " alt = " << alt << "  max alt = " << max_alt_m
 651         //      << endl;
 652         if ( alt > hit_elev && alt < max_alt_m ) {
 653             // cout << "  it's a keeper" << endl;
 654             hit_elev = alt;
 655             this_hit = i;
 656         }
 657         if ( alt > hit_elev ) {
 658             max_elev = alt;
 659             max_hit = i;
 660         }
 661     }
 662
 663     if ( this_hit < 0 ) {
 664         // no hits below us, take the max hit
 665         this_hit = max_hit;
 666         hit_elev = max_elev;
 667     }
 668
 669     if ( hit_elev > -9000 ) {
 670         *terrain_elev = hit_elev;
 671         *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
 672         sgVec3 tmp;
 673         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 674         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
 675         sgdSetVec3( normal, tmp );
 676         // float *up = globals->get_current_view()->get_world_up();
 677         // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
 678         /* ssgState *IntersectedLeafState =
 679               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 680         result = true;
 681     } else {
 682         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
 683         *terrain_elev = 0.0;
 684         float *up = globals->get_current_view()->get_world_up();
 685         sgdSetVec3(normal, up[0], up[1], up[2]);
 686         result = false;
 687     }
 688
 689     // SGTimeStamp finish; finish.stamp();
 690     // hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
 691     // cout << " time per call = " << hitlist1_time << endl;
 692
 693     return result;
 694 }
 695
 696
 697 // static double hitlist2_time = 0.0;
 698
 699 // Determine scenery altitude via ssg.
 700 // returned results are in meters
 701 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
 702                     sgdVec3 scenery_center,
 703                     ssgTransform *terra_transform,
 704                     FGHitList *hit_list,
 705                     double *terrain_elev, double *radius, double *normal)
 706 {
 707     // SGTimeStamp start; start.stamp();
 708
 709     bool result;
 710     sgdVec3 view_pos;
 711     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 712
 713     sgdVec3 orig, dir;
 714     sgdCopyVec3(orig, view_pos );
 715
 716     sgdCopyVec3(dir, abs_view_pos );
 717     sgdNormalizeVec3(dir);
 718
 719     sgMat4 fxform;
 720     sgMakeIdentMat4 ( fxform ) ;
 721     ssgGetEntityTransform( terra_transform, fxform );
 722
 723     sgdMat4 xform;
 724     sgdSetMat4(xform,fxform);
 725     hit_list->Intersect( terra_transform, xform, orig, dir );
 726
 727     int this_hit = -1;
 728     int max_hit = -1;
 729     double hit_elev = -9999;
 730     double max_elev = -9999;
 731     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 732
 733     int hitcount = hit_list->num_hits();
 734     // cout << "hits = " << hitcount << endl;
 735     for ( int i = 0; i < hitcount; ++i ) {
 736         // FIXME: sgCartToGeod is slow.  Call it just once for the
 737         // "sc" point, and then handle the rest with a geodetic "up"
 738         // vector approximation.  Across one tile, this will be
 739         // acceptable.
 740         double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
 741         // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
 742         //      << lat_geod << " alt = " << alt << "  max alt = " << max_alt_m
 743         //      << endl;
 744         if ( alt > hit_elev && alt < max_alt_m ) {
 745             hit_elev = alt;
 746             this_hit = i;
 747             // cout << "  it's a keeper" << endl;
 748         }
 749         if ( alt > hit_elev ) {
 750             max_elev = alt;
 751             max_hit = i;
 752         }
 753     }
 754
 755
 756     if ( this_hit < 0 ) {
 757         // no hits below us, take the max hit
 758         this_hit = max_hit;
 759         hit_elev = max_elev;
 760     }
 761
 762     if ( hit_elev > -9000 ) {
 763         *terrain_elev = hit_elev;
 764         *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
 765
 766         sgVec3 tmp;
 767         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 768         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
 769         sgdSetVec3( normal, tmp );
 770         // float *up = globals->get_current_view()->get_world_up();
 771        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
 772         /* ssgState *IntersectedLeafState =
 773               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 774         result = true;
 775     } else {
 776         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
 777         result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
 778                                 hit_list, terrain_elev, radius, normal);
 779     }
 780
 781     // SGTimeStamp finish; finish.stamp();
 782     // hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
 783     // cout << "time per call 2 = " << hitlist2_time << endl;
 784
 785     return result;
 786 }
 787