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/polar3d.hxx>
19 #include <simgear/math/sg_geodesy.hxx>
20 #include <simgear/math/vector.hxx>
21 #include <simgear/timing/timestamp.hxx>
23 #include <Main/globals.hxx>
24 #include <Main/viewer.hxx>
25 #include <Scenery/scenery.hxx>
27 #include "hitlist.hxx"
29 // Specialized version of sgMultMat4 needed because of mixed matrix
31 static inline void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
32 for ( int j = 0 ; j < 4 ; j++ ) {
33 dst[0][j] = m2[0][0] * m1[0][j] +
38 dst[1][j] = m2[1][0] * m1[0][j] +
43 dst[2][j] = m2[2][0] * m1[0][j] +
48 dst[3][j] = m2[3][0] * m1[0][j] +
57 * Walk backwards up the tree, transforming the vertex by all the
58 * matrices along the way.
60 * Upwards recursion hurts my head.
62 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m ) {
65 // If this node has a parent - get the composite matrix for the
67 if ( entity->getNumParents() > 0 )
68 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
70 sgMakeIdentMat4 ( mat ) ;
72 // If this node has a transform - then concatenate it.
73 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
75 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
76 sgPostMultMat4 ( mat, this_mat ) ;
79 sgCopyMat4 ( m, mat ) ;
83 // return the passed entitity's bsphere's center point radius and
84 // fully formed current model matrix for entity
85 static inline void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
86 float *radius, sgMat4 m )
88 sgSphere *bsphere = entity->getBSphere();
89 *radius = (double)bsphere->getRadius();
90 sgCopyVec3( center, bsphere->getCenter() );
91 sgMakeIdentMat4 ( m ) ;
92 ssgGetEntityTransform( entity, m );
96 // This is same as PLib's sgdIsectInfLinePlane() and can be replaced
97 // by it after the next PLib release
98 static inline bool fgdIsectInfLinePlane( sgdVec3 dst,
101 const sgdVec4 plane )
103 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
105 /* Is line parallel to plane? */
107 if ( fabs ( tmp ) < FLT_EPSILON )
110 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
111 + plane[3] ) / tmp ) ;
112 sgdAddVec3 ( dst, l_org ) ;
118 * Given a point and a triangle lying on the same plane check to see
119 * if the point is inside the triangle
121 * This is same as PLib's sgdPointInTriangle() and can be replaced by
122 * it after the next PLib release
124 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
128 // Some tolerance in meters we accept a point to be outside of the triangle
129 // and still return that it is inside.
132 // punt if outside bouding cube
133 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
134 if( (point[0] < min - eps) || (point[0] > max + eps) )
138 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
139 if( (point[1] < min - eps) || (point[1] > max + eps) )
143 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
144 if( (point[2] < min - eps) || (point[2] > max + eps) )
148 // drop the smallest dimension so we only have to work in 2d.
149 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
150 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
151 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
152 // x is the smallest dimension
161 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
162 // y is the smallest dimension
171 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
172 // z is the smallest dimension
182 // all dimensions are really small so lets call it close
183 // enough and return a successful match
187 // check if intersection point is on the same side of p1 <-> p2 as p3
188 SGDfloat tmp = (y2 - y3);
189 SGDfloat tmpn = (x2 - x3);
190 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
191 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
192 + side1 * eps * fabs(tmpn));
193 if ( side1 != side2 ) {
194 // printf("failed side 1 check\n");
198 // check if intersection point is on correct side of p2 <-> p3 as p1
201 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
202 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
203 + side1 * eps * fabs(tmpn));
204 if ( side1 != side2 ) {
205 // printf("failed side 2 check\n");
209 // check if intersection point is on correct side of p1 <-> p3 as p2
212 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
213 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
214 + side1 * eps * fabs(tmpn));
215 if ( side1 != side2 ) {
216 // printf("failed side 3 check\n");
224 // Check if all three vertices are the same point (or close enough)
225 static inline int isZeroAreaTri( sgdVec3 tri[3] )
227 return( sgdEqualVec3(tri[0], tri[1]) ||
228 sgdEqualVec3(tri[1], tri[2]) ||
229 sgdEqualVec3(tri[2], tri[0]) );
234 FGHitList::FGHitList() :
235 last(NULL), test_dist(DBL_MAX)
241 FGHitList::~FGHitList() {}
244 // http://www.cs.lth.se/home/Tomas_Akenine_Moller/raytri/raytri.c
245 // http://little3d.free.fr/ressources/jgt%20Fast,%20Minumum%20Storage%20Ray-Triangle%20Intersection.htm
246 // http://www.acm.org/jgt/papers/MollerTrumbore97/
248 /* Ray-Triangle Intersection Test Routines */
249 /* Different optimizations of my and Ben Trumbore's */
250 /* code from journals of graphics tools (JGT) */
251 /* http://www.acm.org/jgt/ */
252 /* by Tomas Moller, May 2000 */
254 /* code rewritten to do tests on the sign of the determinant */
255 /* the division is at the end in the code */
256 // cosmetics change by H.J :
257 // make u & v locals since we don't use them, use sg functions
258 static bool intersect_triangle(const double orig[3], const double dir[3],
259 const double vert0[3], const double vert1[3], const double vert2[3],
263 double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
265 const SGDfloat eps = 1e-4;
267 /* find vectors for two edges sharing vert0 */
268 sgdSubVec3(edge1, vert1, vert0);
269 sgdSubVec3(edge2, vert2, vert0);
271 /* begin calculating determinant - also used to calculate U parameter */
272 sgdVectorProductVec3(pvec, dir, edge2);
274 /* if determinant is near zero, ray lies in plane of triangle */
275 double det = sgdScalarProductVec3(edge1, pvec);
279 /* calculate distance from vert0 to ray origin */
280 sgdSubVec3(tvec, orig, vert0);
282 /* calculate U parameter and test bounds */
283 u = sgdScalarProductVec3(tvec, pvec);
284 if (u < 0.0 || u > det)
287 /* prepare to test V parameter */
288 sgdVectorProductVec3(qvec, tvec, edge1);
290 /* calculate V parameter and test bounds */
291 v = sgdScalarProductVec3(dir, qvec);
292 if (v < 0.0 || u + v > det)
298 /* calculate distance from vert0 to ray origin */
299 sgdSubVec3(tvec, orig, vert0);
301 /* calculate U parameter and test bounds */
302 u = sgdScalarProductVec3(tvec, pvec);
303 if (u > 0.0 || u < det)
306 /* prepare to test V parameter */
307 sgdVectorProductVec3(qvec, tvec, edge1);
309 /* calculate V parameter and test bounds */
310 v = sgdScalarProductVec3(dir, qvec) ;
311 if (v > 0.0 || u + v < det)
314 else return false; /* ray is parallell to the plane of the triangle */
316 /* calculate t, ray intersects triangle */
317 *t = sgdScalarProductVec3(edge2, qvec) / det;
324 Find the intersection of an infinite line with a leaf the line being
325 defined by a point and direction.
329 ssgLeaf pointer -- leaf
330 qualified matrix -- m
332 line direction -- dir
334 result -- intersection point
335 normal -- intersected tri's normal
338 true if intersection found
343 If you need an exhaustive list of hitpoints YOU MUST use the generic
344 version of this function as the specialized versions will do an early
345 out of expensive tests if the point can not be the closest one found
349 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
350 sgdVec3 orig, sgdVec3 dir )
355 for ( ; i < leaf->getNumTriangles(); ++i ) {
357 leaf->getTriangle( i, &i1, &i2, &i3 );
360 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
361 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
362 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
365 if( intersect_triangle( orig, dir, tri[0], tri[1], tri[2], &t) ) {
367 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
368 // t is the distance to the triangle plane
369 // so P = Orig + t*dir
371 sgdAddScaledVec3( point, orig, dir, t );
372 sgdXformPnt3( point, point, m );
373 sgdXformPnt4(plane,plane,m);
374 add(leaf,i,point,plane);
378 if( isZeroAreaTri( tri ) )
382 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
385 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
386 if( fgdPointInTriangle( point, tri ) ) {
387 // transform point into passed into desired coordinate frame
388 sgdXformPnt3( point, point, m );
389 sgdXformPnt4(plane,plane,m);
390 add(leaf,i,point,plane);
400 // Short circuit/slightly optimized version of the full IntersectLeaf()
401 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
402 sgdVec3 orig, sgdVec3 dir,
407 // number of hits but there could be more that
408 // were not found because of short circut switch !
409 // so you may want to use the unspecialized IntersectLeaf()
412 int ntri = leaf->getNumTriangles();
413 for ( n = 0; n < ntri; ++n )
420 SG_LOG( SG_TERRAIN, SG_ALERT,
421 "WARNING: dubiously handled GL_POLYGON" );
422 case GL_TRIANGLE_FAN :
423 /* SG_LOG( SG_TERRAIN, SG_ALERT,
424 "IntersectLeaf: GL_TRIANGLE_FAN" ); */
426 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
427 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
428 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
430 sgdCopyVec3( tri[1], tri[2] );
431 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
435 /* SG_LOG( SG_TERRAIN, SG_DEBUG,
436 "IntersectLeaf: GL_TRIANGLES" ); */
437 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
438 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
439 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
442 SG_LOG( SG_TERRAIN, SG_ALERT,
443 "WARNING: dubiously handled GL_QUAD_STRIP" );
444 case GL_TRIANGLE_STRIP :
445 /* SG_LOG( SG_TERRAIN, SG_ALERT,
446 "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
448 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
449 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
450 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
453 sgdCopyVec3( tri[0], tri[2] );
454 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
456 sgdCopyVec3( tri[1], tri[2] );
457 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
462 SG_LOG( SG_TERRAIN, SG_ALERT,
463 "WARNING: dubiously handled GL_QUADS" );
464 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
465 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
466 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
469 SG_LOG( SG_TERRAIN, SG_ALERT,
470 "WARNING: not-handled structure: " << primType );
471 return IntersectLeaf( leaf, m, orig, dir);
474 if( isZeroAreaTri( tri ) )
478 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
482 // find point of intersection of line from point org
483 // in direction dir with triangle's plane
484 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
485 /* Is line parallel to plane? */
486 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
489 // find parametric point
490 sgdScaleVec3 ( point, dir,
491 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
494 // short circut if this point is further away then a previous hit
495 tmp_dist = sgdDistanceSquaredVec3(point, orig );
496 if( tmp_dist > test_dist )
499 // place parametric point in world
500 sgdAddVec3 ( point, orig ) ;
502 if( fgdPointInTriangle( point, tri ) ) {
503 // transform point into passed coordinate frame
504 sgdXformPnt3( point, point, m );
505 sgdXformPnt4(plane,plane,m);
506 add(leaf,n,point,plane);
507 test_dist = tmp_dist;
516 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
517 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
521 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
522 sgdVec3 orig, sgdVec3 dir )
524 /* the lookat vector and matrix in branch's coordinate frame but
525 * we won't determine these unless needed, This 'lazy evaluation'
526 * is a result of profiling data */
527 sgdVec3 orig_leaf, dir_leaf;
530 // 'lazy evaluation' flag
533 for ( ssgEntity *kid = branch->getKid( 0 );
535 kid = branch->getNextKid() )
537 if ( kid->getTraversalMask() & SSGTRAV_HOT
538 && !kid->getBSphere()->isEmpty() )
541 const sgFloat *BSCenter = kid->getBSphere()->getCenter();
546 sgdXformPnt3( center, m ) ;
548 // sgdClosestPointToLineDistSquared( center, orig, dir )
549 // inlined here because because of profiling results
551 sgdSubVec3(u, center, orig);
552 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir) );
553 sgdSubVec3(v, u, u1);
555 // double because of possible overflow
556 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
558 if ( kid->isAKindOf ( ssgTypeBranch() ) )
561 if ( kid->isA( ssgTypeTransform() ) )
564 ((ssgTransform *)kid)->getTransform( fxform );
565 sgMultMat4(m_new, m, fxform);
567 sgdCopyMat4(m_new, m);
569 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
571 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
574 sgdTransposeNegateMat4( m_leaf, m );
575 sgdXformPnt3( orig_leaf, orig, m_leaf );
576 sgdXformVec3( dir_leaf, dir, m_leaf );
579 // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
580 // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
582 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
585 } // branch not requested to be traversed
590 // a temporary hack until we get everything rewritten with sgdVec3
591 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
593 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
597 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
600 sgdMakeIdentMat4 ( m ) ;
601 IntersectBranch( scene, m, orig, dir );
605 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
608 IntersectBranch( scene, m, orig, dir );
612 // Determine scenery altitude via ssg.
613 // returned results are in meters
614 // static double hitlist1_time = 0.0;
616 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
617 sgdVec3 scenery_center,
619 double *terrain_elev, double *radius, double *normal)
621 // SGTimeStamp start; start.stamp();
625 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
628 sgdCopyVec3(orig, view_pos );
629 sgdCopyVec3(dir, abs_view_pos );
631 sgdNormaliseVec3( dir );
632 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
637 double hit_elev = -9999;
638 double max_elev = -9999;
639 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
641 int hitcount = hit_list->num_hits();
642 // cout << "hits = " << hitcount << endl;
643 for ( int i = 0; i < hitcount; ++i ) {
644 // FIXME: sgCartToGeod is slow. Call it just once for the
645 // "sc" point, and then handle the rest with a geodetic "up"
646 // vector approximation. Across one tile, this will be
648 double alt = sgCartToGeod( sc + hit_list->get_point(i) ).elev();
649 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
650 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
652 if ( alt > hit_elev && alt < max_alt_m ) {
653 // cout << " it's a keeper" << endl;
657 if ( alt > hit_elev ) {
663 if ( this_hit < 0 ) {
664 // no hits below us, take the max hit
669 if ( hit_elev > -9000 ) {
670 *terrain_elev = hit_elev;
671 *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
673 sgSetVec3(tmp, hit_list->get_normal(this_hit));
674 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
675 sgdSetVec3( normal, tmp );
676 // float *up = globals->get_current_view()->get_world_up();
677 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
678 /* ssgState *IntersectedLeafState =
679 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
682 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
684 float *up = globals->get_current_view()->get_world_up();
685 sgdSetVec3(normal, up[0], up[1], up[2]);
689 // SGTimeStamp finish; finish.stamp();
690 // hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
691 // cout << " time per call = " << hitlist1_time << endl;
697 // static double hitlist2_time = 0.0;
699 // Determine scenery altitude via ssg.
700 // returned results are in meters
701 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
702 sgdVec3 scenery_center,
703 ssgTransform *terra_transform,
705 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( terra_transform, fxform );
724 sgdSetMat4(xform,fxform);
725 hit_list->Intersect( terra_transform, 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;