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>
21 #include <Main/globals.hxx>
22 #include <Main/viewer.hxx>
23 #include <Scenery/scenery.hxx>
25 #include "hitlist.hxx"
27 // Specialized version of sgMultMat4 needed because of mixed matrix
29 static inline void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
30 for ( int j = 0 ; j < 4 ; j++ ) {
31 dst[0][j] = m2[0][0] * m1[0][j] +
36 dst[1][j] = m2[1][0] * m1[0][j] +
41 dst[2][j] = m2[2][0] * m1[0][j] +
46 dst[3][j] = m2[3][0] * m1[0][j] +
55 * Walk backwards up the tree, transforming the vertex by all the
56 * matrices along the way.
58 * Upwards recursion hurts my head.
60 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m ) {
63 // If this node has a parent - get the composite matrix for the
65 if ( entity->getNumParents() > 0 )
66 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
68 sgMakeIdentMat4 ( mat ) ;
70 // If this node has a transform - then concatenate it.
71 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
73 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
74 sgPostMultMat4 ( mat, this_mat ) ;
77 sgCopyMat4 ( m, mat ) ;
81 // return the passed entitity's bsphere's center point radius and
82 // fully formed current model matrix for entity
83 static inline void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
84 float *radius, sgMat4 m )
86 sgSphere *bsphere = entity->getBSphere();
87 *radius = (double)bsphere->getRadius();
88 sgCopyVec3( center, bsphere->getCenter() );
89 sgMakeIdentMat4 ( m ) ;
90 ssgGetEntityTransform( entity, m );
94 // This is same as PLib's sgdIsectInfLinePlane() and can be replaced
95 // by it after the next PLib release
96 static inline bool fgdIsectInfLinePlane( sgdVec3 dst,
101 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
103 /* Is line parallel to plane? */
105 if ( fabs ( tmp ) < FLT_EPSILON )
108 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
109 + plane[3] ) / tmp ) ;
110 sgdAddVec3 ( dst, l_org ) ;
116 * Given a point and a triangle lying on the same plane check to see
117 * if the point is inside the triangle
119 * This is same as PLib's sgdPointInTriangle() and can be replaced by
120 * it after the next PLib release
122 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
127 // punt if outside bouding cube
128 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
129 if( (point[0] < min) || (point[0] > max) )
133 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
134 if( (point[1] < min) || (point[1] > max) )
138 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
139 if( (point[2] < min) || (point[2] > max) )
143 // drop the smallest dimension so we only have to work in 2d.
144 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
145 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
146 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
147 // x is the smallest dimension
156 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
157 // y is the smallest dimension
166 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
167 // z is the smallest dimension
177 // all dimensions are really small so lets call it close
178 // enough and return a successful match
182 // check if intersection point is on the same side of p1 <-> p2 as p3
183 SGDfloat tmp = (y2 - y3) / (x2 - x3);
184 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
185 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
186 if ( side1 != side2 ) {
187 // printf("failed side 1 check\n");
191 // check if intersection point is on correct side of p2 <-> p3 as p1
192 tmp = (y3 - ry) / (x3 - rx);
193 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
194 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
195 if ( side1 != side2 ) {
196 // printf("failed side 2 check\n");
200 // check if intersection point is on correct side of p1 <-> p3 as p2
201 tmp = (y2 - ry) / (x2 - rx);
202 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
203 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
204 if ( side1 != side2 ) {
205 // printf("failed side 3 check\n");
213 // Check if all three vertices are the same point (or close enough)
214 static inline int isZeroAreaTri( sgdVec3 tri[3] )
216 return( sgdEqualVec3(tri[0], tri[1]) ||
217 sgdEqualVec3(tri[1], tri[2]) ||
218 sgdEqualVec3(tri[2], tri[0]) );
223 FGHitList::FGHitList() :
224 last(NULL), test_dist(DBL_MAX)
230 FGHitList::~FGHitList() {}
234 Find the intersection of an infinite line with a leaf the line being
235 defined by a point and direction.
239 ssgLeaf pointer -- leaf
240 qualified matrix -- m
242 line direction -- dir
244 result -- intersection point
245 normal -- intersected tri's normal
248 true if intersection found
253 If you need an exhaustive list of hitpoints YOU MUST use the generic
254 version of this function as the specialized versions will do an early
255 out of expensive tests if the point can not be the closest one found
259 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
260 sgdVec3 orig, sgdVec3 dir )
265 for ( ; i < leaf->getNumTriangles(); ++i ) {
267 leaf->getTriangle( i, &i1, &i2, &i3 );
270 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
271 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
272 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
274 if( isZeroAreaTri( tri ) )
278 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
281 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
282 if( fgdPointInTriangle( point, tri ) ) {
283 // transform point into passed into desired coordinate frame
284 sgdXformPnt3( point, point, m );
285 sgdXformPnt4(plane,plane,m);
286 add(leaf,i,point,plane);
295 // Short circuit/slightly optimized version of the full IntersectLeaf()
296 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
297 sgdVec3 orig, sgdVec3 dir,
302 // number of hits but there could be more that
303 // were not found because of short circut switch !
304 // so you may want to use the unspecialized IntersectLeaf()
307 int ntri = leaf->getNumTriangles();
308 for ( n = 0; n < ntri; ++n )
315 SG_LOG( SG_TERRAIN, SG_ALERT,
316 "WARNING: dubiously handled GL_POLYGON" );
317 case GL_TRIANGLE_FAN :
318 /* SG_LOG( SG_TERRAIN, SG_ALERT,
319 "IntersectLeaf: GL_TRIANGLE_FAN" ); */
321 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
322 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
323 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
325 sgdCopyVec3( tri[1], tri[2] );
326 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
330 /* SG_LOG( SG_TERRAIN, SG_DEBUG,
331 "IntersectLeaf: GL_TRIANGLES" ); */
332 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
333 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
334 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
337 SG_LOG( SG_TERRAIN, SG_ALERT,
338 "WARNING: dubiously handled GL_QUAD_STRIP" );
339 case GL_TRIANGLE_STRIP :
340 /* SG_LOG( SG_TERRAIN, SG_ALERT,
341 "IntersectLeaf: GL_TRIANGLE_STRIP" ); */
343 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
344 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
345 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
348 sgdCopyVec3( tri[0], tri[2] );
349 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
351 sgdCopyVec3( tri[1], tri[2] );
352 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
357 SG_LOG( SG_TERRAIN, SG_ALERT,
358 "WARNING: dubiously handled GL_QUADS" );
359 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
360 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
361 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
364 SG_LOG( SG_TERRAIN, SG_ALERT,
365 "WARNING: not-handled structure: " << primType );
366 return IntersectLeaf( leaf, m, orig, dir);
369 if( isZeroAreaTri( tri ) )
373 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
377 // find point of intersection of line from point org
378 // in direction dir with triangle's plane
379 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
380 /* Is line parallel to plane? */
381 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
384 // find parametric point
385 sgdScaleVec3 ( point, dir,
386 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
389 // short circut if this point is further away then a previous hit
390 tmp_dist = sgdDistanceSquaredVec3(point, orig );
391 if( tmp_dist > test_dist )
394 // place parametric point in world
395 sgdAddVec3 ( point, orig ) ;
397 if( fgdPointInTriangle( point, tri ) ) {
398 // transform point into passed coordinate frame
399 sgdXformPnt3( point, point, m );
400 sgdXformPnt4(plane,plane,m);
401 add(leaf,n,point,plane);
402 test_dist = tmp_dist;
411 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
412 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
416 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
417 sgdVec3 orig, sgdVec3 dir )
419 /* the lookat vector and matrix in branch's coordinate frame but
420 * we won't determine these unless needed, This 'lazy evaluation'
421 * is a result of profiling data */
422 sgdVec3 orig_leaf, dir_leaf;
425 // 'lazy evaluation' flag
428 for ( ssgEntity *kid = branch->getKid( 0 );
430 kid = branch->getNextKid() )
432 if ( kid->getTraversalMask() & SSGTRAV_HOT
433 && !kid->getBSphere()->isEmpty() )
437 kid->getBSphere()->getCenter()[0],
438 kid->getBSphere()->getCenter()[1],
439 kid->getBSphere()->getCenter()[2] );
440 sgdXformPnt3( center, m ) ;
442 // sgdClosestPointToLineDistSquared( center, orig, dir )
443 // inlined here because because of profiling results
445 sgdSubVec3(u, center, orig);
446 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
447 / sgdScalarProductVec3(dir,dir) );
448 sgdSubVec3(v, u, u1);
450 // double because of possible overflow
451 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
453 if ( kid->isAKindOf ( ssgTypeBranch() ) )
456 if ( kid->isA( ssgTypeTransform() ) )
459 ((ssgTransform *)kid)->getTransform( fxform );
460 sgMultMat4(m_new, m, fxform);
462 sgdCopyMat4(m_new, m);
464 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
466 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
469 sgdTransposeNegateMat4( m_leaf, m );
470 sgdXformPnt3( orig_leaf, orig, m_leaf );
471 sgdXformPnt3( dir_leaf, dir, m_leaf );
474 // GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
475 // IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf,
477 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf );
480 } // branch not requested to be traversed
485 // a temporary hack until we get everything rewritten with sgdVec3
486 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
488 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
492 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
495 sgdMakeIdentMat4 ( m ) ;
496 IntersectBranch( scene, m, orig, dir );
500 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
503 IntersectBranch( scene, m, orig, dir );
507 // Determine scenery altitude via ssg.
508 // returned results are in meters
509 static double hitlist1_time = 0.0;
511 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
512 sgdVec3 scenery_center,
514 double *terrain_elev, double *radius, double *normal)
516 SGTimeStamp start; start.stamp();
520 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
523 sgdCopyVec3(orig, view_pos );
524 sgdCopyVec3(dir, abs_view_pos );
526 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
531 double hit_elev = -9999;
532 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
534 int hitcount = hit_list->num_hits();
535 // cout << "hits = " << hitcount << endl;
536 for ( int i = 0; i < hitcount; ++i ) {
537 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
538 double lat_geod, alt, sea_level_r;
539 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
541 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
542 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
544 if ( alt > hit_elev && alt < max_alt_m ) {
545 // cout << " it's a keeper" << endl;
551 if ( hit_elev > -9000 ) {
552 *terrain_elev = hit_elev;
553 *radius = geoc.radius();
555 sgSetVec3(tmp, hit_list->get_normal(this_hit));
556 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
557 sgdSetVec3( normal, tmp );
558 // float *up = globals->get_current_view()->get_world_up();
559 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
560 /* ssgState *IntersectedLeafState =
561 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
564 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
566 float *up = globals->get_current_view()->get_world_up();
567 sgdSetVec3(normal, up[0], up[1], up[2]);
571 SGTimeStamp finish; finish.stamp();
572 hitlist1_time = ( 29.0 * hitlist1_time + (finish - start) ) / 30.0;
573 // cout << " time per call = " << hitlist1_time << endl;
579 static double hitlist2_time = 0.0;
581 // Determine scenery altitude via ssg.
582 // returned results are in meters
583 bool fgCurrentElev( sgdVec3 abs_view_pos, double max_alt_m,
584 sgdVec3 scenery_center,
585 ssgTransform *terra_transform,
587 double *terrain_elev, double *radius, double *normal)
589 SGTimeStamp start; start.stamp();
593 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
596 sgdCopyVec3(orig, view_pos );
598 sgdCopyVec3(dir, abs_view_pos );
599 sgdNormalizeVec3(dir);
602 sgMakeIdentMat4 ( fxform ) ;
603 ssgGetEntityTransform( terra_transform, fxform );
606 sgdSetMat4(xform,fxform);
607 hit_list->Intersect( terra_transform, xform, orig, dir );
611 double hit_elev = -9999;
612 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
614 int hitcount = hit_list->num_hits();
615 // cout << "hits = " << hitcount << endl;
616 for ( int i = 0; i < hitcount; ++i ) {
617 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
618 double lat_geod, alt, sea_level_r;
619 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
621 // cout << "hit " << i << " lon = " << geoc.lon() << " lat = "
622 // << lat_geod << " alt = " << alt << " max alt = " << max_alt_m
624 if ( alt > hit_elev && alt < max_alt_m ) {
627 // cout << " it's a keeper" << endl;
631 if ( hit_elev > -9000 ) {
632 *terrain_elev = hit_elev;
633 *radius = geoc.radius();
635 sgSetVec3(tmp, hit_list->get_normal(this_hit));
636 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
637 sgdSetVec3( normal, tmp );
638 // float *up = globals->get_current_view()->get_world_up();
639 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
640 /* ssgState *IntersectedLeafState =
641 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
644 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
645 result = fgCurrentElev( abs_view_pos, max_alt_m, scenery_center,
646 hit_list, terrain_elev, radius, normal);
649 SGTimeStamp finish; finish.stamp();
650 hitlist2_time = ( 29.0 * hitlist2_time + (finish - start) ) / 30.0;
651 cout << "time per call 2 = " << hitlist2_time << endl;