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