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 <simgear/constants.h>
  16 #include <simgear/sg_inlines.h>
  17 #include <simgear/debug/logstream.hxx>
  18 #include <simgear/math/point3d.hxx>
  19 #include <simgear/math/sg_geodesy.hxx>
  20 #include <simgear/math/vector.hxx>
  21
  22 #include <Main/globals.hxx>
  23 #include <Main/viewer.hxx>
  24
  25 #include "hitlist.hxx"
  26
  27
  28 extern ssgBranch *terrain;
  29
  30
  31 // check to see if the intersection point is
  32 // actually inside this face
  33 static bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
  34 {
  35     double xmin, xmax, ymin, ymax, zmin, zmax;
  36
  37     // punt if outside bouding cube
  38     if ( point[0] < (xmin = SG_MIN3 (tri[0][0], tri[1][0], tri[2][0])) ) {
  39         return false;
  40     } else if ( point[0] > (xmax = SG_MAX3 (tri[0][0], tri[1][0], tri[2][0])) ) {
  41         return false;
  42     } else if ( point[1] < (ymin = SG_MIN3 (tri[0][1], tri[1][1], tri[2][1])) ) {
  43         return false;
  44     } else if ( point[1] > (ymax = SG_MAX3 (tri[0][1], tri[1][1], tri[2][1])) ) {
  45         return false;
  46     } else if ( point[2] < (zmin = SG_MIN3 (tri[0][2], tri[1][2], tri[2][2])) ) {
  47         return false;
  48     } else if ( point[2] > (zmax = SG_MAX3 (tri[0][2], tri[1][2], tri[2][2])) ) {
  49         return false;
  50     }
  51
  52     // (finally) check to see if the intersection point is
  53     // actually inside this face
  54
  55     //first, drop the smallest dimension so we only have to work
  56     //in 2d.
  57     double dx = xmax - xmin;
  58     double dy = ymax - ymin;
  59     double dz = zmax - zmin;
  60     double min_dim = SG_MIN3 (dx, dy, dz);
  61
  62     //first, drop the smallest dimension so we only have to work
  63     //in 2d.
  64     double x1, y1, x2, y2, x3, y3, rx, ry;
  65     if ( fabs(min_dim-dx) <= SG_EPSILON ) {
  66         // x is the smallest dimension
  67         x1 = point[1];
  68         y1 = point[2];
  69         x2 = tri[0][1];
  70         y2 = tri[0][2];
  71         x3 = tri[1][1];
  72         y3 = tri[1][2];
  73         rx = tri[2][1];
  74         ry = tri[2][2];
  75     } else if ( fabs(min_dim-dy) <= SG_EPSILON ) {
  76         // y is the smallest dimension
  77         x1 = point[0];
  78         y1 = point[2];
  79         x2 = tri[0][0];
  80         y2 = tri[0][2];
  81         x3 = tri[1][0];
  82         y3 = tri[1][2];
  83         rx = tri[2][0];
  84         ry = tri[2][2];
  85     } else if ( fabs(min_dim-dz) <= SG_EPSILON ) {
  86         // z is the smallest dimension
  87         x1 = point[0];
  88         y1 = point[1];
  89         x2 = tri[0][0];
  90         y2 = tri[0][1];
  91         x3 = tri[1][0];
  92         y3 = tri[1][1];
  93         rx = tri[2][0];
  94         ry = tri[2][1];
  95     } else {
  96         // all dimensions are really small so lets call it close
  97         // enough and return a successful match
  98         return true;
  99     }
 100
 101     // check if intersection point is on the same side of p1 <-> p2 as p3
 102     double tmp = (y2 - y3) / (x2 - x3);
 103     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
 104     int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
 105     if ( side1 != side2 ) {
 106         // printf("failed side 1 check\n");
 107         return false;
 108     }
 109
 110     // check if intersection point is on correct side of p2 <-> p3 as p1
 111     tmp = (y3 - ry) / (x3 - rx);
 112     side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
 113     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 114     if ( side1 != side2 ) {
 115         // printf("failed side 2 check\n");
 116         return false;
 117     }
 118
 119     // check if intersection point is on correct side of p1 <-> p3 as p2
 120     tmp = (y2 - ry) / (x2 - rx);
 121     side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
 122     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
 123     if ( side1 != side2 ) {
 124         // printf("failed side 3  check\n");
 125         return false;
 126     }
 127
 128     return true;
 129 }
 130
 131 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
 132                                  const sgdVec3 l_vec, const sgdVec4 plane )
 133 {
 134     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
 135
 136   /* Is line parallel to plane? */
 137
 138     if ( fabs ( tmp ) < FLT_EPSILON )
 139         return false ;
 140
 141     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
 142                                  + plane[3] ) / tmp ) ;
 143     sgdAddVec3  ( dst, l_org ) ;
 144
 145     return true ;
 146 }
 147
 148
 149 static void sgdXformPnt3 ( sgdVec3 dst, const sgVec3 src, const sgdMat4 mat )
 150 {
 151     SGDfloat t0 = src[ 0 ] ;
 152     SGDfloat t1 = src[ 1 ] ;
 153     SGDfloat t2 = src[ 2 ] ;
 154
 155     dst[0] = ( t0 * mat[ 0 ][ 0 ] +
 156                t1 * mat[ 1 ][ 0 ] +
 157                t2 * mat[ 2 ][ 0 ] +
 158                mat[ 3 ][ 0 ] ) ;
 159
 160     dst[1] = ( t0 * mat[ 0 ][ 1 ] +
 161                t1 * mat[ 1 ][ 1 ] +
 162                t2 * mat[ 2 ][ 1 ] +
 163                mat[ 3 ][ 1 ] ) ;
 164
 165     dst[2] = ( t0 * mat[ 0 ][ 2 ] +
 166                t1 * mat[ 1 ][ 2 ] +
 167                t2 * mat[ 2 ][ 2 ] +
 168                mat[ 3 ][ 2 ] ) ;
 169 }
 170
 171 /*
 172    Find the intersection of an infinite line with a leaf
 173    the line being defined by a point and direction.
 174
 175    Variables
 176     In:
 177      ssgLeaf pointer  -- leaf
 178      qualified matrix -- m
 179      line origin      -- orig
 180      line direction   -- dir
 181     Out:
 182      result  -- intersection point
 183      normal  -- intersected tri's normal
 184
 185    Returns:
 186     true if intersection found
 187     false otherwise
 188 */
 189 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 190                                                           sgdVec3 orig, sgdVec3 dir )
 191 {
 192     int num_hits = 0;
 193     for ( int i = 0; i < leaf->getNumTriangles(); ++i ) {
 194         short i1, i2, i3;
 195         leaf->getTriangle( i, &i1, &i2, &i3 );
 196
 197         sgdVec3 tri[3];
 198         sgdXformPnt3( tri[0], leaf->getVertex( i1 ), m );
 199         sgdXformPnt3( tri[1], leaf->getVertex( i2 ), m );
 200         sgdXformPnt3( tri[2], leaf->getVertex( i3 ), m );
 201
 202         //avoid division by zero when two points are the same
 203         if ( sgdEqualVec3(tri[0], tri[1]) ||
 204              sgdEqualVec3(tri[1], tri[2]) ||
 205              sgdEqualVec3(tri[2], tri[0]) ) {
 206             continue;
 207         }
 208
 209         sgdVec4 plane;
 210         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 211
 212         sgdVec3 point;
 213         if( sgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 214             if( sgdPointInTriangle( point, tri ) ) {
 215                 add(leaf,i,point,plane);
 216                 num_hits++;
 217             }
 218         }
 219     }
 220     return num_hits;
 221 }
 222
 223 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 224                                  sgdVec3 orig, sgdVec3 dir )
 225 {
 226     sgSphere *bsphere;
 227     for ( ssgEntity *kid = branch->getKid( 0 );
 228           kid != NULL;
 229           kid = branch->getNextKid() )
 230     {
 231         if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
 232             bsphere = kid->getBSphere();
 233             sgVec3 fcenter;
 234             sgCopyVec3( fcenter, bsphere->getCenter() );
 235             sgdVec3 center;
 236             center[0] = fcenter[0];
 237             center[1] = fcenter[1];
 238             center[2] = fcenter[2];
 239             sgdXformPnt3( center, m ) ;
 240             // watch out for overflow
 241             if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
 242                  double(bsphere->getRadius() * bsphere->getRadius()) )
 243             {
 244                 // possible intersections
 245                 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
 246                     sgdMat4 m_new;
 247                     sgdCopyMat4(m_new, m);
 248                     if ( kid->isA( ssgTypeTransform() ) ) {
 249                         sgMat4 fxform;
 250                         ((ssgTransform *)kid)->getTransform( fxform );
 251                         sgdMat4 xform;
 252                         sgdSetMat4( xform, fxform );
 253                         sgdPreMultMat4( m_new, xform );
 254                     }
 255                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 256                 } else if ( kid->isAKindOf ( ssgTypeLeaf() ) ) {
 257                     IntersectLeaf( (ssgLeaf *)kid, m, orig, dir );
 258                 }
 259             } else {
 260                 // end of the line for this branch
 261             }
 262         } else {
 263             // branch requested not to be traversed
 264         }
 265     }
 266 }
 267
 268
 269 // This expects the inital m to the identity transform
 270 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
 271 {
 272     for ( ssgEntity *parent = branch->getParent(0);
 273           parent != NULL;
 274           parent = parent->getNextParent() )
 275     {
 276         // recurse till we are at the scene root
 277         // then just unwinding the stack should
 278         // give us our cumulative transform :-)  NHV
 279         ssgGetEntityTransform( parent, m );
 280         if ( parent->isA( ssgTypeTransform() ) ) {
 281             sgMat4 xform;
 282             ((ssgTransform *)parent)->getTransform( xform );
 283             sgPreMultMat4( m, xform );
 284         }
 285     }
 286 }
 287
 288
 289 // return the passed entitity's bsphere's center point radius and
 290 // fully formed current model matrix for entity
 291 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
 292                                                    float *radius, sgMat4 m )
 293 {
 294     sgSphere *bsphere = entity->getBSphere();
 295     *radius = (double)bsphere->getRadius();
 296     sgCopyVec3( center, bsphere->getCenter() );
 297     sgMakeIdentMat4 ( m ) ;
 298     ssgGetEntityTransform( entity, m );
 299 }
 300
 301
 302 void FGHitList::IntersectCachedLeaf( sgdMat4 m,
 303                                      sgdVec3 orig, sgdVec3 dir)
 304 {
 305     if ( last_hit() ) {
 306         float radius;
 307         sgVec3 fcenter;
 308         sgMat4 fxform;
 309         // ssgEntity *ent = last_hit();
 310         ssgGetCurrentBSphere( last_hit(), fcenter, &radius, fxform );
 311         sgdMat4 m;
 312         sgdVec3 center;
 313         sgdSetMat4( m, fxform );
 314         sgdXformPnt3( center, m );
 315
 316         if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
 317              double(radius*radius) )
 318         {
 319             IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
 320         }
 321     }
 322 }
 323
 324
 325 static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
 326     sgVec3 tmp;
 327     sgSetVec3(tmp, src[0], src[1], src[2] );
 328     sgMat4 TMP;
 329     sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
 330     sgXformVec3(tmp, tmp, TMP);
 331     sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
 332 }
 333
 334
 335 // a temporary hack until we get everything rewritten with sgdVec3
 336 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
 337 {
 338     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
 339 }
 340
 341
 342 // Determine scenery altitude via ssg.  Normally this just happens
 343 // when we render the scene, but we'd also like to be able to do this
 344 // explicitely.  lat & lon are in radians.  view_pos in current world
 345 // coordinate translated near (0,0,0) (in meters.)  Returns result in
 346 // meters.
 347 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
 348                     FGHitList *hit_list,
 349                     double *terrain_elev, double *radius, double *normal)
 350 {
 351     sgdVec3 view_pos;
 352     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
 353
 354     sgdVec3 orig, dir;
 355     sgdCopyVec3(orig, view_pos );
 356     sgdCopyVec3(dir, abs_view_pos );
 357
 358     hit_list->Intersect( terrain, orig, dir );
 359
 360     int this_hit=0;
 361     Point3D geoc;
 362     double result = -9999;
 363     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
 364
 365     int hitcount = hit_list->num_hits();
 366     for ( int i = 0; i < hitcount; ++i ) {
 367         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
 368         double lat_geod, alt, sea_level_r;
 369         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
 370                      &alt, &sea_level_r);
 371         if ( alt > result && alt < 10000 ) {
 372             result = alt;
 373             this_hit = i;
 374         }
 375     }
 376
 377     if ( result > -9000 ) {
 378         *terrain_elev = result;
 379         *radius = geoc.radius();
 380         sgVec3 tmp;
 381         sgSetVec3(tmp, hit_list->get_normal(this_hit));
 382         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
 383         //      << tmp[2] << endl;
 384         /* ssgState *IntersectedLeafState =
 385            ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
 386         CurrentNormalInLocalPlane(tmp, tmp);
 387         sgdSetVec3( normal, tmp );
 388         // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
 389         return true;
 390     } else {
 391         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
 392         *terrain_elev = 0.0;
 393         float *up = globals->get_current_view()->get_world_up();
 394         sgdSetVec3(normal, up[0], up[1], up[2]);
 395         return false;
 396     }
 397 }