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 - y1) * tmpn
 191                          + side1 * eps * fabs(tmpn));
 192     if ( side1 != side2 ) {
 193         // printf("failed side 1 check\n");
 194         return false;
 195     }
 196
 197     // check if intersection point is on correct side of p2 <-> p3 as p1
 198     tmp = (y3 - ry);
 199     tmpn = (x3 - rx);
 200     side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
 201     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 202                      + side1 * eps * fabs(tmpn));
 203     if ( side1 != side2 ) {
 204         // printf("failed side 2 check\n");
 205         return false;
 206     }
 207
 208     // check if intersection point is on correct side of p1 <-> p3 as p2
 209     tmp = (y2 - ry);
 210     tmpn = (x2 - rx);
 211     side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
 212     side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
 213                      + side1 * eps * fabs(tmpn));
 214     if ( side1 != side2 ) {
 215         // printf("failed side 3  check\n");
 216         return false;
 217     }
 218
 219     return true;
 220 }
 221
 222
 223 // Check if all three vertices are the same point (or close enough)
 224 static inline int isZeroAreaTri( sgdVec3 tri[3] )
 225 {
 226     return( sgdEqualVec3(tri[0], tri[1]) ||
 227             sgdEqualVec3(tri[1], tri[2]) ||
 228             sgdEqualVec3(tri[2], tri[0]) );
 229 }
 230
 231
 232 // Constructor
 233 FGHitList::FGHitList() :
 234     last(NULL), test_dist(DBL_MAX)
 235 {
 236 }
 237
 238
 239 // Destructor
 240 FGHitList::~FGHitList() {}
 241
 242
 243 /*
 244 Find the intersection of an infinite line with a leaf the line being
 245 defined by a point and direction.
 246
 247 Variables
 248 In:
 249 ssgLeaf pointer  -- leaf
 250 qualified matrix -- m
 251 line origin      -- orig
 252 line direction   -- dir
 253 Out:
 254 result  -- intersection point
 255 normal  -- intersected tri's normal
 256
 257 Returns:
 258 true if intersection found
 259 false otherwise
 260
 261 !!! WARNING !!!
 262
 263 If you need an exhaustive list of hitpoints YOU MUST use the generic
 264 version of this function as the specialized versions will do an early
 265 out of expensive tests if the point can not be the closest one found
 266
 267 !!! WARNING !!!
 268 */
 269 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 270                               sgdVec3 orig, sgdVec3 dir )
 271 {
 272     int num_hits = 0;
 273     int i = 0;
 274
 275     for ( ; i < leaf->getNumTriangles(); ++i ) {
 276         short i1, i2, i3;
 277         leaf->getTriangle( i, &i1, &i2, &i3 );
 278
 279         sgdVec3 tri[3];
 280         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
 281         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
 282         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
 283
 284         if( isZeroAreaTri( tri ) )
 285             continue;
 286
 287         sgdVec4 plane;
 288         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 289
 290         sgdVec3 point;
 291         if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 292             if( fgdPointInTriangle( point, tri ) ) {
 293                 // transform point into passed into desired coordinate frame
 294                 sgdXformPnt3( point, point, m );
 295                 sgdXformPnt4(plane,plane,m);
 296                 add(leaf,i,point,plane);
 297                 num_hits++;
 298             }
 299         }
 300     }
 301     return num_hits;
 302 }
 303
 304
 305 // Short circuit/slightly optimized version of the full IntersectLeaf()
 306 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 307                               sgdVec3 orig, sgdVec3 dir,
 308                               GLenum primType )
 309 {
 310     double tmp_dist;
 311
 312     // number of hits but there could be more that
 313     // were not found because of short circut switch !
 314     // so you may want to use the unspecialized IntersectLeaf()
 315     int n, num_hits = 0;
 316
 317     int ntri = leaf->getNumTriangles();
 318     for ( n = 0; n < ntri; ++n )
 319     {
 320         sgdVec3 tri[3];
 321
 322         switch ( primType )
 323         {
 324         case GL_POLYGON :
 325             SG_LOG( SG_TERRAIN, SG_ALERT,
 326                     "WARNING: dubiously handled GL_POLYGON" );
 327         case GL_TRIANGLE_FAN :
 328             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 329                "IntersectLeaf: GL_TRIANGLE_FAN" ); */
 330             if ( !n ) {
 331                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 332                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 333                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 334             } else {
 335                 sgdCopyVec3( tri[1], tri[2] );
 336                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 337             }
 338             break;
 339         case GL_TRIANGLES :
 340             /* SG_LOG( SG_TERRAIN, SG_DEBUG,
 341                "IntersectLeaf: GL_TRIANGLES" ); */
 342             sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
 343             sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
 344             sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
 345             break;
 346         case GL_QUAD_STRIP :
 347             SG_LOG( SG_TERRAIN, SG_ALERT,
 348                     "WARNING: dubiously handled GL_QUAD_STRIP" );
 349         case GL_TRIANGLE_STRIP :
 350             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 351                "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
 352             if ( !n ) {
 353                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 354                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 355                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 356             } else {
 357                 if ( n & 1 ) {
 358                     sgdCopyVec3( tri[0], tri[2] );
 359                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 360                 } else {
 361                     sgdCopyVec3( tri[1], tri[2] );
 362                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 363                 }
 364             }
 365             break;
 366         case GL_QUADS :
 367             SG_LOG( SG_TERRAIN, SG_ALERT,
 368                     "WARNING: dubiously handled GL_QUADS" );
 369             sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
 370             sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
 371             sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
 372             break;
 373         default:
 374             SG_LOG( SG_TERRAIN, SG_ALERT,
 375                     "WARNING: not-handled structure: " << primType );
 376             return IntersectLeaf( leaf, m, orig, dir);
 377         }
 378
 379         if( isZeroAreaTri( tri ) )
 380             continue;
 381
 382         sgdVec4 plane;
 383         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 384
 385         sgdVec3 point;
 386
 387         // find point of intersection of line from point org
 388         // in direction dir with triangle's plane
 389         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 390         /* Is line parallel to plane? */
 391         if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 392             continue ;
 393
 394         // find parametric point
 395         sgdScaleVec3 ( point, dir,
 396                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 397                        / tmp ) ;
 398
 399         // short circut if this point is further away then a previous hit
 400         tmp_dist = sgdDistanceSquaredVec3(point, orig );
 401         if( tmp_dist > test_dist )
 402             continue;
 403
 404         // place parametric point in world
 405         sgdAddVec3 ( point, orig ) ;
 406
 407         if( fgdPointInTriangle( point, tri ) ) {
 408             // transform point into passed coordinate frame
 409             sgdXformPnt3( point, point, m );
 410             sgdXformPnt4(plane,plane,m);
 411             add(leaf,n,point,plane);
 412             test_dist = tmp_dist;
 413             num_hits++;
 414         }
 415     }
 416     return num_hits;
 417 }
 418
 419
 420
 421 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
 422     return ( sgdScalarProductVec3(v, v) < (radius*radius) );
 423 }
 424
 425
 426 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 427                                  sgdVec3 orig, sgdVec3 dir )
 428 {
 429     /* the lookat vector and matrix in branch's coordinate frame but
 430      * we won't determine these unless needed, This 'lazy evaluation'
 431      * is a result of profiling data */
 432     sgdVec3 orig_leaf, dir_leaf;
 433     sgdMat4 m_leaf;
 434
 435     // 'lazy evaluation' flag
 436     int first_time = 1;
 437
 438     for ( ssgEntity *kid = branch->getKid( 0 );
 439             kid != NULL;
 440             kid = branch->getNextKid() )
 441     {
 442         if ( kid->getTraversalMask() & SSGTRAV_HOT
 443              && !kid->getBSphere()->isEmpty() )
 444         {
 445             sgdVec3 center;
 446             sgdSetVec3( center,
 447                         kid->getBSphere()->getCenter()[0],
 448                         kid->getBSphere()->getCenter()[1],
 449                         kid->getBSphere()->getCenter()[2] );
 450             sgdXformPnt3( center, m ) ;
 451
 452             // sgdClosestPointToLineDistSquared( center, orig, dir )
 453             // inlined here because because of profiling results
 454             sgdVec3 u, u1, v;
 455             sgdSubVec3(u, center, orig);
 456             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
 457                           / sgdScalarProductVec3(dir,dir) );
 458             sgdSubVec3(v, u, u1);
 459
 460             // double because of possible overflow
 461             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
 462             {
 463                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
 464                 {
 465                     sgdMat4 m_new;
 466                     if ( kid->isA( ssgTypeTransform() ) )
 467                     {
 468                         sgMat4 fxform;
 469                         ((ssgTransform *)kid)->getTransform( fxform );
 470                         sgMultMat4(m_new, m, fxform);
 471                     } else {
 472                         sgdCopyMat4(m_new, m);
 473                     }
 474                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 475                 }
 476                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
 477                 {
 478                     if ( first_time ) {
 479                         sgdTransposeNegateMat4( m_leaf, m );
 480                         sgdXformPnt3( orig_leaf, orig, m_leaf );
 481                         sgdXformVec3( dir_leaf,  dir,  m_leaf );
 482                         first_time = 0;
 483                     }
 484                     // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
 485                     // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
 486                     //                primType );
 487                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
 488                 }
 489             }  // Out of range
 490         } // branch not requested to be traversed
 491     } // end for loop
 492 }
 493
 494
 495 // a temporary hack until we get everything rewritten with sgdVec3
 496 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 497 {
 498     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 499 }
 500
 501
 502 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
 503     sgdMat4 m;
 504     clear();
 505     sgdMakeIdentMat4 ( m ) ;
 506     IntersectBranch( scene, m, orig, dir );
 507 }
 508
 509
 510 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
 511 {
 512     clear();
 513     IntersectBranch( scene, m, orig, dir );
 514 }
 515
 516
 517 // Determine scenery altitude via ssg.
 518 // returned results are in meters
 519 // static double hitlist1_time = 0.0;
 520
 521 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
 522                     sgdVec3 scenery_center,
 523                     FGHitList *hit_list,
 524                     double *terrain_elev, double *radius, double *normal)
 525 {
 526     // SGTimeStamp start; start.stamp();
 527
 528     bool result;
 529     sgdVec3 view_pos;
 530     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 531
 532     sgdVec3 orig, dir;
 533     sgdCopyVec3(orig, view_pos );
 534     sgdCopyVec3(dir, abs_view_pos );
 535
 536     hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
 537                          orig, dir );
 538
 539     int this_hit = -1;
 540     int max_hit = -1;
 541     Point3D geoc;
 542     double hit_elev = -9999;
 543     double max_elev = -9999;
 544     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 545
 546     int hitcount = hit_list->num_hits();
 547     // cout << "hits = " << hitcount << endl;
 548     for ( int i = 0; i < hitcount; ++i ) {
 549         // FIXME: sgCartToGeod is slow.  Call it just once for the
 550         // "sc" point, and then handle the rest with a geodetic "up"
 551         // vector approximation.  Across one tile, this will be
 552         // acceptable.
 553         double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
 554         // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
 555         //      << lat_geod << " alt = " << alt << "  max alt = " << max_alt_m
 556         //      << endl;
 557         if ( alt > hit_elev && alt < max_alt_m ) {
 558             // cout << "  it's a keeper" << endl;
 559             hit_elev = alt;
 560             this_hit = i;
 561         }
 562         if ( alt > hit_elev ) {
 563             max_elev = alt;
 564             max_hit = i;
 565         }
 566     }
 567
 568     if ( this_hit < 0 ) {
 569         // no hits below us, take the max hit
 570         this_hit = max_hit;
 571         hit_elev = max_elev;
 572     }
 573
 574     if ( hit_elev > -9000 ) {
 575         *terrain_elev = hit_elev;
 576         *radius = geoc.radius();
 577         sgVec3 tmp;
 578         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 579         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
 580         sgdSetVec3( normal, tmp );
 581         // float *up = globals->get_current_view()->get_world_up();
 582         // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
 583         /* ssgState *IntersectedLeafState =
 584               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 585         result = true;
 586     } else {
 587         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
 588         *terrain_elev = 0.0;
 589         float *up = globals->get_current_view()->get_world_up();
 590         sgdSetVec3(normal, up[0], up[1], up[2]);
 591         result = false;
 592     }
 593
 594     // SGTimeStamp finish; finish.stamp();
 595     // hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
 596     // cout << " time per call = " << hitlist1_time << endl;
 597
 598     return result;
 599 }
 600
 601
 602 // static double hitlist2_time = 0.0;
 603
 604 // Determine scenery altitude via ssg.
 605 // returned results are in meters
 606 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
 607                     sgdVec3 scenery_center,
 608                     ssgTransform *terra_transform,
 609                     FGHitList *hit_list,
 610                     double *terrain_elev, double *radius, double *normal)
 611 {
 612     // SGTimeStamp start; start.stamp();
 613
 614     bool result;
 615     sgdVec3 view_pos;
 616     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 617
 618     sgdVec3 orig, dir;
 619     sgdCopyVec3(orig, view_pos );
 620
 621     sgdCopyVec3(dir, abs_view_pos );
 622     sgdNormalizeVec3(dir);
 623
 624     sgMat4 fxform;
 625     sgMakeIdentMat4 ( fxform ) ;
 626     ssgGetEntityTransform( terra_transform, fxform );
 627
 628     sgdMat4 xform;
 629     sgdSetMat4(xform,fxform);
 630     hit_list->Intersect( terra_transform, xform, orig, dir );
 631
 632     int this_hit = -1;
 633     int max_hit = -1;
 634     Point3D geoc;
 635     double hit_elev = -9999;
 636     double max_elev = -9999;
 637     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 638
 639     int hitcount = hit_list->num_hits();
 640     // cout << "hits = " << hitcount << endl;
 641     for ( int i = 0; i < hitcount; ++i ) {
 642         // FIXME: sgCartToGeod is slow.  Call it just once for the
 643         // "sc" point, and then handle the rest with a geodetic "up"
 644         // vector approximation.  Across one tile, this will be
 645         // acceptable.
 646         double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
 647         // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
 648         //      << lat_geod << " alt = " << alt << "  max alt = " << max_alt_m
 649         //      << endl;
 650         if ( alt > hit_elev && alt < max_alt_m ) {
 651             hit_elev = alt;
 652             this_hit = i;
 653             // cout << "  it's a keeper" << endl;
 654         }
 655         if ( alt > hit_elev ) {
 656             max_elev = alt;
 657             max_hit = i;
 658         }
 659     }
 660
 661     if ( this_hit < 0 ) {
 662         // no hits below us, take the max hit
 663         this_hit = max_hit;
 664         hit_elev = max_elev;
 665     }
 666
 667     if ( hit_elev > -9000 ) {
 668         *terrain_elev = hit_elev;
 669         *radius = geoc.radius();
 670         sgVec3 tmp;
 671         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 672         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
 673         sgdSetVec3( normal, tmp );
 674         // float *up = globals->get_current_view()->get_world_up();
 675        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
 676         /* ssgState *IntersectedLeafState =
 677               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 678         result = true;
 679     } else {
 680         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
 681         result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
 682                                 hit_list, terrain_elev, radius, normal);
 683     }
 684
 685     // SGTimeStamp finish; finish.stamp();
 686     // hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
 687     // cout << "time per call 2 = " << hitlist2_time << endl;
 688
 689     return result;
 690 }
 691