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 <math.h>
  10
  11 #include <GL/glut.h>
  12
  13 #include <simgear/constants.h>
  14 #include <simgear/math/vector.hxx>
  15 #include <simgear/xgl/xgl.h>
  16
  17 #include "hitlist.hxx"
  18
  19 template <class T>
  20    inline const int FG_SIGN(const T x) {
  21     return x < T(0) ? -1 : 1;
  22 }
  23
  24 template <class T>
  25    inline const T FG_MIN(const T a, const T b) {
  26     return a < b ? a : b;
  27 }
  28
  29 // return the minimum of three values
  30 template <class T>
  31    inline const T FG_MIN3( const T a, const T b, const T c)
  32 {
  33     return (a < b ? FG_MIN (a, c) : FG_MIN (b, c));
  34 }
  35
  36 template <class T>
  37    inline const T FG_MAX(const T a, const T b) {
  38     return  a > b ? a : b;
  39 }
  40
  41 // return the maximum of three values
  42 template <class T>
  43    inline const T FG_MAX3 (const T a, const T b, const T c)
  44 {
  45     return (a > b ? FG_MAX (a, c) : FG_MAX (b, c));
  46 }
  47
  48 // check to see if the intersection point is
  49 // actually inside this face
  50 static bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
  51 {
  52     double xmin, xmax, ymin, ymax, zmin, zmax;
  53
  54     // punt if outside bouding cube
  55     if ( point[0] < (xmin = FG_MIN3 (tri[0][0], tri[1][0], tri[2][0])) ) {
  56         return false;
  57     } else if ( point[0] > (xmax = FG_MAX3 (tri[0][0], tri[1][0], tri[2][0])) ) {
  58         return false;
  59     } else if ( point[1] < (ymin = FG_MIN3 (tri[0][1], tri[1][1], tri[2][1])) ) {
  60         return false;
  61     } else if ( point[1] > (ymax = FG_MAX3 (tri[0][1], tri[1][1], tri[2][1])) ) {
  62         return false;
  63     } else if ( point[2] < (zmin = FG_MIN3 (tri[0][2], tri[1][2], tri[2][2])) ) {
  64         return false;
  65     } else if ( point[2] > (zmax = FG_MAX3 (tri[0][2], tri[1][2], tri[2][2])) ) {
  66         return false;
  67     }
  68
  69     // (finally) check to see if the intersection point is
  70     // actually inside this face
  71
  72     //first, drop the smallest dimension so we only have to work
  73     //in 2d.
  74     double dx = xmax - xmin;
  75     double dy = ymax - ymin;
  76     double dz = zmax - zmin;
  77     double min_dim = FG_MIN3 (dx, dy, dz);
  78
  79     //first, drop the smallest dimension so we only have to work
  80     //in 2d.
  81     double x1, y1, x2, y2, x3, y3, rx, ry;
  82     if ( fabs(min_dim-dx) <= FG_EPSILON ) {
  83         // x is the smallest dimension
  84         x1 = point[1];
  85         y1 = point[2];
  86         x2 = tri[0][1];
  87         y2 = tri[0][2];
  88         x3 = tri[1][1];
  89         y3 = tri[1][2];
  90         rx = tri[2][1];
  91         ry = tri[2][2];
  92     } else if ( fabs(min_dim-dy) <= FG_EPSILON ) {
  93         // y is the smallest dimension
  94         x1 = point[0];
  95         y1 = point[2];
  96         x2 = tri[0][0];
  97         y2 = tri[0][2];
  98         x3 = tri[1][0];
  99         y3 = tri[1][2];
 100         rx = tri[2][0];
 101         ry = tri[2][2];
 102     } else if ( fabs(min_dim-dz) <= FG_EPSILON ) {
 103         // z is the smallest dimension
 104         x1 = point[0];
 105         y1 = point[1];
 106         x2 = tri[0][0];
 107         y2 = tri[0][1];
 108         x3 = tri[1][0];
 109         y3 = tri[1][1];
 110         rx = tri[2][0];
 111         ry = tri[2][1];
 112     } else {
 113         // all dimensions are really small so lets call it close
 114         // enough and return a successful match
 115         return true;
 116     }
 117
 118     // check if intersection point is on the same side of p1 <-> p2 as p3
 119     double tmp = (y2 - y3) / (x2 - x3);
 120     int side1 = FG_SIGN (tmp * (rx - x3) + y3 - ry);
 121     int side2 = FG_SIGN (tmp * (x1 - x3) + y3 - y1);
 122     if ( side1 != side2 ) {
 123         // printf("failed side 1 check\n");
 124         return false;
 125     }
 126
 127     // check if intersection point is on correct side of p2 <-> p3 as p1
 128     tmp = (y3 - ry) / (x3 - rx);
 129     side1 = FG_SIGN (tmp * (x2 - rx) + ry - y2);
 130     side2 = FG_SIGN (tmp * (x1 - rx) + ry - y1);
 131     if ( side1 != side2 ) {
 132         // printf("failed side 2 check\n");
 133         return false;
 134     }
 135
 136     // check if intersection point is on correct side of p1 <-> p3 as p2
 137     tmp = (y2 - ry) / (x2 - rx);
 138     side1 = FG_SIGN (tmp * (x3 - rx) + ry - y3);
 139     side2 = FG_SIGN (tmp * (x1 - rx) + ry - y1);
 140     if ( side1 != side2 ) {
 141         // printf("failed side 3  check\n");
 142         return false;
 143     }
 144
 145     return true;
 146 }
 147
 148 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
 149                                  const sgdVec3 l_vec, const sgdVec4 plane )
 150 {
 151     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
 152
 153   /* Is line parallel to plane? */
 154
 155     if ( fabs ( tmp ) < FLT_EPSILON )
 156         return false ;
 157
 158     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
 159                                  + plane[3] ) / tmp ) ;
 160     sgdAddVec3  ( dst, l_org ) ;
 161
 162     return true ;
 163 }
 164
 165
 166 static void sgdXformPnt3 ( sgdVec3 dst, const sgVec3 src, const sgdMat4 mat )
 167 {
 168     SGDfloat t0 = src[ 0 ] ;
 169     SGDfloat t1 = src[ 1 ] ;
 170     SGDfloat t2 = src[ 2 ] ;
 171
 172     dst[0] = ( t0 * mat[ 0 ][ 0 ] +
 173                t1 * mat[ 1 ][ 0 ] +
 174                t2 * mat[ 2 ][ 0 ] +
 175                mat[ 3 ][ 0 ] ) ;
 176
 177     dst[1] = ( t0 * mat[ 0 ][ 1 ] +
 178                t1 * mat[ 1 ][ 1 ] +
 179                t2 * mat[ 2 ][ 1 ] +
 180                mat[ 3 ][ 1 ] ) ;
 181
 182     dst[2] = ( t0 * mat[ 0 ][ 2 ] +
 183                t1 * mat[ 1 ][ 2 ] +
 184                t2 * mat[ 2 ][ 2 ] +
 185                mat[ 3 ][ 2 ] ) ;
 186 }
 187
 188 /*
 189    Find the intersection of an infinite line with a leaf
 190    the line being defined by a point and direction.
 191
 192    Variables
 193     In:
 194      ssgLeaf pointer  -- leaf
 195      qualified matrix -- m
 196      line origin      -- orig
 197      line direction   -- dir
 198     Out:
 199      result  -- intersection point
 200      normal  -- intersected tri's normal
 201
 202    Returns:
 203     true if intersection found
 204     false otherwise
 205 */
 206 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
 207                                                           sgdVec3 orig, sgdVec3 dir )
 208 {
 209     int num_hits = 0;
 210     for ( int i = 0; i < leaf->getNumTriangles(); ++i ) {
 211         short i1, i2, i3;
 212         leaf->getTriangle( i, &i1, &i2, &i3 );
 213
 214         sgdVec3 tri[3];
 215         sgdXformPnt3( tri[0], leaf->getVertex( i1 ), m );
 216         sgdXformPnt3( tri[1], leaf->getVertex( i2 ), m );
 217         sgdXformPnt3( tri[2], leaf->getVertex( i3 ), m );
 218
 219         sgdVec4 plane;
 220         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
 221
 222         sgdVec3 point;
 223         if( sgdIsectInfLinePlane( point, orig, dir, plane ) ) {
 224             if( sgdPointInTriangle( point, tri ) ) {
 225                 add(leaf,i,point,plane);
 226                 num_hits++;
 227             }
 228         }
 229     }
 230     return num_hits;
 231 }
 232
 233 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
 234                                  sgdVec3 orig, sgdVec3 dir )
 235 {
 236     sgSphere *bsphere;
 237     for ( ssgEntity *kid = branch->getKid( 0 );
 238           kid != NULL;
 239           kid = branch->getNextKid() )
 240     {
 241         if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
 242             bsphere = kid->getBSphere();
 243             sgVec3 fcenter;
 244             sgCopyVec3( fcenter, bsphere->getCenter() );
 245             sgdVec3 center;
 246             center[0] = fcenter[0];
 247             center[1] = fcenter[1];
 248             center[2] = fcenter[2];
 249             sgdXformPnt3( center, m ) ;
 250             double radius_sqd = bsphere->getRadius() * bsphere->getRadius();
 251             if ( sgdPointLineDistSquared( center, orig, dir ) < radius_sqd ) {
 252                 // possible intersections
 253                 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
 254                     sgdMat4 m_new;
 255                     sgdCopyMat4(m_new, m);
 256                     if ( kid->isA( ssgTypeTransform() ) ) {
 257                         sgMat4 fxform;
 258                         ((ssgTransform *)kid)->getTransform( fxform );
 259                         sgdMat4 xform;
 260                         sgdSetMat4( xform, fxform );
 261                         sgdPreMultMat4( m_new, xform );
 262                     }
 263                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
 264                 } else if ( kid->isAKindOf ( ssgTypeLeaf() ) ) {
 265                     IntersectLeaf( (ssgLeaf *)kid, m, orig, dir );
 266                 }
 267             } else {
 268                 // end of the line for this branch
 269             }
 270         } else {
 271             // branch requested not to be traversed
 272         }
 273     }
 274 }
 275
 276
 277 // This expects the inital m to the identity transform
 278 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
 279 {
 280     for ( ssgEntity *parent = branch->getParent(0);
 281           parent != NULL;
 282           parent = parent->getNextParent() )
 283     {
 284         // recurse till we are at the scene root
 285         // then just unwinding the stack should
 286         // give us our cumulative transform :-)  NHV
 287         ssgGetEntityTransform( parent, m );
 288         if ( parent->isA( ssgTypeTransform() ) ) {
 289             sgMat4 xform;
 290             ((ssgTransform *)parent)->getTransform( xform );
 291             sgPreMultMat4( m, xform );
 292         }
 293     }
 294 }
 295
 296
 297 // return the passed entitity's bsphere's center point radius and
 298 // fully formed current model matrix for entity
 299 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
 300                                                    float *radius, sgMat4 m )
 301 {
 302     sgSphere *bsphere = entity->getBSphere();
 303     *radius = (double)bsphere->getRadius();
 304     sgCopyVec3( center, bsphere->getCenter() );
 305     sgMakeIdentMat4 ( m ) ;
 306     ssgGetEntityTransform( entity, m );
 307 }
 308
 309
 310 void FGHitList::IntersectCachedLeaf( sgdMat4 m,
 311                                      sgdVec3 orig, sgdVec3 dir)
 312 {
 313     if ( last_hit() ) {
 314         float radius;
 315         sgVec3 fcenter;
 316         sgMat4 fxform;
 317         // ssgEntity *ent = last_hit();
 318         ssgGetCurrentBSphere( last_hit(), fcenter, &radius, fxform );
 319         sgdMat4 m;
 320         sgdVec3 center;
 321         sgdSetMat4( m, fxform );
 322         sgdXformPnt3( center, m );
 323
 324         if ( sgdPointLineDistSquared( center, orig, dir ) < radius*radius ) {
 325             IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
 326         }
 327     }
 328 }