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