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 // http://www.cs.lth.se/home/Tomas_Akenine_Moller/raytri/raytri.c
 244 // http://little3d.free.fr/ressources/jgt%20Fast,%20Minumum%20Storage%20Ray-Triangle%20Intersection.htm
 245 // http://www.acm.org/jgt/papers/MollerTrumbore97/
 246
 247 /* Ray-Triangle Intersection Test Routines          */
 248 /* Different optimizations of my and Ben Trumbore's */
 249 /* code from journals of graphics tools (JGT)       */
 250 /* http://www.acm.org/jgt/                          */
 251 /* by Tomas Moller, May 2000                        */
 252
 253 /* code rewritten to do tests on the sign of the determinant */
 254 /* the division is at the end in the code                    */
 255 // cosmetics change by H.J :
 256 // make u & v locals since we don't use them, use sg functions
 257 static bool intersect_triangle(const double orig[3], const double dir[3],
 258                         const double vert0[3], const double vert1[3], const double vert2[3],
 259                         double *t)
 260 {
 261    double u, v;
 262    double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
 263
 264    const SGDfloat eps = 1e-4;
 265
 266    /* find vectors for two edges sharing vert0 */
 267    sgdSubVec3(edge1, vert1, vert0);
 268    sgdSubVec3(edge2, vert2, vert0);
 269
 270    /* begin calculating determinant - also used to calculate U parameter */
 271    sgdVectorProductVec3(pvec, dir, edge2);
 272
 273    /* if determinant is near zero, ray lies in plane of triangle */
 274    double det = sgdScalarProductVec3(edge1, pvec);
 275
 276    if (det > eps)
 277    {
 278       /* calculate distance from vert0 to ray origin */
 279       sgdSubVec3(tvec, orig, vert0);
 280
 281       /* calculate U parameter and test bounds */
 282       u = sgdScalarProductVec3(tvec, pvec);
 283       if (u < 0.0 || u > det)
 284         return false;
 285
 286       /* prepare to test V parameter */
 287       sgdVectorProductVec3(qvec, tvec, edge1);
 288
 289       /* calculate V parameter and test bounds */
 290       v = sgdScalarProductVec3(dir, qvec);
 291       if (v < 0.0 || u + v > det)
 292         return false;
 293
 294    }
 295    else if(det < -eps)
 296    {
 297       /* calculate distance from vert0 to ray origin */
 298       sgdSubVec3(tvec, orig, vert0);
 299
 300       /* calculate U parameter and test bounds */
 301       u = sgdScalarProductVec3(tvec, pvec);
 302       if (u > 0.0 || u < det)
 303         return false;
 304
 305       /* prepare to test V parameter */
 306       sgdVectorProductVec3(qvec, tvec, edge1);
 307
 308       /* calculate V parameter and test bounds */
 309       v = sgdScalarProductVec3(dir, qvec) ;
 310       if (v > 0.0 || u + v < det)
 311         return false;
 312    }
 313    else return false;  /* ray is parallell to the plane of the triangle */
 314
 315    /* calculate t, ray intersects triangle */
 316    *t = sgdScalarProductVec3(edge2, qvec) / det;
 317
 318    return true;
 319 }
 320
 321
 322 /*
 323 Find the intersection of an infinite line with a leaf the line being
 324 defined by a point and direction.
 325
 326 Variables
 327 In:
 328 ssgLeaf pointer  -- leaf
 329 qualified matrix -- m
 330 line origin      -- orig
 331 line direction   -- dir
 332 Out:
 333 result  -- intersection point
 334 normal  -- intersected tri's normal
 335
 336 Returns:
 337 true if intersection found
 338 false otherwise
 339
 340 !!! WARNING !!!
 341
 342 If you need an exhaustive list of hitpoints YOU MUST use the generic
 343 version of this function as the specialized versions will do an early
 344 out of expensive tests if the point can not be the closest one found
 345
 346 !!! WARNING !!!
 347 */
 348 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 349                               sgdVec3 orig, sgdVec3 dir )
 350 {
 351     int num_hits = 0;
 352     int i = 0;
 353
 354     for ( ; i < leaf->getNumTriangles(); ++i ) {
 355         short i1, i2, i3;
 356         leaf->getTriangle( i, &i1, &i2, &i3 );
 357
 358         sgdVec3 tri[3];
 359         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
 360         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
 361         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
 362 #if 1
 363         sgdFloat t;
 364         if( intersect_triangle( orig, dir, tri[0], tri[1], tri[2], &t) ) {
 365             sgdVec4 plane;
 366             sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 367             // t is the distance to the triangle plane
 368             // so P = Orig + t*dir
 369             sgdVec3 point;
 370             sgdAddScaledVec3( point, orig, dir, t );
 371             sgdXformPnt3( point, point, m );
 372             sgdXformPnt4(plane,plane,m);
 373             add(leaf,i,point,plane);
 374             num_hits++;
 375         }
 376 #else
 377         if( isZeroAreaTri( tri ) )
 378             continue;
 379
 380         sgdVec4 plane;
 381         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 382
 383         sgdVec3 point;
 384         if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 385             if( fgdPointInTriangle( point, tri ) ) {
 386                 // transform point into passed into desired coordinate frame
 387                 sgdXformPnt3( point, point, m );
 388                 sgdXformPnt4(plane,plane,m);
 389                 add(leaf,i,point,plane);
 390                 num_hits++;
 391             }
 392         }
 393 #endif
 394     }
 395     return num_hits;
 396 }
 397
 398
 399 // Short circuit/slightly optimized version of the full IntersectLeaf()
 400 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 401                               sgdVec3 orig, sgdVec3 dir,
 402                               GLenum primType )
 403 {
 404     double tmp_dist;
 405
 406     // number of hits but there could be more that
 407     // were not found because of short circut switch !
 408     // so you may want to use the unspecialized IntersectLeaf()
 409     int n, num_hits = 0;
 410
 411     int ntri = leaf->getNumTriangles();
 412     for ( n = 0; n < ntri; ++n )
 413     {
 414         sgdVec3 tri[3];
 415
 416         switch ( primType )
 417         {
 418         case GL_POLYGON :
 419             SG_LOG( SG_TERRAIN, SG_ALERT,
 420                     "WARNING: dubiously handled GL_POLYGON" );
 421         case GL_TRIANGLE_FAN :
 422             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 423                "IntersectLeaf: GL_TRIANGLE_FAN" ); */
 424             if ( !n ) {
 425                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 426                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 427                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 428             } else {
 429                 sgdCopyVec3( tri[1], tri[2] );
 430                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 431             }
 432             break;
 433         case GL_TRIANGLES :
 434             /* SG_LOG( SG_TERRAIN, SG_DEBUG,
 435                "IntersectLeaf: GL_TRIANGLES" ); */
 436             sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
 437             sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
 438             sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
 439             break;
 440         case GL_QUAD_STRIP :
 441             SG_LOG( SG_TERRAIN, SG_ALERT,
 442                     "WARNING: dubiously handled GL_QUAD_STRIP" );
 443         case GL_TRIANGLE_STRIP :
 444             /* SG_LOG( SG_TERRAIN, SG_ALERT,
 445                "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
 446             if ( !n ) {
 447                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
 448                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
 449                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
 450             } else {
 451                 if ( n & 1 ) {
 452                     sgdCopyVec3( tri[0], tri[2] );
 453                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 454                 } else {
 455                     sgdCopyVec3( tri[1], tri[2] );
 456                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
 457                 }
 458             }
 459             break;
 460         case GL_QUADS :
 461             SG_LOG( SG_TERRAIN, SG_ALERT,
 462                     "WARNING: dubiously handled GL_QUADS" );
 463             sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
 464             sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
 465             sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
 466             break;
 467         default:
 468             SG_LOG( SG_TERRAIN, SG_ALERT,
 469                     "WARNING: not-handled structure: " << primType );
 470             return IntersectLeaf( leaf, m, orig, dir);
 471         }
 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         // find point of intersection of line from point org
 482         // in direction dir with triangle's plane
 483         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 484         /* Is line parallel to plane? */
 485         if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
 486             continue ;
 487
 488         // find parametric point
 489         sgdScaleVec3 ( point, dir,
 490                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 491                        / tmp ) ;
 492
 493         // short circut if this point is further away then a previous hit
 494         tmp_dist = sgdDistanceSquaredVec3(point, orig );
 495         if( tmp_dist > test_dist )
 496             continue;
 497
 498         // place parametric point in world
 499         sgdAddVec3 ( point, orig ) ;
 500
 501         if( fgdPointInTriangle( point, tri ) ) {
 502             // transform point into passed coordinate frame
 503             sgdXformPnt3( point, point, m );
 504             sgdXformPnt4(plane,plane,m);
 505             add(leaf,n,point,plane);
 506             test_dist = tmp_dist;
 507             num_hits++;
 508         }
 509     }
 510     return num_hits;
 511 }
 512
 513
 514
 515 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
 516     return ( sgdScalarProductVec3(v, v) < (radius*radius) );
 517 }
 518
 519
 520 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 521                                  sgdVec3 orig, sgdVec3 dir )
 522 {
 523     /* the lookat vector and matrix in branch's coordinate frame but
 524      * we won't determine these unless needed, This 'lazy evaluation'
 525      * is a result of profiling data */
 526     sgdVec3 orig_leaf, dir_leaf;
 527     sgdMat4 m_leaf;
 528
 529     // 'lazy evaluation' flag
 530     int first_time = 1;
 531
 532     for ( ssgEntity *kid = branch->getKid( 0 );
 533             kid != NULL;
 534             kid = branch->getNextKid() )
 535     {
 536         if ( kid->getTraversalMask() & SSGTRAV_HOT
 537              && !kid->getBSphere()->isEmpty() )
 538         {
 539             sgdVec3 center;
 540             const sgFloat *BSCenter = kid->getBSphere()->getCenter();
 541             sgdSetVec3( center,
 542                         BSCenter[0],
 543                         BSCenter[1],
 544                         BSCenter[2] );
 545             sgdXformPnt3( center, m ) ;
 546
 547             // sgdClosestPointToLineDistSquared( center, orig, dir )
 548             // inlined here because because of profiling results
 549             sgdVec3 u, u1, v;
 550             sgdSubVec3(u, center, orig);
 551             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)  );
 552             sgdSubVec3(v, u, u1);
 553
 554             // double because of possible overflow
 555             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
 556             {
 557                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
 558                 {
 559                     sgdMat4 m_new;
 560                     if ( kid->isA( ssgTypeTransform() ) )
 561                     {
 562                         sgMat4 fxform;
 563                         ((ssgTransform *)kid)->getTransform( fxform );
 564                         sgMultMat4(m_new, m, fxform);
 565                     } else {
 566                         sgdCopyMat4(m_new, m);
 567                     }
 568                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 569                 }
 570                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
 571                 {
 572                     if ( first_time ) {
 573                         sgdTransposeNegateMat4( m_leaf, m );
 574                         sgdXformPnt3( orig_leaf, orig, m_leaf );
 575                         sgdXformVec3( dir_leaf,  dir,  m_leaf );
 576                         first_time = 0;
 577                     }
 578                     // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
 579                     // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
 580                     //                primType );
 581                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
 582                 }
 583             }  // Out of range
 584         } // branch not requested to be traversed
 585     } // end for loop
 586 }
 587
 588
 589 // a temporary hack until we get everything rewritten with sgdVec3
 590 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 591 {
 592     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 593 }
 594
 595
 596 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
 597     sgdMat4 m;
 598     clear();
 599     sgdMakeIdentMat4 ( m ) ;
 600     IntersectBranch( scene, m, orig, dir );
 601 }
 602
 603
 604 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
 605 {
 606     clear();
 607     IntersectBranch( scene, m, orig, dir );
 608 }
 609
 610
 611 // Determine scenery altitude via ssg.
 612 // returned results are in meters
 613 // static double hitlist1_time = 0.0;
 614
 615 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
 616                     sgdVec3 scenery_center,
 617                     FGHitList *hit_list,
 618                     double *terrain_elev, double *radius, double *normal)
 619 {
 620     // SGTimeStamp start; start.stamp();
 621
 622     bool result;
 623     sgdVec3 view_pos;
 624     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 625
 626     sgdVec3 orig, dir;
 627     sgdCopyVec3(orig, view_pos );
 628     sgdCopyVec3(dir, abs_view_pos );
 629
 630     sgdNormaliseVec3( dir );
 631     hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
 632                          orig, dir );
 633
 634     int this_hit = -1;
 635     int max_hit = -1;
 636     Point3D geoc;
 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 = geoc.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     Point3D geoc;
 730     double hit_elev = -9999;
 731     double max_elev = -9999;
 732     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 733
 734     int hitcount = hit_list->num_hits();
 735     // cout << "hits = " << hitcount << endl;
 736     for ( int i = 0; i < hitcount; ++i ) {
 737         // FIXME: sgCartToGeod is slow.  Call it just once for the
 738         // "sc" point, and then handle the rest with a geodetic "up"
 739         // vector approximation.  Across one tile, this will be
 740         // acceptable.
 741         double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
 742         // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
 743         //      << lat_geod << " alt = " << alt << "  max alt = " << max_alt_m
 744         //      << endl;
 745         if ( alt > hit_elev && alt < max_alt_m ) {
 746             hit_elev = alt;
 747             this_hit = i;
 748             // cout << "  it's a keeper" << endl;
 749         }
 750         if ( alt > hit_elev ) {
 751             max_elev = alt;
 752             max_hit = i;
 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 = geoc.radius();
 765         sgVec3 tmp;
 766         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 767         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
 768         sgdSetVec3( normal, tmp );
 769         // float *up = globals->get_current_view()->get_world_up();
 770        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
 771         /* ssgState *IntersectedLeafState =
 772               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 773         result = true;
 774     } else {
 775         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
 776         result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
 777                                 hit_list, terrain_elev, radius, normal);
 778     }
 779
 780     // SGTimeStamp finish; finish.stamp();
 781     // hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
 782     // cout << "time per call 2 = " << hitlist2_time << endl;
 783
 784     return result;
 785 }
 786