2 // Height Over Terrain and Assosciated Routines for FlightGear based Scenery
3 // Written by Norman Vine, started 2000.
14 #include <simgear/sg_inlines.h>
15 #include <simgear/debug/logstream.hxx>
16 #include <simgear/math/point3d.hxx>
17 #include <simgear/math/polar3d.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() {}
243 // http://www.cs.lth.se/home/Tomas_Akenine_Moller/raytri/raytri.c
244 // http://little3d.free.fr/ressources/jgt%20Fast,%20Minumum%20Storage%20Ray-Triangle%20Intersection.htm
245 // http://www.acm.org/jgt/papers/MollerTrumbore97/
247 /* Ray-Triangle Intersection Test Routines */
248 /* Different optimizations of my and Ben Trumbore's */
249 /* code from journals of graphics tools (JGT) */
250 /* http://www.acm.org/jgt/ */
251 /* by Tomas Moller, May 2000 */
253 /* code rewritten to do tests on the sign of the determinant */
254 /* the division is at the end in the code */
255 // cosmetics change by H.J :
256 // make u & v locals since we don't use them, use sg functions
257 static bool intersect_triangle(const double orig[3], const double dir[3],
258 const double vert0[3], const double vert1[3], const double vert2[3],
262 double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
264 const SGDfloat eps = 1e-4;
266 /* find vectors for two edges sharing vert0 */
267 sgdSubVec3(edge1, vert1, vert0);
268 sgdSubVec3(edge2, vert2, vert0);
270 /* begin calculating determinant - also used to calculate U parameter */
271 sgdVectorProductVec3(pvec, dir, edge2);
273 /* if determinant is near zero, ray lies in plane of triangle */
274 double det = sgdScalarProductVec3(edge1, pvec);
278 /* calculate distance from vert0 to ray origin */
279 sgdSubVec3(tvec, orig, vert0);
281 /* calculate U parameter and test bounds */
282 u = sgdScalarProductVec3(tvec, pvec);
283 if (u < 0.0 || u > det)
286 /* prepare to test V parameter */
287 sgdVectorProductVec3(qvec, tvec, edge1);
289 /* calculate V parameter and test bounds */
290 v = sgdScalarProductVec3(dir, qvec);
291 if (v < 0.0 || u + v > det)
297 /* calculate distance from vert0 to ray origin */
298 sgdSubVec3(tvec, orig, vert0);
300 /* calculate U parameter and test bounds */
301 u = sgdScalarProductVec3(tvec, pvec);
302 if (u > 0.0 || u < det)
305 /* prepare to test V parameter */
306 sgdVectorProductVec3(qvec, tvec, edge1);
308 /* calculate V parameter and test bounds */
309 v = sgdScalarProductVec3(dir, qvec) ;
310 if (v > 0.0 || u + v < det)
313 else return false; /* ray is parallell to the plane of the triangle */
315 /* calculate t, ray intersects triangle */
316 *t = sgdScalarProductVec3(edge2, qvec) / det;
323 Find the intersection of an infinite line with a leaf the line being
324 defined by a point and direction.
328 ssgLeaf pointer -- leaf
329 qualified matrix -- m
331 line direction -- dir
333 result -- intersection point
334 normal -- intersected tri's normal
337 true if intersection found
342 If you need an exhaustive list of hitpoints YOU MUST use the generic
343 version of this function as the specialized versions will do an early
344 out of expensive tests if the point can not be the closest one found
348 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
349 sgdVec3 orig, sgdVec3 dir )
354 for ( ; i < leaf->getNumTriangles(); ++i ) {
356 leaf->getTriangle( i, &i1, &i2, &i3 );
359 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
360 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
361 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
364 if( intersect_triangle( orig, dir, tri[0], tri[1], tri[2], &t) ) {
366 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
367 // t is the distance to the triangle plane
368 // so P = Orig + t*dir
370 sgdAddScaledVec3( point, orig, dir, t );
371 sgdXformPnt3( point, point, m );
372 sgdXformPnt4(plane,plane,m);
373 add(leaf,i,point,plane);
377 if( isZeroAreaTri( tri ) )
381 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
384 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
385 if( fgdPointInTriangle( point, tri ) ) {
386 // transform point into passed into desired coordinate frame
387 sgdXformPnt3( point, point, m );
388 sgdXformPnt4(plane,plane,m);
389 add(leaf,i,point,plane);
399 // Short circuit/slightly optimized version of the full IntersectLeaf()
400 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
401 sgdVec3 orig, sgdVec3 dir,
406 // number of hits but there could be more that
407 // were not found because of short circut switch !
408 // so you may want to use the unspecialized IntersectLeaf()
411 int ntri = leaf->getNumTriangles();
412 for ( n = 0; n < ntri; ++n )
419 SG_LOG( SG_TERRAIN, SG_ALERT,
420 "WARNING: dubiously handled GL_POLYGON" );
421 case GL_TRIANGLE_FAN :
422 /* SG_LOG( SG_TERRAIN, SG_ALERT,
423 "IntersectLeaf: GL_TRIANGLE_FAN" ); */
425 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
426 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
427 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
429 sgdCopyVec3( tri[1], tri[2] );
430 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
434 /* SG_LOG( SG_TERRAIN, SG_DEBUG,
435 "IntersectLeaf: GL_TRIANGLES" ); */
436 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
437 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
438 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
441 SG_LOG( SG_TERRAIN, SG_ALERT,
442 "WARNING: dubiously handled GL_QUAD_STRIP" );
443 case GL_TRIANGLE_STRIP :
444 /* SG_LOG( SG_TERRAIN, SG_ALERT,
445 "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
447 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
448 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
449 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
452 sgdCopyVec3( tri[0], tri[2] );
453 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
455 sgdCopyVec3( tri[1], tri[2] );
456 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
461 SG_LOG( SG_TERRAIN, SG_ALERT,
462 "WARNING: dubiously handled GL_QUADS" );
463 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
464 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
465 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
468 SG_LOG( SG_TERRAIN, SG_ALERT,
469 "WARNING: not-handled structure: " << primType );
470 return IntersectLeaf( leaf, m, orig, dir);
473 if( isZeroAreaTri( tri ) )
477 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
481 // find point of intersection of line from point org
482 // in direction dir with triangle's plane
483 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
484 /* Is line parallel to plane? */
485 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
488 // find parametric point
489 sgdScaleVec3 ( point, dir,
490 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
493 // short circut if this point is further away then a previous hit
494 tmp_dist = sgdDistanceSquaredVec3(point, orig );
495 if( tmp_dist > test_dist )
498 // place parametric point in world
499 sgdAddVec3 ( point, orig ) ;
501 if( fgdPointInTriangle( point, tri ) ) {
502 // transform point into passed coordinate frame
503 sgdXformPnt3( point, point, m );
504 sgdXformPnt4(plane,plane,m);
505 add(leaf,n,point,plane);
506 test_dist = tmp_dist;
515 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
516 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
520 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
521 sgdVec3 orig, sgdVec3 dir )
523 /* the lookat vector and matrix in branch's coordinate frame but
524 * we won't determine these unless needed, This 'lazy evaluation'
525 * is a result of profiling data */
526 sgdVec3 orig_leaf, dir_leaf;
529 // 'lazy evaluation' flag
532 for ( ssgEntity *kid = branch->getKid( 0 );
534 kid = branch->getNextKid() )
536 if ( kid->getTraversalMask() & SSGTRAV_HOT
537 && !kid->getBSphere()->isEmpty() )
540 const sgFloat *BSCenter = kid->getBSphere()->getCenter();
545 sgdXformPnt3( center, m ) ;
547 // sgdClosestPointToLineDistSquared( center, orig, dir )
548 // inlined here because because of profiling results
550 sgdSubVec3(u, center, orig);
551 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir) );
552 sgdSubVec3(v, u, u1);
554 // double because of possible overflow
555 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
557 if ( kid->isAKindOf ( ssgTypeBranch() ) )
560 if ( kid->isA( ssgTypeTransform() ) )
563 ((ssgTransform *)kid)->getTransform( fxform );
564 sgMultMat4(m_new, m, fxform);
566 sgdCopyMat4(m_new, m);
568 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
570 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
573 sgdTransposeNegateMat4( m_leaf, m );
574 sgdXformPnt3( orig_leaf, orig, m_leaf );
575 sgdXformVec3( dir_leaf, dir, m_leaf );
578 // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
579 // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
581 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
584 } // branch not requested to be traversed
589 // a temporary hack until we get everything rewritten with sgdVec3
590 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
592 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
596 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
599 sgdMakeIdentMat4 ( m ) ;
600 IntersectBranch( scene, m, orig, dir );
604 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
607 IntersectBranch( scene, m, orig, dir );
611 // Determine scenery altitude via ssg.
612 // returned results are in meters
613 // static double hitlist1_time = 0.0;
615 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
616 sgdVec3 scenery_center,
618 double *terrain_elev, double *radius, double *normal)
620 // SGTimeStamp start; start.stamp();
624 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
627 sgdCopyVec3(orig, view_pos );
628 sgdCopyVec3(dir, abs_view_pos );
630 sgdNormaliseVec3( dir );
631 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
636 double hit_elev = -9999;
637 double max_elev = -9999;
638 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
640 int hitcount = hit_list->num_hits();
641 // cout << "hits = " << hitcount << endl;
642 for ( int i = 0; i < hitcount; ++i ) {
643 // FIXME: sgCartToGeod is slow. Call it just once for the
644 // "sc" point, and then handle the rest with a geodetic "up"
645 // vector approximation. Across one tile, this will be
647 double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
648 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
649 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
651 if ( alt > hit_elev && alt < max_alt_m ) {
652 // cout << " it's a keeper" << endl;
656 if ( alt > hit_elev ) {
662 if ( this_hit < 0 ) {
663 // no hits below us, take the max hit
668 if ( hit_elev > -9000 ) {
669 *terrain_elev = hit_elev;
670 *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
672 sgSetVec3(tmp, hit_list->get_normal(this_hit));
673 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
674 sgdSetVec3( normal, tmp );
675 // float *up = globals->get_current_view()->get_world_up();
676 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
677 /* ssgState *IntersectedLeafState =
678 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
681 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
683 float *up = globals->get_current_view()->get_world_up();
684 sgdSetVec3(normal, up[0], up[1], up[2]);
688 // SGTimeStamp finish; finish.stamp();
689 // hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
690 // cout << " time per call = " << hitlist1_time << endl;
696 // static double hitlist2_time = 0.0;
698 // Determine scenery altitude via ssg.
699 // returned results are in meters
700 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
701 sgdVec3 scenery_center,
704 double *terrain_elev, double *radius, double *normal,
707 // SGTimeStamp start; start.stamp();
711 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
714 sgdCopyVec3(orig, view_pos );
716 sgdCopyVec3(dir, abs_view_pos );
717 sgdNormalizeVec3(dir);
720 sgMakeIdentMat4 ( fxform ) ;
721 ssgGetEntityTransform( branch, fxform );
724 sgdSetMat4(xform,fxform);
725 hit_list->Intersect( branch, xform, orig, dir );
729 double hit_elev = -9999;
730 double max_elev = -9999;
731 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
733 int hitcount = hit_list->num_hits();
734 // cout << "hits = " << hitcount << endl;
735 for ( int i = 0; i < hitcount; ++i ) {
736 // FIXME: sgCartToGeod is slow. Call it just once for the
737 // "sc" point, and then handle the rest with a geodetic "up"
738 // vector approximation. Across one tile, this will be
740 double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
741 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
742 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
744 if ( alt > hit_elev && alt < max_alt_m ) {
747 // cout << " it's a keeper" << endl;
749 if ( alt > hit_elev ) {
756 if ( this_hit < 0 ) {
757 // no hits below us, take the max hit
762 if ( hit_elev > -9000 ) {
763 *terrain_elev = hit_elev;
764 *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
767 sgSetVec3(tmp, hit_list->get_normal(this_hit));
768 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
769 sgdSetVec3( normal, tmp );
770 // float *up = globals->get_current_view()->get_world_up();
771 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
772 /* ssgState *IntersectedLeafState =
773 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
776 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
777 result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
778 hit_list, terrain_elev, radius, normal);
781 // SGTimeStamp finish; finish.stamp();
782 // hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
783 // cout << "time per call 2 = " << hitlist2_time << endl;