2 // Height Over Terrain and Assosciated Routines for FlightGear based Scenery
3 // Written by Norman Vine, started 2000.
15 #include <simgear/sg_inlines.h>
16 #include <simgear/debug/logstream.hxx>
17 #include <simgear/math/point3d.hxx>
18 #include <simgear/math/sg_geodesy.hxx>
19 #include <simgear/math/vector.hxx>
20 #include <simgear/timing/timestamp.hxx>
22 #include <Main/globals.hxx>
23 #include <Main/viewer.hxx>
24 #include <Scenery/scenery.hxx>
26 #include "hitlist.hxx"
28 // Specialized version of sgMultMat4 needed because of mixed matrix
30 static inline void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
31 for ( int j = 0 ; j < 4 ; j++ ) {
32 dst[0][j] = m2[0][0] * m1[0][j] +
37 dst[1][j] = m2[1][0] * m1[0][j] +
42 dst[2][j] = m2[2][0] * m1[0][j] +
47 dst[3][j] = m2[3][0] * m1[0][j] +
56 * Walk backwards up the tree, transforming the vertex by all the
57 * matrices along the way.
59 * Upwards recursion hurts my head.
61 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m ) {
64 // If this node has a parent - get the composite matrix for the
66 if ( entity->getNumParents() > 0 )
67 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
69 sgMakeIdentMat4 ( mat ) ;
71 // If this node has a transform - then concatenate it.
72 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
74 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
75 sgPostMultMat4 ( mat, this_mat ) ;
78 sgCopyMat4 ( m, mat ) ;
82 // return the passed entitity's bsphere's center point radius and
83 // fully formed current model matrix for entity
84 static inline void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
85 float *radius, sgMat4 m )
87 sgSphere *bsphere = entity->getBSphere();
88 *radius = (double)bsphere->getRadius();
89 sgCopyVec3( center, bsphere->getCenter() );
90 sgMakeIdentMat4 ( m ) ;
91 ssgGetEntityTransform( entity, m );
95 // This is same as PLib's sgdIsectInfLinePlane() and can be replaced
96 // by it after the next PLib release
97 static inline bool fgdIsectInfLinePlane( sgdVec3 dst,
100 const sgdVec4 plane )
102 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
104 /* Is line parallel to plane? */
106 if ( fabs ( tmp ) < FLT_EPSILON )
109 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
110 + plane[3] ) / tmp ) ;
111 sgdAddVec3 ( dst, l_org ) ;
117 * Given a point and a triangle lying on the same plane check to see
118 * if the point is inside the triangle
120 * This is same as PLib's sgdPointInTriangle() and can be replaced by
121 * it after the next PLib release
123 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
127 // Some tolerance in meters we accept a point to be outside of the triangle
128 // and still return that it is inside.
131 // punt if outside bouding cube
132 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
133 if( (point[0] < min - eps) || (point[0] > max + eps) )
137 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
138 if( (point[1] < min - eps) || (point[1] > max + eps) )
142 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
143 if( (point[2] < min - eps) || (point[2] > max + eps) )
147 // drop the smallest dimension so we only have to work in 2d.
148 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
149 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
150 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
151 // x is the smallest dimension
160 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
161 // y is the smallest dimension
170 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
171 // z is the smallest dimension
181 // all dimensions are really small so lets call it close
182 // enough and return a successful match
186 // check if intersection point is on the same side of p1 <-> p2 as p3
187 SGDfloat tmp = (y2 - y3);
188 SGDfloat tmpn = (x2 - x3);
189 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
190 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
191 + side1 * eps * fabs(tmpn));
192 if ( side1 != side2 ) {
193 // printf("failed side 1 check\n");
197 // check if intersection point is on correct side of p2 <-> p3 as p1
200 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
201 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
202 + side1 * eps * fabs(tmpn));
203 if ( side1 != side2 ) {
204 // printf("failed side 2 check\n");
208 // check if intersection point is on correct side of p1 <-> p3 as p2
211 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
212 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
213 + side1 * eps * fabs(tmpn));
214 if ( side1 != side2 ) {
215 // printf("failed side 3 check\n");
223 // Check if all three vertices are the same point (or close enough)
224 static inline int isZeroAreaTri( sgdVec3 tri[3] )
226 return( sgdEqualVec3(tri[0], tri[1]) ||
227 sgdEqualVec3(tri[1], tri[2]) ||
228 sgdEqualVec3(tri[2], tri[0]) );
233 FGHitList::FGHitList() :
234 last(NULL), test_dist(DBL_MAX)
240 FGHitList::~FGHitList() {}
244 Find the intersection of an infinite line with a leaf the line being
245 defined by a point and direction.
249 ssgLeaf pointer -- leaf
250 qualified matrix -- m
252 line direction -- dir
254 result -- intersection point
255 normal -- intersected tri's normal
258 true if intersection found
263 If you need an exhaustive list of hitpoints YOU MUST use the generic
264 version of this function as the specialized versions will do an early
265 out of expensive tests if the point can not be the closest one found
269 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
270 sgdVec3 orig, sgdVec3 dir )
275 for ( ; i < leaf->getNumTriangles(); ++i ) {
277 leaf->getTriangle( i, &i1, &i2, &i3 );
280 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
281 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
282 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
284 if( isZeroAreaTri( tri ) )
288 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
291 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
292 if( fgdPointInTriangle( point, tri ) ) {
293 // transform point into passed into desired coordinate frame
294 sgdXformPnt3( point, point, m );
295 sgdXformPnt4(plane,plane,m);
296 add(leaf,i,point,plane);
305 // Short circuit/slightly optimized version of the full IntersectLeaf()
306 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
307 sgdVec3 orig, sgdVec3 dir,
312 // number of hits but there could be more that
313 // were not found because of short circut switch !
314 // so you may want to use the unspecialized IntersectLeaf()
317 int ntri = leaf->getNumTriangles();
318 for ( n = 0; n < ntri; ++n )
325 SG_LOG( SG_TERRAIN, SG_ALERT,
326 "WARNING: dubiously handled GL_POLYGON" );
327 case GL_TRIANGLE_FAN :
328 /* SG_LOG( SG_TERRAIN, SG_ALERT,
329 "IntersectLeaf: GL_TRIANGLE_FAN" ); */
331 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
332 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
333 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
335 sgdCopyVec3( tri[1], tri[2] );
336 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
340 /* SG_LOG( SG_TERRAIN, SG_DEBUG,
341 "IntersectLeaf: GL_TRIANGLES" ); */
342 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
343 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
344 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
347 SG_LOG( SG_TERRAIN, SG_ALERT,
348 "WARNING: dubiously handled GL_QUAD_STRIP" );
349 case GL_TRIANGLE_STRIP :
350 /* SG_LOG( SG_TERRAIN, SG_ALERT,
351 "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
353 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
354 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
355 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
358 sgdCopyVec3( tri[0], tri[2] );
359 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
361 sgdCopyVec3( tri[1], tri[2] );
362 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
367 SG_LOG( SG_TERRAIN, SG_ALERT,
368 "WARNING: dubiously handled GL_QUADS" );
369 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
370 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
371 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
374 SG_LOG( SG_TERRAIN, SG_ALERT,
375 "WARNING: not-handled structure: " << primType );
376 return IntersectLeaf( leaf, m, orig, dir);
379 if( isZeroAreaTri( tri ) )
383 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
387 // find point of intersection of line from point org
388 // in direction dir with triangle's plane
389 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
390 /* Is line parallel to plane? */
391 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
394 // find parametric point
395 sgdScaleVec3 ( point, dir,
396 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
399 // short circut if this point is further away then a previous hit
400 tmp_dist = sgdDistanceSquaredVec3(point, orig );
401 if( tmp_dist > test_dist )
404 // place parametric point in world
405 sgdAddVec3 ( point, orig ) ;
407 if( fgdPointInTriangle( point, tri ) ) {
408 // transform point into passed coordinate frame
409 sgdXformPnt3( point, point, m );
410 sgdXformPnt4(plane,plane,m);
411 add(leaf,n,point,plane);
412 test_dist = tmp_dist;
421 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
422 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
426 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
427 sgdVec3 orig, sgdVec3 dir )
429 /* the lookat vector and matrix in branch's coordinate frame but
430 * we won't determine these unless needed, This 'lazy evaluation'
431 * is a result of profiling data */
432 sgdVec3 orig_leaf, dir_leaf;
435 // 'lazy evaluation' flag
438 for ( ssgEntity *kid = branch->getKid( 0 );
440 kid = branch->getNextKid() )
442 if ( kid->getTraversalMask() & SSGTRAV_HOT
443 && !kid->getBSphere()->isEmpty() )
447 kid->getBSphere()->getCenter()[0],
448 kid->getBSphere()->getCenter()[1],
449 kid->getBSphere()->getCenter()[2] );
450 sgdXformPnt3( center, m ) ;
452 // sgdClosestPointToLineDistSquared( center, orig, dir )
453 // inlined here because because of profiling results
455 sgdSubVec3(u, center, orig);
456 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
457 / sgdScalarProductVec3(dir,dir) );
458 sgdSubVec3(v, u, u1);
460 // double because of possible overflow
461 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
463 if ( kid->isAKindOf ( ssgTypeBranch() ) )
466 if ( kid->isA( ssgTypeTransform() ) )
469 ((ssgTransform *)kid)->getTransform( fxform );
470 sgMultMat4(m_new, m, fxform);
472 sgdCopyMat4(m_new, m);
474 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
476 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
479 sgdTransposeNegateMat4( m_leaf, m );
480 sgdXformPnt3( orig_leaf, orig, m_leaf );
481 sgdXformVec3( dir_leaf, dir, m_leaf );
484 // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
485 // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
487 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
490 } // branch not requested to be traversed
495 // a temporary hack until we get everything rewritten with sgdVec3
496 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
498 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
502 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
505 sgdMakeIdentMat4 ( m ) ;
506 IntersectBranch( scene, m, orig, dir );
510 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
513 IntersectBranch( scene, m, orig, dir );
517 // Determine scenery altitude via ssg.
518 // returned results are in meters
519 // static double hitlist1_time = 0.0;
521 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
522 sgdVec3 scenery_center,
524 double *terrain_elev, double *radius, double *normal)
526 // SGTimeStamp start; start.stamp();
530 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
533 sgdCopyVec3(orig, view_pos );
534 sgdCopyVec3(dir, abs_view_pos );
536 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
542 double hit_elev = -9999;
543 double max_elev = -9999;
544 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
546 int hitcount = hit_list->num_hits();
547 // cout << "hits = " << hitcount << endl;
548 for ( int i = 0; i < hitcount; ++i ) {
549 // FIXME: sgCartToGeod is slow. Call it just once for the
550 // "sc" point, and then handle the rest with a geodetic "up"
551 // vector approximation. Across one tile, this will be
553 double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
554 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
555 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
557 if ( alt > hit_elev && alt < max_alt_m ) {
558 // cout << " it's a keeper" << endl;
562 if ( alt > hit_elev ) {
568 if ( this_hit < 0 ) {
569 // no hits below us, take the max hit
574 if ( hit_elev > -9000 ) {
575 *terrain_elev = hit_elev;
576 *radius = geoc.radius();
578 sgSetVec3(tmp, hit_list->get_normal(this_hit));
579 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
580 sgdSetVec3( normal, tmp );
581 // float *up = globals->get_current_view()->get_world_up();
582 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
583 /* ssgState *IntersectedLeafState =
584 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
587 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
589 float *up = globals->get_current_view()->get_world_up();
590 sgdSetVec3(normal, up[0], up[1], up[2]);
594 // SGTimeStamp finish; finish.stamp();
595 // hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
596 // cout << " time per call = " << hitlist1_time << endl;
602 // static double hitlist2_time = 0.0;
604 // Determine scenery altitude via ssg.
605 // returned results are in meters
606 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
607 sgdVec3 scenery_center,
608 ssgTransform *terra_transform,
610 double *terrain_elev, double *radius, double *normal)
612 // SGTimeStamp start; start.stamp();
616 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
619 sgdCopyVec3(orig, view_pos );
621 sgdCopyVec3(dir, abs_view_pos );
622 sgdNormalizeVec3(dir);
625 sgMakeIdentMat4 ( fxform ) ;
626 ssgGetEntityTransform( terra_transform, fxform );
629 sgdSetMat4(xform,fxform);
630 hit_list->Intersect( terra_transform, xform, orig, dir );
635 double hit_elev = -9999;
636 double max_elev = -9999;
637 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
639 int hitcount = hit_list->num_hits();
640 // cout << "hits = " << hitcount << endl;
641 for ( int i = 0; i < hitcount; ++i ) {
642 // FIXME: sgCartToGeod is slow. Call it just once for the
643 // "sc" point, and then handle the rest with a geodetic "up"
644 // vector approximation. Across one tile, this will be
646 double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
647 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
648 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
650 if ( alt > hit_elev && alt < max_alt_m ) {
653 // cout << " it's a keeper" << endl;
655 if ( alt > hit_elev ) {
661 if ( this_hit < 0 ) {
662 // no hits below us, take the max hit
667 if ( hit_elev > -9000 ) {
668 *terrain_elev = hit_elev;
669 *radius = geoc.radius();
671 sgSetVec3(tmp, hit_list->get_normal(this_hit));
672 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
673 sgdSetVec3( normal, tmp );
674 // float *up = globals->get_current_view()->get_world_up();
675 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
676 /* ssgState *IntersectedLeafState =
677 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
680 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
681 result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
682 hit_list, terrain_elev, radius, normal);
685 // SGTimeStamp finish; finish.stamp();
686 // hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
687 // cout << "time per call 2 = " << hitlist2_time << endl;