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