src/Scenery/hitlist.cxx

   1 #ifdef HAVE_CONFIG_H
   2 #  include <config.h>
   3 #endif
   4
   5 #ifdef HAVE_WINDOWS_H
   6 #  include <windows.h>
   7 #endif
   8
   9 #include <float.h>
  10 #include <math.h>
  11
  12 #include <GL/glut.h>
  13 #include <GL/gl.h>
  14
  15 #include <plib/sg.h>
  16
  17 #include <simgear/constants.h>
  18 #include <simgear/sg_inlines.h>
  19 #include <simgear/debug/logstream.hxx>
  20 #include <simgear/math/point3d.hxx>
  21 #include <simgear/math/sg_geodesy.hxx>
  22 #include <simgear/math/vector.hxx>
  23
  24 #include <Main/globals.hxx>
  25 #include <Main/viewer.hxx>
  26
  27 #include "hitlist.hxx"
  28
  29
  30 extern ssgBranch *terrain_branch;
  31
  32 #if 0
  33 // check to see if the intersection point is
  34 // actually inside this face
  35 static bool pointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
  36 {
  37     double xmin, xmax, ymin, ymax, zmin, zmax;
  38
  39     // punt if outside bouding cube
  40     if ( point[0] < (xmin = SG_MIN3 (tri[0][0], tri[1][0], tri[2][0])) ) {
  41         return false;
  42     } else if ( point[0] > (xmax = SG_MAX3 (tri[0][0], tri[1][0], tri[2][0])) )
  43     {
  44         return false;
  45     } else if ( point[1] < (ymin = SG_MIN3 (tri[0][1], tri[1][1], tri[2][1])) )
  46     {
  47         return false;
  48     } else if ( point[1] > (ymax = SG_MAX3 (tri[0][1], tri[1][1], tri[2][1])) )
  49     {
  50         return false;
  51     } else if ( point[2] < (zmin = SG_MIN3 (tri[0][2], tri[1][2], tri[2][2])) )
  52     {
  53         return false;
  54     } else if ( point[2] > (zmax = SG_MAX3 (tri[0][2], tri[1][2], tri[2][2])) )
  55     {
  56         return false;
  57     }
  58
  59     // (finally) check to see if the intersection point is
  60     // actually inside this face
  61
  62     //first, drop the smallest dimension so we only have to work
  63     //in 2d.
  64     double dx = xmax - xmin;
  65     double dy = ymax - ymin;
  66     double dz = zmax - zmin;
  67     double min_dim = SG_MIN3 (dx, dy, dz);
  68
  69     //first, drop the smallest dimension so we only have to work
  70     //in 2d.
  71     double x1, y1, x2, y2, x3, y3, rx, ry;
  72     if ( fabs(min_dim-dx) <= SG_EPSILON ) {
  73         // x is the smallest dimension
  74         x1 = point[1];
  75         y1 = point[2];
  76         x2 = tri[0][1];
  77         y2 = tri[0][2];
  78         x3 = tri[1][1];
  79         y3 = tri[1][2];
  80         rx = tri[2][1];
  81         ry = tri[2][2];
  82     } else if ( fabs(min_dim-dy) <= SG_EPSILON ) {
  83         // y is the smallest dimension
  84         x1 = point[0];
  85         y1 = point[2];
  86         x2 = tri[0][0];
  87         y2 = tri[0][2];
  88         x3 = tri[1][0];
  89         y3 = tri[1][2];
  90         rx = tri[2][0];
  91         ry = tri[2][2];
  92     } else if ( fabs(min_dim-dz) <= SG_EPSILON ) {
  93         // z is the smallest dimension
  94         x1 = point[0];
  95         y1 = point[1];
  96         x2 = tri[0][0];
  97         y2 = tri[0][1];
  98         x3 = tri[1][0];
  99         y3 = tri[1][1];
 100         rx = tri[2][0];
 101         ry = tri[2][1];
 102     } else {
 103         // all dimensions are really small so lets call it close
 104         // enough and return a successful match
 105         return true;
 106     }
 107
 108     // check if intersection point is on the same side of p1 <-> p2 as p3
 109     double tmp = (y2 - y3) / (x2 - x3);
 110     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
 111     int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
 112     if ( side1 != side2 ) {
 113         // printf("failed side 1 check\n");
 114         return false;
 115     }
 116
 117     // check if intersection point is on correct side of p2 <-> p3 as p1
 118     tmp = (y3 - ry) / (x3 - rx);
 119     side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
 120     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 121     if ( side1 != side2 ) {
 122         // printf("failed side 2 check\n");
 123         return false;
 124     }
 125
 126     // check if intersection point is on correct side of p1 <-> p3 as p2
 127     tmp = (y2 - ry) / (x2 - rx);
 128     side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
 129     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 130     if ( side1 != side2 ) {
 131         // printf("failed side 3  check\n");
 132         return false;
 133     }
 134
 135     return true;
 136 }
 137
 138 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
 139                                  const sgdVec3 l_vec, const sgdVec4 plane )
 140 {
 141     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
 142
 143   /* Is line parallel to plane? */
 144
 145     if ( fabs ( tmp ) < FLT_EPSILON )
 146         return false ;
 147
 148     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
 149                                  + plane[3] ) / tmp ) ;
 150     sgdAddVec3  ( dst, l_org ) ;
 151
 152     return true ;
 153 }
 154
 155
 156 static void sgdXformPnt3 ( sgdVec3 dst, const sgVec3 src, const sgdMat4 mat )
 157 {
 158     SGDfloat t0 = src[ 0 ] ;
 159     SGDfloat t1 = src[ 1 ] ;
 160     SGDfloat t2 = src[ 2 ] ;
 161
 162     dst[0] = ( t0 * mat[ 0 ][ 0 ] +
 163                t1 * mat[ 1 ][ 0 ] +
 164                t2 * mat[ 2 ][ 0 ] +
 165                mat[ 3 ][ 0 ] ) ;
 166
 167     dst[1] = ( t0 * mat[ 0 ][ 1 ] +
 168                t1 * mat[ 1 ][ 1 ] +
 169                t2 * mat[ 2 ][ 1 ] +
 170                mat[ 3 ][ 1 ] ) ;
 171
 172     dst[2] = ( t0 * mat[ 0 ][ 2 ] +
 173                t1 * mat[ 1 ][ 2 ] +
 174                t2 * mat[ 2 ][ 2 ] +
 175                mat[ 3 ][ 2 ] ) ;
 176 }
 177 #endif // 0
 178
 179
 180 /*
 181    Find the intersection of an infinite line with a plane
 182    (the line being defined by a point and direction).
 183
 184    Norman Vine -- nhv@yahoo.com  (with hacks by Steve)
 185 */
 186
 187 int sgdIsectInfLinePlane( sgdVec3 dst, sgdVec3 l_org,
 188                           sgdVec3 l_vec, sgdVec4 plane )
 189 {
 190   SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
 191
 192   /* Is line parallel to plane? */
 193
 194   if ( sgdAbs ( tmp ) < DBL_EPSILON )
 195     return FALSE ;
 196
 197   sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
 198                                 + plane[3] ) / tmp ) ;
 199   sgdAddVec3  ( dst, l_org ) ;
 200
 201   return TRUE ;
 202 }
 203
 204
 205 /*
 206  * Given a point and a triangle lying on the same plane
 207  * check to see if the point is inside the triangle
 208  */
 209 bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
 210 {
 211     sgdVec3 dif;
 212
 213     SGDfloat min, max;
 214     // punt if outside bouding cube
 215     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
 216     if( (point[0] < min) || (point[0] > max) )
 217         return false;
 218     dif[0] = max - min;
 219
 220     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
 221     if( (point[1] < min) || (point[1] > max) )
 222         return false;
 223     dif[1] = max - min;
 224
 225     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
 226     if( (point[2] < min) || (point[2] > max) )
 227         return false;
 228     dif[2] = max - min;
 229
 230     // drop the smallest dimension so we only have to work in 2d.
 231     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
 232     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
 233     if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
 234         // x is the smallest dimension
 235         x1 = point[1];
 236         y1 = point[2];
 237         x2 = tri[0][1];
 238         y2 = tri[0][2];
 239         x3 = tri[1][1];
 240         y3 = tri[1][2];
 241         rx = tri[2][1];
 242         ry = tri[2][2];
 243     } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
 244         // y is the smallest dimension
 245         x1 = point[0];
 246         y1 = point[2];
 247         x2 = tri[0][0];
 248         y2 = tri[0][2];
 249         x3 = tri[1][0];
 250         y3 = tri[1][2];
 251         rx = tri[2][0];
 252         ry = tri[2][2];
 253     } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
 254         // z is the smallest dimension
 255         x1 = point[0];
 256         y1 = point[1];
 257         x2 = tri[0][0];
 258         y2 = tri[0][1];
 259         x3 = tri[1][0];
 260         y3 = tri[1][1];
 261         rx = tri[2][0];
 262         ry = tri[2][1];
 263     } else {
 264         // all dimensions are really small so lets call it close
 265         // enough and return a successful match
 266         return true;
 267     }
 268
 269     // check if intersection point is on the same side of p1 <-> p2 as p3
 270     SGDfloat tmp = (y2 - y3) / (x2 - x3);
 271     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
 272     int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
 273     if ( side1 != side2 ) {
 274         // printf("failed side 1 check\n");
 275         return false;
 276     }
 277
 278     // check if intersection point is on correct side of p2 <-> p3 as p1
 279     tmp = (y3 - ry) / (x3 - rx);
 280     side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
 281     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 282     if ( side1 != side2 ) {
 283         // printf("failed side 2 check\n");
 284         return false;
 285     }
 286
 287     // check if intersection point is on correct side of p1 <-> p3 as p2
 288     tmp = (y2 - ry) / (x2 - rx);
 289     side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
 290     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 291     if ( side1 != side2 ) {
 292         // printf("failed side 3  check\n");
 293         return false;
 294     }
 295
 296     return true;
 297 }
 298
 299
 300 /*
 301    Find the intersection of an infinite line with a leaf
 302    the line being defined by a point and direction.
 303
 304    Variables
 305     In:
 306      ssgLeaf pointer  -- leaf
 307      qualified matrix -- m
 308      line origin      -- orig
 309      line direction   -- dir
 310     Out:
 311      result  -- intersection point
 312      normal  -- intersected tri's normal
 313
 314    Returns:
 315     true if intersection found
 316     false otherwise
 317 */
 318 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 319                               sgdVec3 orig, sgdVec3 dir )
 320 {
 321     int num_hits = 0;
 322     for ( int i = 0; i < leaf->getNumTriangles(); ++i ) {
 323         short i1, i2, i3;
 324         leaf->getTriangle( i, &i1, &i2, &i3 );
 325
 326         sgdVec3 tri[3];
 327         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
 328         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
 329         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
 330
 331         //avoid division by zero when two points are the same
 332         if ( sgdEqualVec3(tri[0], tri[1]) ||
 333              sgdEqualVec3(tri[1], tri[2]) ||
 334              sgdEqualVec3(tri[2], tri[0]) ) {
 335             continue;
 336         }
 337
 338         sgdVec4 plane;
 339         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 340
 341         sgdVec3 point;
 342
 343         //inlined IsectInfLinePlane( point dst, orig, dir, plane )
 344         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
 345
 346         /* Is line parallel to plane? */
 347         if ( sgdAbs ( tmp ) < DBL_EPSILON )
 348             continue ;
 349
 350         sgdScaleVec3 ( point, dir,
 351                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
 352                        / tmp ) ;
 353
 354         sgdAddVec3  ( point, orig ) ;
 355         // end of inlined intersection
 356 #if 0
 357         if( pointInTriangle( point, tri ) ) {
 358             add(leaf,i,point,plane);
 359             num_hits++;
 360         }
 361 #endif // 0
 362         if( sgdPointInTriangle( point, tri ) ) {
 363             // transform point into passed into desired coordinate frame
 364             sgdXformPnt3( point, point, m );
 365             add(leaf,i,point,plane);
 366             num_hits++;
 367         }
 368     }
 369     return num_hits;
 370 }
 371
 372
 373 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 374                                  sgdVec3 orig, sgdVec3 dir )
 375 {
 376     sgSphere *bsphere;
 377
 378     /* the lookat vector and matrix in branch's coordinate frame
 379      * but we won't determine these unless needed to,
 380      * This 'lazy evaluation' is a result of profiling data */
 381     sgdVec3 _orig, _dir;
 382     sgdMat4 _m;
 383
 384     // 'lazy evaluation' flag
 385     int first_time = 1;
 386
 387     for ( ssgEntity *kid = branch->getKid( 0 );
 388           kid != NULL;
 389           kid = branch->getNextKid() )
 390     {
 391         if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
 392             bsphere = kid->getBSphere();
 393             sgdVec3         center;
 394             sgdSetVec3( center,
 395                         bsphere->getCenter()[0],
 396                         bsphere->getCenter()[1],
 397                         bsphere->getCenter()[2] );
 398             sgdXformPnt3( center, m ) ;
 399
 400             // sgdClosestPointToLineDistSquared( center, orig, dir )
 401             // inlined here because because of profiling results
 402             sgdVec3 u, u1, v;
 403             sgdSubVec3(u, center, orig);
 404             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
 405                           / sgdScalarProductVec3(dir,dir) );
 406             sgdSubVec3(v, u, u1);
 407
 408             // doubles because of possible overflow
 409 #define SQUARE(x) (x*x)
 410             if( sgdScalarProductVec3(v, v)
 411                 < SQUARE( double(bsphere->getRadius()) ) )
 412             {
 413                 // possible intersections
 414                 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
 415                     sgdMat4 m_new;
 416                     sgdCopyMat4(m_new, m);
 417                     if ( kid->isA( ssgTypeTransform() ) ) {
 418                         sgMat4 fxform;
 419                         ((ssgTransform *)kid)->getTransform( fxform );
 420                         sgdMat4 xform;
 421                         sgdSetMat4( xform, fxform );
 422                         sgdPreMultMat4( m_new, xform );
 423                     }
 424                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 425                 } else if ( kid->isAKindOf( ssgTypeLeaf() ) ) {
 426                     if( first_time) {
 427                         // OK we need these
 428                         sgdTransposeNegateMat4( _m, m);
 429                         sgdXformPnt3( _orig, orig, _m );
 430                         sgdXformPnt3( _dir,  dir,  _m );
 431                         first_time = 0;
 432                     }
 433                     IntersectLeaf( (ssgLeaf *)kid, m, _orig, _dir );
 434                 }
 435             } else {
 436                 // end of the line for this branch
 437             }
 438         } else {
 439             // branch requested not to be traversed
 440         }
 441     }
 442 }
 443
 444
 445 // This expects the inital m to the identity transform
 446 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
 447 {
 448     for ( ssgEntity *parent = branch->getParent(0);
 449           parent != NULL;
 450           parent = parent->getNextParent() )
 451     {
 452         // recurse till we are at the scene root
 453         // then just unwinding the stack should
 454         // give us our cumulative transform :-)  NHV
 455         ssgGetEntityTransform( parent, m );
 456         if ( parent->isA( ssgTypeTransform() ) ) {
 457             sgMat4 xform;
 458             ((ssgTransform *)parent)->getTransform( xform );
 459             sgPreMultMat4( m, xform );
 460         }
 461     }
 462 }
 463
 464
 465 // return the passed entitity's bsphere's center point radius and
 466 // fully formed current model matrix for entity
 467 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
 468                                                    float *radius, sgMat4 m )
 469 {
 470     sgSphere *bsphere = entity->getBSphere();
 471     *radius = (double)bsphere->getRadius();
 472     sgCopyVec3( center, bsphere->getCenter() );
 473     sgMakeIdentMat4 ( m ) ;
 474     ssgGetEntityTransform( entity, m );
 475 }
 476
 477
 478 void FGHitList::IntersectCachedLeaf( sgdMat4 m,
 479                                      sgdVec3 orig, sgdVec3 dir)
 480 {
 481     if ( last_hit() ) {
 482         float radius;
 483         sgVec3 fcenter;
 484         sgMat4 fxform;
 485         // ssgEntity *ent = last_hit();
 486         ssgGetCurrentBSphere( last_hit(), fcenter, &radius, fxform );
 487         sgdMat4 m;
 488         sgdVec3 center;
 489         sgdSetMat4( m, fxform );
 490         sgdXformPnt3( center, m );
 491
 492         if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
 493              double(radius*radius) )
 494         {
 495             IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
 496         }
 497     }
 498 }
 499
 500
 501 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
 502     sgdMat4 m;
 503
 504 // #define USE_CACHED_HIT
 505
 506 #ifdef USE_CACHED_HIT
 507     // This optimization gives a slight speedup
 508     // but it precludes using the hitlist for dynamic
 509     // objects  NHV
 510     init();
 511     if( last_hit() ) {
 512         sgdMakeIdentMat4 ( m ) ;
 513         IntersectCachedLeaf(m, orig, dir);
 514     }
 515     if( ! num_hits() ) {
 516 #endif
 517
 518         clear();
 519         sgdMakeIdentMat4 ( m ) ;
 520         IntersectBranch( scene, m, orig, dir);
 521
 522 #ifdef USE_CACHED_HIT
 523     }
 524 #endif
 525 }
 526
 527
 528 static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
 529     sgVec3 tmp;
 530     sgSetVec3(tmp, src[0], src[1], src[2] );
 531     sgMat4 TMP;
 532     sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
 533     sgXformVec3(tmp, tmp, TMP);
 534     sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
 535 }
 536
 537
 538 // a temporary hack until we get everything rewritten with sgdVec3
 539 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 540 {
 541     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 542 }
 543
 544
 545 // Determine scenery altitude via ssg.  Normally this just happens
 546 // when we render the scene, but we'd also like to be able to do this
 547 // explicitely.  lat & lon are in radians.  view_pos in current world
 548 // coordinate translated near (0,0,0) (in meters.)  Returns result in
 549 // meters.
 550 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
 551                     FGHitList *hit_list,
 552                     double *terrain_elev, double *radius, double *normal)
 553 {
 554     sgdVec3 view_pos;
 555     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 556
 557     sgdVec3 orig, dir;
 558     sgdCopyVec3(orig, view_pos );
 559     sgdCopyVec3(dir, abs_view_pos );
 560
 561     hit_list->Intersect( terrain_branch, orig, dir );
 562
 563     int this_hit=0;
 564     Point3D geoc;
 565     double result = -9999;
 566     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 567
 568     int hitcount = hit_list->num_hits();
 569     for ( int i = 0; i < hitcount; ++i ) {
 570         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
 571         double lat_geod, alt, sea_level_r;
 572         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
 573                      &alt, &sea_level_r);
 574         if ( alt > result && alt < 10000 ) {
 575             result = alt;
 576             this_hit = i;
 577         }
 578     }
 579
 580     if ( result > -9000 ) {
 581         *terrain_elev = result;
 582         *radius = geoc.radius();
 583         sgVec3 tmp;
 584         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 585         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
 586         //      << tmp[2] << endl;
 587         /* ssgState *IntersectedLeafState =
 588            ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 589         CurrentNormalInLocalPlane(tmp, tmp);
 590         sgdSetVec3( normal, tmp );
 591         // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
 592         return true;
 593     } else {
 594         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
 595         *terrain_elev = 0.0;
 596         float *up = globals->get_current_view()->get_world_up();
 597         sgdSetVec3(normal, up[0], up[1], up[2]);
 598         return false;
 599     }
 600 }