]> git.mxchange.org Git - flightgear.git/blob - src/Scenery/hitlist.cxx
Mac OS X fixes and MSVC warning fixes from Jonathan Polley.
[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                 sgdXformPnt4(plane,plane,m);
216                 add(leaf,i,point,plane);
217                 num_hits++;
218             }
219         }
220     }
221     return num_hits;
222 }
223
224 // ======================
225
226 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
227                               sgdVec3 orig, sgdVec3 dir,
228                               GLenum primType )
229 {
230     double tmp_dist;
231
232     // number of hits but there could be more that
233     // were not found because of short circut switch !
234     // so you may want to use the unspecialized IntersectLeaf()
235     int n, num_hits = 0;
236
237     int ntri = leaf->getNumTriangles();
238     for ( n = 0; n < ntri; ++n )
239     {
240         sgdVec3 tri[3];
241
242         switch ( primType )
243         {
244         case GL_POLYGON :
245         case GL_TRIANGLE_FAN :
246             if ( !n ) {
247                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
248                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
249                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
250             } else {
251                 sgdCopyVec3( tri[1], tri[2] );
252                 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
253             }
254             break;
255         case GL_TRIANGLES :
256             sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
257             sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
258             sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
259             break;
260         case GL_TRIANGLE_STRIP :
261         case GL_QUAD_STRIP :
262             if ( !n ) {
263                 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
264                 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
265                 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
266             } else {
267                 if ( n & 1 ) {
268                     sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
269                     sgdCopyVec3( tri[1], tri[2] );
270                     sgdCopyVec3( tri[2], tri[1] );
271
272                 } else {
273                     sgdCopyVec3( tri[0], tri[1] );
274                     sgdCopyVec3( tri[1], tri[2] );
275                     sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
276                 }
277             }
278             break;
279         case GL_QUADS :
280             sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
281             sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
282             sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
283             break;
284         default:
285             return IntersectLeaf( leaf, m, orig, dir);
286         }
287
288         if( isZeroAreaTri( tri ) )
289             continue;
290
291         sgdVec4 plane;
292         sgdMakePlane( plane, tri[0], tri[1], tri[2] );
293
294         sgdVec3 point;
295
296         // find point of intersection of line from point org
297         // in direction dir with triangle's plane
298         SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
299         /* Is line parallel to plane? */
300         if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
301             continue ;
302
303         // find parametric point
304         sgdScaleVec3 ( point, dir,
305                        -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
306                        / tmp ) ;
307
308         // short circut if this point is further away then a previous hit
309         tmp_dist = sgdDistanceSquaredVec3(point, orig );
310         if( tmp_dist > test_dist )
311             continue;
312
313         // place parametric point in world
314         sgdAddVec3 ( point, orig ) ;
315
316         if( fgdPointInTriangle( point, tri ) ) {
317             // transform point into passed coordinate frame
318             sgdXformPnt3( point, point, m );
319             sgdXformPnt4(plane,plane,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     hit_list->Intersect( globals->get_terrain_branch(), orig, dir );
513
514     int this_hit=0;
515     Point3D geoc;
516     double result = -9999;
517     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
518
519     // cout << "hits = ";
520     int hitcount = hit_list->num_hits();
521     for ( int i = 0; i < hitcount; ++i ) {
522         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
523         double lat_geod, alt, sea_level_r;
524         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
525                      &alt, &sea_level_r);
526         // cout << alt << " ";
527         if ( alt > result && alt < 10000 ) {
528             result = alt;
529             this_hit = i;
530         }
531     }
532     // cout << endl;
533
534     if ( result > -9000 ) {
535         *terrain_elev = result;
536         *radius = geoc.radius();
537         sgVec3 tmp;
538         sgSetVec3(tmp, hit_list->get_normal(this_hit));
539         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
540         sgdSetVec3( normal, tmp );
541         // float *up = globals->get_current_view()->get_world_up();
542         // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
543         /* ssgState *IntersectedLeafState =
544               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
545         return true;
546     } else {
547         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
548         *terrain_elev = 0.0;
549         float *up = globals->get_current_view()->get_world_up();
550         sgdSetVec3(normal, up[0], up[1], up[2]);
551         return false;
552     }
553 }
554
555
556 // ======================
557 // Determine scenery altitude via ssg.
558 // returned results are in meters
559 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
560                     ssgTransform *terra_transform,
561                     FGHitList *hit_list,
562                     double *terrain_elev, double *radius, double *normal)
563 {
564     sgdVec3 view_pos;
565     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
566
567     sgdVec3 orig, dir;
568     sgdCopyVec3(orig, view_pos );
569
570     sgdCopyVec3(dir, abs_view_pos );
571     sgdNormalizeVec3(dir);
572
573     sgMat4 fxform;
574     sgMakeIdentMat4 ( fxform ) ;
575     ssgGetEntityTransform( terra_transform, fxform );
576
577     sgdMat4 xform;
578     sgdSetMat4(xform,fxform);
579     hit_list->Intersect( terra_transform, xform, orig, dir );
580
581     int this_hit=0;
582     Point3D geoc;
583     double result = -9999;
584     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
585
586     int hitcount = hit_list->num_hits();
587     for ( int i = 0; i < hitcount; ++i ) {
588         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
589         double lat_geod, alt, sea_level_r;
590         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
591                      &alt, &sea_level_r);
592         if ( alt > result && alt < 20000 ) {
593             result = alt;
594             this_hit = i;
595         }
596     }
597
598     if ( result > -9000 ) {
599         *terrain_elev = result;
600         *radius = geoc.radius();
601         sgVec3 tmp;
602         sgSetVec3(tmp, hit_list->get_normal(this_hit));
603         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
604         sgdSetVec3( normal, tmp );
605         // float *up = globals->get_current_view()->get_world_up();
606        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
607         /* ssgState *IntersectedLeafState =
608               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
609         return true;
610     } else {
611         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
612         return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
613                               terrain_elev,radius,normal);
614     }
615 }
616