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] )
128 // punt if outside bouding cube
129 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
130 if( (point[0] < min) || (point[0] > max) )
134 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
135 if( (point[1] < min) || (point[1] > max) )
139 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
140 if( (point[2] < min) || (point[2] > max) )
144 // drop the smallest dimension so we only have to work in 2d.
145 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
146 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
147 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
148 // x is the smallest dimension
157 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
158 // y is the smallest dimension
167 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
168 // z is the smallest dimension
178 // all dimensions are really small so lets call it close
179 // enough and return a successful match
183 // check if intersection point is on the same side of p1 <-> p2 as p3
184 SGDfloat tmp = (y2 - y3) / (x2 - x3);
185 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
186 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
187 if ( side1 != side2 ) {
188 // printf("failed side 1 check\n");
192 // check if intersection point is on correct side of p2 <-> p3 as p1
193 tmp = (y3 - ry) / (x3 - rx);
194 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
195 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
196 if ( side1 != side2 ) {
197 // printf("failed side 2 check\n");
201 // check if intersection point is on correct side of p1 <-> p3 as p2
202 tmp = (y2 - ry) / (x2 - rx);
203 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
204 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
205 if ( side1 != side2 ) {
206 // printf("failed side 3 check\n");
214 // Check if all three vertices are the same point (or close enough)
215 static inline int isZeroAreaTri( sgdVec3 tri[3] )
217 return( sgdEqualVec3(tri[0], tri[1]) ||
218 sgdEqualVec3(tri[1], tri[2]) ||
219 sgdEqualVec3(tri[2], tri[0]) );
224 FGHitList::FGHitList() :
225 last(NULL), test_dist(DBL_MAX)
231 FGHitList::~FGHitList() {}
235 Find the intersection of an infinite line with a leaf the line being
236 defined by a point and direction.
240 ssgLeaf pointer -- leaf
241 qualified matrix -- m
243 line direction -- dir
245 result -- intersection point
246 normal -- intersected tri's normal
249 true if intersection found
254 If you need an exhaustive list of hitpoints YOU MUST use the generic
255 version of this function as the specialized versions will do an early
256 out of expensive tests if the point can not be the closest one found
260 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
261 sgdVec3 orig, sgdVec3 dir )
266 for ( ; i < leaf->getNumTriangles(); ++i ) {
268 leaf->getTriangle( i, &i1, &i2, &i3 );
271 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
272 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
273 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
275 if( isZeroAreaTri( tri ) )
279 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
282 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
283 if( fgdPointInTriangle( point, tri ) ) {
284 // transform point into passed into desired coordinate frame
285 sgdXformPnt3( point, point, m );
286 sgdXformPnt4(plane,plane,m);
287 add(leaf,i,point,plane);
296 // Short circuit/slightly optimized version of the full IntersectLeaf()
297 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
298 sgdVec3 orig, sgdVec3 dir,
303 // number of hits but there could be more that
304 // were not found because of short circut switch !
305 // so you may want to use the unspecialized IntersectLeaf()
308 int ntri = leaf->getNumTriangles();
309 for ( n = 0; n < ntri; ++n )
316 SG_LOG( SG_TERRAIN, SG_ALERT,
317 "WARNING: dubiously handled GL_POLYGON" );
318 case GL_TRIANGLE_FAN :
319 /* SG_LOG( SG_TERRAIN, SG_ALERT,
320 "IntersectLeaf: GL_TRIANGLE_FAN" ); */
322 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
323 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
324 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
326 sgdCopyVec3( tri[1], tri[2] );
327 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
331 /* SG_LOG( SG_TERRAIN, SG_DEBUG,
332 "IntersectLeaf: GL_TRIANGLES" ); */
333 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
334 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
335 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
338 SG_LOG( SG_TERRAIN, SG_ALERT,
339 "WARNING: dubiously handled GL_QUAD_STRIP" );
340 case GL_TRIANGLE_STRIP :
341 /* SG_LOG( SG_TERRAIN, SG_ALERT,
342 "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
344 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
345 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
346 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
349 sgdCopyVec3( tri[0], tri[2] );
350 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
352 sgdCopyVec3( tri[1], tri[2] );
353 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
358 SG_LOG( SG_TERRAIN, SG_ALERT,
359 "WARNING: dubiously handled GL_QUADS" );
360 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
361 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
362 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
365 SG_LOG( SG_TERRAIN, SG_ALERT,
366 "WARNING: not-handled structure: " << primType );
367 return IntersectLeaf( leaf, m, orig, dir);
370 if( isZeroAreaTri( tri ) )
374 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
378 // find point of intersection of line from point org
379 // in direction dir with triangle's plane
380 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
381 /* Is line parallel to plane? */
382 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
385 // find parametric point
386 sgdScaleVec3 ( point, dir,
387 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
390 // short circut if this point is further away then a previous hit
391 tmp_dist = sgdDistanceSquaredVec3(point, orig );
392 if( tmp_dist > test_dist )
395 // place parametric point in world
396 sgdAddVec3 ( point, orig ) ;
398 if( fgdPointInTriangle( point, tri ) ) {
399 // transform point into passed coordinate frame
400 sgdXformPnt3( point, point, m );
401 sgdXformPnt4(plane,plane,m);
402 add(leaf,n,point,plane);
403 test_dist = tmp_dist;
412 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
413 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
417 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
418 sgdVec3 orig, sgdVec3 dir )
420 /* the lookat vector and matrix in branch's coordinate frame but
421 * we won't determine these unless needed, This 'lazy evaluation'
422 * is a result of profiling data */
423 sgdVec3 orig_leaf, dir_leaf;
426 // 'lazy evaluation' flag
429 for ( ssgEntity *kid = branch->getKid( 0 );
431 kid = branch->getNextKid() )
433 if ( kid->getTraversalMask() & SSGTRAV_HOT
434 && !kid->getBSphere()->isEmpty() )
438 kid->getBSphere()->getCenter()[0],
439 kid->getBSphere()->getCenter()[1],
440 kid->getBSphere()->getCenter()[2] );
441 sgdXformPnt3( center, m ) ;
443 // sgdClosestPointToLineDistSquared( center, orig, dir )
444 // inlined here because because of profiling results
446 sgdSubVec3(u, center, orig);
447 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
448 / sgdScalarProductVec3(dir,dir) );
449 sgdSubVec3(v, u, u1);
451 // double because of possible overflow
452 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
454 if ( kid->isAKindOf ( ssgTypeBranch() ) )
457 if ( kid->isA( ssgTypeTransform() ) )
460 ((ssgTransform *)kid)->getTransform( fxform );
461 sgMultMat4(m_new, m, fxform);
463 sgdCopyMat4(m_new, m);
465 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
467 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
470 sgdTransposeNegateMat4( m_leaf, m );
471 sgdXformPnt3( orig_leaf, orig, m_leaf );
472 sgdXformPnt3( dir_leaf, dir, m_leaf );
475 // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
476 // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
478 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
481 } // branch not requested to be traversed
486 // a temporary hack until we get everything rewritten with sgdVec3
487 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
489 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
493 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
496 sgdMakeIdentMat4 ( m ) ;
497 IntersectBranch( scene, m, orig, dir );
501 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
504 IntersectBranch( scene, m, orig, dir );
508 // Determine scenery altitude via ssg.
509 // returned results are in meters
510 static double hitlist1_time = 0.0;
512 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
513 sgdVec3 scenery_center,
515 double *terrain_elev, double *radius, double *normal)
517 SGTimeStamp start; start.stamp();
521 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
524 sgdCopyVec3(orig, view_pos );
525 sgdCopyVec3(dir, abs_view_pos );
527 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
532 double hit_elev = -9999;
533 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
535 int hitcount = hit_list->num_hits();
536 // cout << "hits = " << hitcount << endl;
537 for ( int i = 0; i < hitcount; ++i ) {
538 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
539 double lat_geod, alt, sea_level_r;
540 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
542 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
543 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
545 if ( alt > hit_elev && alt < max_alt_m ) {
546 // cout << " it's a keeper" << endl;
552 if ( hit_elev > -9000 ) {
553 *terrain_elev = hit_elev;
554 *radius = geoc.radius();
556 sgSetVec3(tmp, hit_list->get_normal(this_hit));
557 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
558 sgdSetVec3( normal, tmp );
559 // float *up = globals->get_current_view()->get_world_up();
560 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
561 /* ssgState *IntersectedLeafState =
562 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
565 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
567 float *up = globals->get_current_view()->get_world_up();
568 sgdSetVec3(normal, up[0], up[1], up[2]);
572 SGTimeStamp finish; finish.stamp();
573 hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
574 // cout << " time per call = " << hitlist1_time << endl;
580 static double hitlist2_time = 0.0;
582 // Determine scenery altitude via ssg.
583 // returned results are in meters
584 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
585 sgdVec3 scenery_center,
586 ssgTransform *terra_transform,
588 double *terrain_elev, double *radius, double *normal)
590 SGTimeStamp start; start.stamp();
594 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
597 sgdCopyVec3(orig, view_pos );
599 sgdCopyVec3(dir, abs_view_pos );
600 sgdNormalizeVec3(dir);
603 sgMakeIdentMat4 ( fxform ) ;
604 ssgGetEntityTransform( terra_transform, fxform );
607 sgdSetMat4(xform,fxform);
608 hit_list->Intersect( terra_transform, xform, orig, dir );
612 double hit_elev = -9999;
613 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
615 int hitcount = hit_list->num_hits();
616 // cout << "hits = " << hitcount << endl;
617 for ( int i = 0; i < hitcount; ++i ) {
618 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
619 double lat_geod, alt, sea_level_r;
620 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
622 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
623 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
625 if ( alt > hit_elev && alt < max_alt_m ) {
628 // cout << " it's a keeper" << endl;
632 if ( hit_elev > -9000 ) {
633 *terrain_elev = hit_elev;
634 *radius = geoc.radius();
636 sgSetVec3(tmp, hit_list->get_normal(this_hit));
637 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
638 sgdSetVec3( normal, tmp );
639 // float *up = globals->get_current_view()->get_world_up();
640 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
641 /* ssgState *IntersectedLeafState =
642 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
645 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
646 result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
647 hit_list, terrain_elev, radius, normal);
650 SGTimeStamp finish; finish.stamp();
651 hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
652 cout << "time per call 2 = " << hitlist2_time << endl;