]> git.mxchange.org Git - flightgear.git/blob - src/Scenery/hitlist.cxx
Don't scale elevator by 0.5.
[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              && !kid->getBSphere()->isEmpty() )
353         {
354             sgdVec3 center;
355             sgdSetVec3( center,
356                         kid->getBSphere()->getCenter()[0],
357                         kid->getBSphere()->getCenter()[1],
358                         kid->getBSphere()->getCenter()[2] );
359             sgdXformPnt3( center, m ) ;
360
361             // sgdClosestPointToLineDistSquared( center, orig, dir )
362             // inlined here because because of profiling results
363             sgdVec3 u, u1, v;
364             sgdSubVec3(u, center, orig);
365             sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
366                           / sgdScalarProductVec3(dir,dir) );
367             sgdSubVec3(v, u, u1);
368
369             // double because of possible overflow
370             if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
371             {
372                 if ( kid->isAKindOf ( ssgTypeBranch() ) )
373                 {
374                     sgdMat4 m_new;
375                     if ( kid->isA( ssgTypeTransform() ) )
376                     {
377                         sgMat4 fxform;
378                         ((ssgTransform *)kid)->getTransform( fxform );
379                         sgMultMat4(m_new, m, fxform);
380                     } else {
381                         sgdCopyMat4(m_new, m);
382                     }
383                     IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
384                 }
385                 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
386                 {
387                     if ( first_time ) {
388                         sgdTransposeNegateMat4( m_leaf, m );
389                         sgdXformPnt3( orig_leaf, orig, m_leaf );
390                         sgdXformPnt3( dir_leaf,  dir,  m_leaf );
391                         first_time = 0;
392                     }
393                     GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
394                     IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
395                 }
396             }  // Out of range
397         } // branch not requested to be traversed
398     } // end for loop
399 }
400
401
402
403 // ======================
404 // a temporary hack until we get everything rewritten with sgdVec3
405 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
406 {
407     return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
408 }
409
410
411 // ======================
412 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
413     sgdMat4 m;
414     clear();
415     sgdMakeIdentMat4 ( m ) ;
416     IntersectBranch( scene, m, orig, dir );
417 }
418
419 // ======================
420 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
421 {
422     clear();
423     IntersectBranch( scene, m, orig, dir );
424 }
425
426 // ======================
427 // Need these because of mixed matrix types
428 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
429 {
430     for ( int j = 0 ; j < 4 ; j++ )
431     {
432         dst[0][j] = m2[0][0] * m1[0][j] +
433                     m2[0][1] * m1[1][j] +
434                     m2[0][2] * m1[2][j] +
435                     m2[0][3] * m1[3][j] ;
436
437         dst[1][j] = m2[1][0] * m1[0][j] +
438                     m2[1][1] * m1[1][j] +
439                     m2[1][2] * m1[2][j] +
440                     m2[1][3] * m1[3][j] ;
441
442         dst[2][j] = m2[2][0] * m1[0][j] +
443                     m2[2][1] * m1[1][j] +
444                     m2[2][2] * m1[2][j] +
445                     m2[2][3] * m1[3][j] ;
446
447         dst[3][j] = m2[3][0] * m1[0][j] +
448                     m2[3][1] * m1[1][j] +
449                     m2[3][2] * m1[2][j] +
450                     m2[3][3] * m1[3][j] ;
451     }
452 }
453
454 // ======================
455 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
456 {
457     /*
458        Walk backwards up the tree, transforming the
459        vertex by all the matrices along the way.
460
461        Upwards recursion hurts my head.
462        */
463
464     sgMat4 mat ;
465
466     /*
467        If this node has a parent - get the composite
468        matrix for the parent.
469        */
470     if ( entity->getNumParents() > 0 )
471         ssgGetEntityTransform ( entity->getParent(0), mat ) ;
472     else
473         sgMakeIdentMat4 ( mat ) ;
474
475     /*
476        If this node has a transform - then concatenate it.
477        */
478     if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
479         sgMat4 this_mat ;
480         ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
481         sgPostMultMat4 ( mat, this_mat ) ;
482     }
483
484     sgCopyMat4 ( m, mat ) ;
485 }
486
487 // ======================
488 // return the passed entitity's bsphere's center point radius and
489 // fully formed current model matrix for entity
490 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
491 {
492     sgSphere *bsphere = entity->getBSphere();
493     *radius = (double)bsphere->getRadius();
494     sgCopyVec3( center, bsphere->getCenter() );
495     sgMakeIdentMat4 ( m ) ;
496     ssgGetEntityTransform( entity, m );
497 }
498
499
500 // ======================
501 // Determine scenery altitude via ssg.
502 // returned results are in meters
503 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
504                     FGHitList *hit_list,
505                     double *terrain_elev, double *radius, double *normal)
506 {
507     sgdVec3 view_pos;
508     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
509
510     sgdVec3 orig, dir;
511     sgdCopyVec3(orig, view_pos );
512     sgdCopyVec3(dir, abs_view_pos );
513
514     hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
515                          orig, dir );
516
517     int this_hit=0;
518     Point3D geoc;
519     double result = -9999;
520     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
521
522     // cout << "hits = ";
523     int hitcount = hit_list->num_hits();
524     for ( int i = 0; i < hitcount; ++i ) {
525         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
526         double lat_geod, alt, sea_level_r;
527         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
528                      &alt, &sea_level_r);
529         // cout << alt << " ";
530         if ( alt > result && alt < 10000 ) {
531             result = alt;
532             this_hit = i;
533         }
534     }
535     // cout << endl;
536
537     if ( result > -9000 ) {
538         *terrain_elev = result;
539         *radius = geoc.radius();
540         sgVec3 tmp;
541         sgSetVec3(tmp, hit_list->get_normal(this_hit));
542         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
543         sgdSetVec3( normal, tmp );
544         // float *up = globals->get_current_view()->get_world_up();
545         // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
546         /* ssgState *IntersectedLeafState =
547               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
548         return true;
549     } else {
550         SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
551         *terrain_elev = 0.0;
552         float *up = globals->get_current_view()->get_world_up();
553         sgdSetVec3(normal, up[0], up[1], up[2]);
554         return false;
555     }
556 }
557
558
559 // ======================
560 // Determine scenery altitude via ssg.
561 // returned results are in meters
562 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
563                     ssgTransform *terra_transform,
564                     FGHitList *hit_list,
565                     double *terrain_elev, double *radius, double *normal)
566 {
567     sgdVec3 view_pos;
568     sgdSubVec3( view_pos, abs_view_pos, scenery_center );
569
570     sgdVec3 orig, dir;
571     sgdCopyVec3(orig, view_pos );
572
573     sgdCopyVec3(dir, abs_view_pos );
574     sgdNormalizeVec3(dir);
575
576     sgMat4 fxform;
577     sgMakeIdentMat4 ( fxform ) ;
578     ssgGetEntityTransform( terra_transform, fxform );
579
580     sgdMat4 xform;
581     sgdSetMat4(xform,fxform);
582     hit_list->Intersect( terra_transform, xform, orig, dir );
583
584     int this_hit=0;
585     Point3D geoc;
586     double result = -9999;
587     Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
588
589     int hitcount = hit_list->num_hits();
590     for ( int i = 0; i < hitcount; ++i ) {
591         geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
592         double lat_geod, alt, sea_level_r;
593         sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
594                      &alt, &sea_level_r);
595         if ( alt > result && alt < 20000 ) {
596             result = alt;
597             this_hit = i;
598         }
599     }
600
601     if ( result > -9000 ) {
602         *terrain_elev = result;
603         *radius = geoc.radius();
604         sgVec3 tmp;
605         sgSetVec3(tmp, hit_list->get_normal(this_hit));
606         // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "  << tmp[2] << endl;
607         sgdSetVec3( normal, tmp );
608         // float *up = globals->get_current_view()->get_world_up();
609        // cout << "world_up  : " << up[0] << " " << up[1] << " " << up[2] << endl;
610         /* ssgState *IntersectedLeafState =
611               ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
612         return true;
613     } else {
614         SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
615         return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
616                               terrain_elev,radius,normal);
617     }
618 }
619