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