]> git.mxchange.org Git - flightgear.git/blob - src/Scenery/hitlist.cxx
Load ground ATC frequency data, and map all stations by bucket index
[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 #include <Scenery/scenery.hxx>
21
22 #include "hitlist.hxx"
23
24 // forward declaration of our helper/convenience functions
25 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2);
26 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m );
27 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m );
28
29
30 // ======================
31 // This is same as PLib's sgdIsectInfLinePlane()
32 // and can be replaced by it after the next PLib release
33 static int fgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
34                                  const sgdVec3 l_vec, const sgdVec4 plane )
35 {
36     SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
37
38     /* Is line parallel to plane? */
39
40     if ( fabs ( tmp ) < FLT_EPSILON )
41         return false ;
42
43     sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
44                                   + plane[3] ) / tmp ) ;
45     sgdAddVec3  ( dst, l_org ) ;
46
47     return true ;
48 }
49
50 // ======================
51
52 /*
53  * Given a point and a triangle lying on the same plane
54  * check to see if the point is inside the triangle
55  */
56 // This is same as PLib's sgdPointInTriangle()
57 // and can be replaced by it after the next PLib release
58 static bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
59 {
60     sgdVec3 dif;
61
62     SGDfloat min, max;
63     // punt if outside bouding cube
64     SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
65     if( (point[0] < min) || (point[0] > max) )
66         return false;
67     dif[0] = max - min;
68
69     SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
70     if( (point[1] < min) || (point[1] > max) )
71         return false;
72     dif[1] = max - min;
73
74     SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
75     if( (point[2] < min) || (point[2] > max) )
76         return false;
77     dif[2] = max - min;
78
79     // drop the smallest dimension so we only have to work in 2d.
80     SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
81     SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
82     if ( fabs(min_dim-dif[0]) <= DBL_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-dif[1]) <= DBL_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-dif[2]) <= DBL_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     SGDfloat tmp = (y2 - y3) / (x2 - x3);
120     int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
121     int side2 = SG_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 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
130     side2 = SG_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 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
139     side2 = SG_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 // ======================
149
150 inline static int isZeroAreaTri( sgdVec3 tri[3] )
151 {
152     return( sgdEqualVec3(tri[0], tri[1]) ||
153             sgdEqualVec3(tri[1], tri[2]) ||
154             sgdEqualVec3(tri[2], tri[0]) );
155 }
156
157 FGHitList::FGHitList() :
158         last(NULL), test_dist(DBL_MAX)
159 {
160 }
161
162 FGHitList::~FGHitList() {}
163
164
165 /*
166 Find the intersection of an infinite line with a leaf
167 the line being defined by a point and direction.
168
169 Variables
170 In:
171 ssgLeaf pointer  -- leaf
172 qualified matrix -- m
173 line origin      -- orig
174 line direction   -- dir
175 Out:
176 result  -- intersection point
177 normal  -- intersected tri's normal
178
179 Returns:
180 true if intersection found
181 false otherwise
182
183 !!! WARNING !!!
184 If you need an exhaustive list of hitpoints YOU MUST use
185 the generic version of this function as the specialized
186 versions will do an early out of expensive tests if the point
187 can not be the closest one found
188 !!! WARNING !!!
189 */
190 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
191                               sgdVec3 orig, sgdVec3 dir )
192 {
193     int num_hits = 0;
194     int i = 0;
195
196     for ( ; i < leaf->getNumTriangles(); ++i ) {
197         short i1, i2, i3;
198         leaf->getTriangle( i, &i1, &i2, &i3 );
199
200         sgdVec3 tri[3];
201         sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
202         sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
203         sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
204
205         if( isZeroAreaTri( tri ) )
206             continue;
207
208         sgdVec4 plane;
209         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
210
211         sgdVec3 point;
212         if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
213             if( fgdPointInTriangle( point, tri ) ) {
214                 // transform point into passed into desired coordinate frame
215                 sgdXformPnt3( point, point, m );
216                 sgdXformPnt4(plane,plane,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;
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             sgdXformPnt4(plane,plane,m);
321             add(leaf,n,point,plane);
322             test_dist = tmp_dist;
323             num_hits++;
324         }
325     }
326     return num_hits;
327 }
328
329 // ======================
330 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
331     return ( sgdScalarProductVec3(v, v) < (radius*radius) );
332 }
333
334 // ======================
335 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
336                                  sgdVec3 orig, sgdVec3 dir )
337 {
338     /* the lookat vector and matrix in branch's coordinate frame
339      * but we won't determine these unless needed,
340      * This 'lazy evaluation' is a result of profiling data */
341     sgdVec3 orig_leaf, dir_leaf;
342     sgdMat4 m_leaf;
343
344     // 'lazy evaluation' flag
345     int first_time = 1;
346
347     for ( ssgEntity *kid = branch->getKid( 0 );
348             kid != NULL;
349             kid = branch->getNextKid() )
350     {
351         if ( kid->getTraversalMask() & SSGTRAV_HOT )
352         {
353             sgdVec3 center;
354             sgdSetVec3( center,
355                         kid->getBSphere()->getCenter()[0],
356                         kid->getBSphere()->getCenter()[1],
357                         kid->getBSphere()->getCenter()[2] );
358             sgdXformPnt3( center, m ) ;
359
360             // sgdClosestPointToLineDistSquared( center, orig, dir )
361             // inlined here because because of profiling results
362             sgdVec3 u, u1, v;
363             sgdSubVec3(u, center, orig);
364             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
365                           / sgdScalarProductVec3(dir,dir) );
366             sgdSubVec3(v, u, u1);
367
368             // double because of possible overflow
369             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
370             {
371                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
372                 {
373                     sgdMat4 m_new;
374                     if ( kid->isA( ssgTypeTransform() ) )
375                     {
376                         sgMat4 fxform;
377                         ((ssgTransform *)kid)->getTransform( fxform );
378                         sgMultMat4(m_new, m, fxform);
379                     } else {
380                         sgdCopyMat4(m_new, m);
381                     }
382                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
383                 }
384                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
385                 {
386                     if ( first_time ) {
387                         sgdTransposeNegateMat4( m_leaf, m );
388                         sgdXformPnt3( orig_leaf, orig, m_leaf );
389                         sgdXformPnt3( dir_leaf,  dir,  m_leaf );
390                         first_time = 0;
391                     }
392                     GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
393                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
394                 }
395             }  // Out of range
396         } // branch not requested to be traversed
397     } // end for loop
398 }
399
400
401
402 // ======================
403 // a temporary hack until we get everything rewritten with sgdVec3
404 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
405 {
406     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
407 }
408
409
410 // ======================
411 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
412     sgdMat4 m;
413     clear();
414     sgdMakeIdentMat4 ( m ) ;
415     IntersectBranch( scene, m, orig, dir );
416 }
417
418 // ======================
419 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
420 {
421     clear();
422     IntersectBranch( scene, m, orig, dir );
423 }
424
425 // ======================
426 // Need these because of mixed matrix types
427 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
428 {
429     for ( int j = 0 ; j < 4 ; j++ )
430     {
431         dst[0][j] = m2[0][0] * m1[0][j] +
432                     m2[0][1] * m1[1][j] +
433                     m2[0][2] * m1[2][j] +
434                     m2[0][3] * m1[3][j] ;
435
436         dst[1][j] = m2[1][0] * m1[0][j] +
437                     m2[1][1] * m1[1][j] +
438                     m2[1][2] * m1[2][j] +
439                     m2[1][3] * m1[3][j] ;
440
441         dst[2][j] = m2[2][0] * m1[0][j] +
442                     m2[2][1] * m1[1][j] +
443                     m2[2][2] * m1[2][j] +
444                     m2[2][3] * m1[3][j] ;
445
446         dst[3][j] = m2[3][0] * m1[0][j] +
447                     m2[3][1] * m1[1][j] +
448                     m2[3][2] * m1[2][j] +
449                     m2[3][3] * m1[3][j] ;
450     }
451 }
452
453 // ======================
454 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
455 {
456     /*
457        Walk backwards up the tree, transforming the
458        vertex by all the matrices along the way.
459
460        Upwards recursion hurts my head.
461        */
462
463     sgMat4 mat ;
464
465     /*
466        If this node has a parent - get the composite
467        matrix for the parent.
468        */
469     if ( entity->getNumParents() > 0 )
470         ssgGetEntityTransform ( entity->getParent(0), mat ) ;
471     else
472         sgMakeIdentMat4 ( mat ) ;
473
474     /*
475        If this node has a transform - then concatenate it.
476        */
477     if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
478         sgMat4 this_mat ;
479         ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
480         sgPostMultMat4 ( mat, this_mat ) ;
481     }
482
483     sgCopyMat4 ( m, mat ) ;
484 }
485
486 // ======================
487 // return the passed entitity's bsphere's center point radius and
488 // fully formed current model matrix for entity
489 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
490 {
491     sgSphere *bsphere = entity->getBSphere();
492     *radius = (double)bsphere->getRadius();
493     sgCopyVec3( center, bsphere->getCenter() );
494     sgMakeIdentMat4 ( m ) ;
495     ssgGetEntityTransform( entity, m );
496 }
497
498
499 // ======================
500 // Determine scenery altitude via ssg.
501 // returned results are in meters
502 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
503                     FGHitList *hit_list,
504                     double *terrain_elev, double *radius, double *normal)
505 {
506     sgdVec3 view_pos;
507     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
508
509     sgdVec3 orig, dir;
510     sgdCopyVec3(orig, view_pos );
511     sgdCopyVec3(dir, abs_view_pos );
512
513     hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
514                          orig, dir );
515
516     int this_hit=0;
517     Point3D geoc;
518     double result = -9999;
519     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
520
521     // cout << "hits = ";
522     int hitcount = hit_list->num_hits();
523     for ( int i = 0; i < hitcount; ++i ) {
524         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
525         double lat_geod, alt, sea_level_r;
526         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
527                      &alt, &sea_level_r);
528         // cout << alt << " ";
529         if ( alt > result && alt < 10000 ) {
530             result = alt;
531             this_hit = i;
532         }
533     }
534     // cout << endl;
535
536     if ( result > -9000 ) {
537         *terrain_elev = result;
538         *radius = geoc.radius();
539         sgVec3 tmp;
540         sgSetVec3(tmp, hit_list->get_normal(this_hit));
541         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
542         sgdSetVec3( normal, tmp );
543         // float *up = globals->get_current_view()->get_world_up();
544         // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
545         /* ssgState *IntersectedLeafState =
546               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
547         return true;
548     } else {
549         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
550         *terrain_elev = 0.0;
551         float *up = globals->get_current_view()->get_world_up();
552         sgdSetVec3(normal, up[0], up[1], up[2]);
553         return false;
554     }
555 }
556
557
558 // ======================
559 // Determine scenery altitude via ssg.
560 // returned results are in meters
561 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
562                     ssgTransform *terra_transform,
563                     FGHitList *hit_list,
564                     double *terrain_elev, double *radius, double *normal)
565 {
566     sgdVec3 view_pos;
567     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
568
569     sgdVec3 orig, dir;
570     sgdCopyVec3(orig, view_pos );
571
572     sgdCopyVec3(dir, abs_view_pos );
573     sgdNormalizeVec3(dir);
574
575     sgMat4 fxform;
576     sgMakeIdentMat4 ( fxform ) ;
577     ssgGetEntityTransform( terra_transform, fxform );
578
579     sgdMat4 xform;
580     sgdSetMat4(xform,fxform);
581     hit_list->Intersect( terra_transform, xform, orig, dir );
582
583     int this_hit=0;
584     Point3D geoc;
585     double result = -9999;
586     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
587
588     int hitcount = hit_list->num_hits();
589     for ( int i = 0; i < hitcount; ++i ) {
590         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
591         double lat_geod, alt, sea_level_r;
592         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
593                      &alt, &sea_level_r);
594         if ( alt > result && alt < 20000 ) {
595             result = alt;
596             this_hit = i;
597         }
598     }
599
600     if ( result > -9000 ) {
601         *terrain_elev = result;
602         *radius = geoc.radius();
603         sgVec3 tmp;
604         sgSetVec3(tmp, hit_list->get_normal(this_hit));
605         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
606         sgdSetVec3( normal, tmp );
607         // float *up = globals->get_current_view()->get_world_up();
608        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
609         /* ssgState *IntersectedLeafState =
610               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
611         return true;
612     } else {
613         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
614         return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
615                               terrain_elev,radius,normal);
616     }
617 }
618