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