]> git.mxchange.org Git - flightgear.git/blob - src/Scenery/hitlist.cxx
Here is a cleaned up hitlist that should solve the PLib conflict
[flightgear.git] / src / Scenery / hitlist.cxx
1 // hitlist.cxx -
2 // Height Over Terrain and Assosciated Routines for FlightGear based Scenery
3 // Written by Norman Vine, started 2000.
4
5 #ifdef HAVE_CONFIG_H
6 #  include <config.h>
7 #endif
8
9 #include <float.h>
10 #include <math.h>
11
12 #include <simgear/sg_inlines.h>
13 #include <simgear/debug/logstream.hxx>
14 #include <simgear/math/point3d.hxx>
15 #include <simgear/math/sg_geodesy.hxx>
16 #include <simgear/math/vector.hxx>
17
18 #include <Main/globals.hxx>
19 #include <Main/viewer.hxx>
20
21 #include "hitlist.hxx"
22
23 extern ssgBranch *terrain_branch;
24
25 // forward declaration of our helper/convenience functions
26 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2);
27 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m );
28 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m );
29
30
31 // ======================
32 // This is same as PLib's sgdIsectInfLinePlane()
33 // and can be replaced by it after the next PLib release
34 static int fgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
35                                  const sgdVec3 l_vec, const sgdVec4 plane )
36 {
37     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
38
39     /* Is line parallel to plane? */
40
41     if ( fabs ( tmp ) < FLT_EPSILON )
42         return false ;
43
44     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
45                                   + plane[3] ) / tmp ) ;
46     sgdAddVec3  ( dst, l_org ) ;
47
48     return true ;
49 }
50
51 // ======================
52
53 /*
54  * Given a point and a triangle lying on the same plane
55  * check to see if the point is inside the triangle
56  */
57 // This is same as PLib's sgdPointInTriangle()
58 // and can be replaced by it after the next PLib release
59 static bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
60 {
61     sgdVec3 dif;
62
63     SGDfloat min, max;
64     // punt if outside bouding cube
65     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
66     if( (point[0] < min) || (point[0] > max) )
67         return false;
68     dif[0] = max - min;
69
70     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
71     if( (point[1] < min) || (point[1] > max) )
72         return false;
73     dif[1] = max - min;
74
75     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
76     if( (point[2] < min) || (point[2] > max) )
77         return false;
78     dif[2] = max - min;
79
80     // drop the smallest dimension so we only have to work in 2d.
81     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
82     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
83     if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
84         // x is the smallest dimension
85         x1 = point[1];
86         y1 = point[2];
87         x2 = tri[0][1];
88         y2 = tri[0][2];
89         x3 = tri[1][1];
90         y3 = tri[1][2];
91         rx = tri[2][1];
92         ry = tri[2][2];
93     } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
94         // y is the smallest dimension
95         x1 = point[0];
96         y1 = point[2];
97         x2 = tri[0][0];
98         y2 = tri[0][2];
99         x3 = tri[1][0];
100         y3 = tri[1][2];
101         rx = tri[2][0];
102         ry = tri[2][2];
103     } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
104         // z is the smallest dimension
105         x1 = point[0];
106         y1 = point[1];
107         x2 = tri[0][0];
108         y2 = tri[0][1];
109         x3 = tri[1][0];
110         y3 = tri[1][1];
111         rx = tri[2][0];
112         ry = tri[2][1];
113     } else {
114         // all dimensions are really small so lets call it close
115         // enough and return a successful match
116         return true;
117     }
118
119     // check if intersection point is on the same side of p1 <-> p2 as p3
120     SGDfloat tmp = (y2 - y3) / (x2 - x3);
121     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
122     int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
123     if ( side1 != side2 ) {
124         // printf("failed side 1 check\n");
125         return false;
126     }
127
128     // check if intersection point is on correct side of p2 <-> p3 as p1
129     tmp = (y3 - ry) / (x3 - rx);
130     side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
131     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
132     if ( side1 != side2 ) {
133         // printf("failed side 2 check\n");
134         return false;
135     }
136
137     // check if intersection point is on correct side of p1 <-> p3 as p2
138     tmp = (y2 - ry) / (x2 - rx);
139     side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
140     side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
141     if ( side1 != side2 ) {
142         // printf("failed side 3  check\n");
143         return false;
144     }
145
146     return true;
147 }
148
149 // ======================
150
151 inline static int isZeroAreaTri( sgdVec3 tri[3] )
152 {
153     return( sgdEqualVec3(tri[0], tri[1]) ||
154             sgdEqualVec3(tri[1], tri[2]) ||
155             sgdEqualVec3(tri[2], tri[0]) );
156 }
157
158 FGHitList::FGHitList() :
159         last(NULL), test_dist(DBL_MAX)
160 {
161 }
162
163 FGHitList::~FGHitList() {}
164
165
166 /*
167 Find the intersection of an infinite line with a leaf
168 the line being defined by a point and direction.
169
170 Variables
171 In:
172 ssgLeaf pointer  -- leaf
173 qualified matrix -- m
174 line origin      -- orig
175 line direction   -- dir
176 Out:
177 result  -- intersection point
178 normal  -- intersected tri's normal
179
180 Returns:
181 true if intersection found
182 false otherwise
183
184 !!! WARNING !!!
185 If you need an exhaustive list of hitpoints YOU MUST use
186 the generic version of this function as the specialized
187 versions will do an early out of expensive tests if the point
188 can not be the closest one found
189 !!! WARNING !!!
190 */
191 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
192                               sgdVec3 orig, sgdVec3 dir )
193 {
194     int num_hits = 0;
195     int i = 0;
196
197     for ( ; i < leaf->getNumTriangles(); ++i ) {
198         short i1, i2, i3;
199         leaf->getTriangle( i, &i1, &i2, &i3 );
200
201         sgdVec3 tri[3];
202         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
203         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
204         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
205
206         if( isZeroAreaTri( tri ) )
207             continue;
208
209         sgdVec4 plane;
210         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
211
212         sgdVec3 point;
213         if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
214             if( fgdPointInTriangle( point, tri ) ) {
215                 // transform point into passed into desired coordinate frame
216                 sgdXformPnt3( point, point, m );
217                 add(leaf,i,point,plane);
218                 num_hits++;
219             }
220         }
221     }
222     return num_hits;
223 }
224
225 // ======================
226
227 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
228                               sgdVec3 orig, sgdVec3 dir,
229                               GLenum primType )
230 {
231     double tmp_dist;
232
233     // number of hits but there could be more that
234     // were not found because of short circut switch !
235     // so you may want to use the unspecialized IntersectLeaf()
236     int n, num_hits = 0;
237
238     int ntri = leaf->getNumTriangles();
239     for ( n = 0; n < ntri; ++n )
240     {
241         sgdVec3 tri[3];
242
243         switch ( primType )
244         {
245         case GL_POLYGON :
246         case GL_TRIANGLE_FAN :
247             if ( !n ) {
248                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
249                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
250                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
251             } else {
252                 sgdCopyVec3( tri[1], tri[2] );
253                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
254             }
255             break;
256         case GL_TRIANGLES :
257             sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
258             sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
259             sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
260             break;
261         case GL_TRIANGLE_STRIP :
262         case GL_QUAD_STRIP :
263             if ( !n ) {
264                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
265                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
266                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
267             } else {
268                 if ( n & 1 ) {
269                     sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
270                     sgdCopyVec3( tri[1], tri[2] );
271                     sgdCopyVec3( tri[2], tri[1] );
272
273                 } else {
274                     sgdCopyVec3( tri[0], tri[1] );
275                     sgdCopyVec3( tri[1], tri[2] );
276                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
277                 }
278             }
279             break;
280         case GL_QUADS :
281             sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
282             sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
283             sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
284             break;
285         default:
286             return IntersectLeaf( leaf, m, orig, dir);
287         }
288
289         if( isZeroAreaTri( tri ) )
290             continue;
291
292         sgdVec4 plane;
293         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
294
295         sgdVec3 point, test;
296
297         // find point of intersection of line from point org
298         // in direction dir with triangle's plane
299         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
300         /* Is line parallel to plane? */
301         if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
302             continue ;
303
304         // find parametric point
305         sgdScaleVec3 ( point, dir,
306                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
307                        / tmp ) ;
308
309         // short circut if this point is further away then a previous hit
310         tmp_dist = sgdDistanceSquaredVec3(point, orig );
311         if( tmp_dist > test_dist )
312             continue;
313
314         // place parametric point in world
315         sgdAddVec3 ( point, orig ) ;
316
317         if( fgdPointInTriangle( point, tri ) ) {
318             // transform point into passed coordinate frame
319             sgdXformPnt3( point, point, m );
320             add(leaf,n,point,plane);
321             test_dist = tmp_dist;
322             num_hits++;
323         }
324     }
325     return num_hits;
326 }
327
328 // ======================
329 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
330     return ( sgdScalarProductVec3(v, v) < (radius*radius) );
331 }
332
333 // ======================
334 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
335                                  sgdVec3 orig, sgdVec3 dir )
336 {
337     /* the lookat vector and matrix in branch's coordinate frame
338      * but we won't determine these unless needed,
339      * This 'lazy evaluation' is a result of profiling data */
340     sgdVec3 orig_leaf, dir_leaf;
341     sgdMat4 m_leaf;
342
343     // 'lazy evaluation' flag
344     int first_time = 1;
345
346     for ( ssgEntity *kid = branch->getKid( 0 );
347             kid != NULL;
348             kid = branch->getNextKid() )
349     {
350         if ( kid->getTraversalMask() & SSGTRAV_HOT )
351         {
352             sgdVec3 center;
353             sgdSetVec3( center,
354                         kid->getBSphere()->getCenter()[0],
355                         kid->getBSphere()->getCenter()[1],
356                         kid->getBSphere()->getCenter()[2] );
357             sgdXformPnt3( center, m ) ;
358
359             // sgdClosestPointToLineDistSquared( center, orig, dir )
360             // inlined here because because of profiling results
361             sgdVec3 u, u1, v;
362             sgdSubVec3(u, center, orig);
363             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
364                           / sgdScalarProductVec3(dir,dir) );
365             sgdSubVec3(v, u, u1);
366
367             // double because of possible overflow
368             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
369             {
370                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
371                 {
372                     sgdMat4 m_new;
373                     if ( kid->isA( ssgTypeTransform() ) )
374                     {
375                         sgMat4 fxform;
376                         ((ssgTransform *)kid)->getTransform( fxform );
377                         sgMultMat4(m_new, m, fxform);
378                     } else {
379                         sgdCopyMat4(m_new, m);
380                     }
381                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
382                 }
383                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
384                 {
385                     if ( first_time ) {
386                         sgdTransposeNegateMat4( m_leaf, m );
387                         sgdXformPnt3( orig_leaf, orig, m_leaf );
388                         sgdXformPnt3( dir_leaf,  dir,  m_leaf );
389                         first_time = 0;
390                     }
391                     GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
392                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
393                 }
394             }  // Out of range
395         } // branch not requested to be traversed
396     } // end for loop
397 }
398
399
400
401 // ======================
402 // a temporary hack until we get everything rewritten with sgdVec3
403 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
404 {
405     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
406 }
407
408
409 // ======================
410 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
411     sgdMat4 m;
412     clear();
413     sgdMakeIdentMat4 ( m ) ;
414     IntersectBranch( scene, m, orig, dir );
415 }
416
417 // ======================
418 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
419 {
420     clear();
421     IntersectBranch( scene, m, orig, dir );
422 }
423
424 // ======================
425 // Need these because of mixed matrix types
426 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
427 {
428     for ( int j = 0 ; j < 4 ; j++ )
429     {
430         dst[0][j] = m2[0][0] * m1[0][j] +
431                     m2[0][1] * m1[1][j] +
432                     m2[0][2] * m1[2][j] +
433                     m2[0][3] * m1[3][j] ;
434
435         dst[1][j] = m2[1][0] * m1[0][j] +
436                     m2[1][1] * m1[1][j] +
437                     m2[1][2] * m1[2][j] +
438                     m2[1][3] * m1[3][j] ;
439
440         dst[2][j] = m2[2][0] * m1[0][j] +
441                     m2[2][1] * m1[1][j] +
442                     m2[2][2] * m1[2][j] +
443                     m2[2][3] * m1[3][j] ;
444
445         dst[3][j] = m2[3][0] * m1[0][j] +
446                     m2[3][1] * m1[1][j] +
447                     m2[3][2] * m1[2][j] +
448                     m2[3][3] * m1[3][j] ;
449     }
450 }
451
452 // ======================
453 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
454 {
455     /*
456        Walk backwards up the tree, transforming the
457        vertex by all the matrices along the way.
458
459        Upwards recursion hurts my head.
460        */
461
462     sgMat4 mat ;
463
464     /*
465        If this node has a parent - get the composite
466        matrix for the parent.
467        */
468     if ( entity->getNumParents() > 0 )
469         ssgGetEntityTransform ( entity->getParent(0), mat ) ;
470     else
471         sgMakeIdentMat4 ( mat ) ;
472
473     /*
474        If this node has a transform - then concatenate it.
475        */
476     if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
477         sgMat4 this_mat ;
478         ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
479         sgPostMultMat4 ( mat, this_mat ) ;
480     }
481
482     sgCopyMat4 ( m, mat ) ;
483 }
484
485 // ======================
486 // return the passed entitity's bsphere's center point radius and
487 // fully formed current model matrix for entity
488 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
489 {
490     sgSphere *bsphere = entity->getBSphere();
491     *radius = (double)bsphere->getRadius();
492     sgCopyVec3( center, bsphere->getCenter() );
493     sgMakeIdentMat4 ( m ) ;
494     ssgGetEntityTransform( entity, m );
495 }
496
497
498 // ======================
499 // Determine scenery altitude via ssg.
500 // returned results are in meters
501 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
502                                         FGHitList *hit_list,
503                                         double *terrain_elev, double *radius, double *normal)
504 {
505         sgdVec3 view_pos;
506         sgdSubVec3( view_pos, abs_view_pos, scenery_center );
507
508         sgdVec3 orig, dir;
509         sgdCopyVec3(orig, view_pos );
510         sgdCopyVec3(dir, abs_view_pos );
511
512         // !! why is terrain not globals->get_terrain()
513         hit_list->Intersect( terrain_branch, orig, dir );
514
515         int this_hit=0;
516         Point3D geoc;
517         double result = -9999;
518         Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
519
520         // cout << "hits = ";
521         int hitcount = hit_list->num_hits();
522         for ( int i = 0; i < hitcount; ++i ) {
523                 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
524                 double lat_geod, alt, sea_level_r;
525                 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
526                                          &alt, &sea_level_r);
527                 // cout << alt << " ";
528                 if ( alt > result && alt < 10000 ) {
529                         result = alt;
530                         this_hit = i;
531                 }
532         }
533         // cout << endl;
534
535         if ( result > -9000 ) {
536                 *terrain_elev = result;
537                 *radius = geoc.radius();
538                 sgVec3 tmp;
539                 sgMat4 TMP;
540                 sgSetVec3(tmp, hit_list->get_normal(this_hit));
541                 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
542                 //      << tmp[2] << endl;
543                 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
544                 sgXformVec3(tmp, tmp, TMP);
545                 // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
546                 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
547                 /* ssgState *IntersectedLeafState =
548         ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
549                 return true;
550         } else {
551                 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
552                 *terrain_elev = 0.0;
553                 float *up = globals->get_current_view()->get_world_up();
554                 sgdSetVec3(normal, up[0], up[1], up[2]);
555                 return false;
556         }
557 }
558
559
560 // ======================
561 // Determine scenery altitude via ssg.
562 // returned results are in meters
563 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
564                                         ssgTransform *terra_transform,
565                                         FGHitList *hit_list,
566                                         double *terrain_elev, double *radius, double *normal)
567 {
568         sgdVec3 view_pos;
569         sgdSubVec3( view_pos, abs_view_pos, scenery_center );
570
571         sgdVec3 orig, dir;
572         sgdCopyVec3(orig, view_pos );
573
574         sgdCopyVec3(dir, abs_view_pos );
575         sgdNormalizeVec3(dir);
576
577         sgMat4 fxform;
578         sgMakeIdentMat4 ( fxform ) ;
579         ssgGetEntityTransform( terra_transform, fxform );
580
581         sgdMat4 xform;
582         sgdSetMat4(xform,fxform);
583         hit_list->Intersect( terra_transform, xform, orig, dir );
584
585         int this_hit=0;
586         Point3D geoc;
587         double result = -9999;
588         Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
589
590         int hitcount = hit_list->num_hits();
591         for ( int i = 0; i < hitcount; ++i ) {
592                 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
593                 double lat_geod, alt, sea_level_r;
594                 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
595                                          &alt, &sea_level_r);
596                 if ( alt > result && alt < 20000 ) {
597                         result = alt;
598                         this_hit = i;
599                 }
600         }
601
602         if ( result > -9000 ) {
603                 *terrain_elev = result;
604                 *radius = geoc.radius();
605                 sgVec3 tmp;
606                 sgMat4 TMP;
607                 sgSetVec3(tmp, hit_list->get_normal(this_hit));
608                 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
609                 sgXformVec3(tmp, tmp, TMP);
610                 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
611                 /* ssgState *IntersectedLeafState =
612         ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
613                 return true;
614         } else {
615                 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
616                 return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
617                                                           terrain_elev,radius,normal);
618         }
619 }
620