2 // Height Over Terrain and Assosciated Routines for FlightGear based Scenery
3 // Written by Norman Vine, started 2000.
12 #include <simgear/sg_inlines.h>
13 #include <simgear/debug/logstream.hxx>
14 #include <simgear/math/point3d.hxx>
15 #include <simgear/math/sg_geodesy.hxx>
16 #include <simgear/math/vector.hxx>
18 #include <Main/globals.hxx>
19 #include <Main/viewer.hxx>
21 #include "hitlist.hxx"
23 extern ssgBranch *terrain_branch;
25 // forward declaration of our helper/convenience functions
26 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2);
27 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m );
28 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m );
31 // ======================
32 // This is same as PLib's sgdIsectInfLinePlane()
33 // and can be replaced by it after the next PLib release
34 static int fgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
35 const sgdVec3 l_vec, const sgdVec4 plane )
37 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
39 /* Is line parallel to plane? */
41 if ( fabs ( tmp ) < FLT_EPSILON )
44 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
45 + plane[3] ) / tmp ) ;
46 sgdAddVec3 ( dst, l_org ) ;
51 // ======================
54 * Given a point and a triangle lying on the same plane
55 * check to see if the point is inside the triangle
57 // This is same as PLib's sgdPointInTriangle()
58 // and can be replaced by it after the next PLib release
59 static bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
64 // punt if outside bouding cube
65 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
66 if( (point[0] < min) || (point[0] > max) )
70 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
71 if( (point[1] < min) || (point[1] > max) )
75 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
76 if( (point[2] < min) || (point[2] > max) )
80 // drop the smallest dimension so we only have to work in 2d.
81 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
82 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
83 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
84 // x is the smallest dimension
93 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
94 // y is the smallest dimension
103 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
104 // z is the smallest dimension
114 // all dimensions are really small so lets call it close
115 // enough and return a successful match
119 // check if intersection point is on the same side of p1 <-> p2 as p3
120 SGDfloat tmp = (y2 - y3) / (x2 - x3);
121 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
122 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
123 if ( side1 != side2 ) {
124 // printf("failed side 1 check\n");
128 // check if intersection point is on correct side of p2 <-> p3 as p1
129 tmp = (y3 - ry) / (x3 - rx);
130 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
131 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
132 if ( side1 != side2 ) {
133 // printf("failed side 2 check\n");
137 // check if intersection point is on correct side of p1 <-> p3 as p2
138 tmp = (y2 - ry) / (x2 - rx);
139 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
140 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
141 if ( side1 != side2 ) {
142 // printf("failed side 3 check\n");
149 // ======================
151 inline static int isZeroAreaTri( sgdVec3 tri[3] )
153 return( sgdEqualVec3(tri[0], tri[1]) ||
154 sgdEqualVec3(tri[1], tri[2]) ||
155 sgdEqualVec3(tri[2], tri[0]) );
158 FGHitList::FGHitList() :
159 last(NULL), test_dist(DBL_MAX)
163 FGHitList::~FGHitList() {}
167 Find the intersection of an infinite line with a leaf
168 the line being defined by a point and direction.
172 ssgLeaf pointer -- leaf
173 qualified matrix -- m
175 line direction -- dir
177 result -- intersection point
178 normal -- intersected tri's normal
181 true if intersection found
185 If you need an exhaustive list of hitpoints YOU MUST use
186 the generic version of this function as the specialized
187 versions will do an early out of expensive tests if the point
188 can not be the closest one found
191 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
192 sgdVec3 orig, sgdVec3 dir )
197 for ( ; i < leaf->getNumTriangles(); ++i ) {
199 leaf->getTriangle( i, &i1, &i2, &i3 );
202 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
203 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
204 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
206 if( isZeroAreaTri( tri ) )
210 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
213 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
214 if( fgdPointInTriangle( point, tri ) ) {
215 // transform point into passed into desired coordinate frame
216 sgdXformPnt3( point, point, m );
217 add(leaf,i,point,plane);
225 // ======================
227 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
228 sgdVec3 orig, sgdVec3 dir,
233 // number of hits but there could be more that
234 // were not found because of short circut switch !
235 // so you may want to use the unspecialized IntersectLeaf()
238 int ntri = leaf->getNumTriangles();
239 for ( n = 0; n < ntri; ++n )
246 case GL_TRIANGLE_FAN :
248 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
249 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
250 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
252 sgdCopyVec3( tri[1], tri[2] );
253 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
257 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
258 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
259 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
261 case GL_TRIANGLE_STRIP :
264 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
265 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
266 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
269 sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
270 sgdCopyVec3( tri[1], tri[2] );
271 sgdCopyVec3( tri[2], tri[1] );
274 sgdCopyVec3( tri[0], tri[1] );
275 sgdCopyVec3( tri[1], tri[2] );
276 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
281 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
282 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
283 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
286 return IntersectLeaf( leaf, m, orig, dir);
289 if( isZeroAreaTri( tri ) )
293 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
297 // find point of intersection of line from point org
298 // in direction dir with triangle's plane
299 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
300 /* Is line parallel to plane? */
301 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
304 // find parametric point
305 sgdScaleVec3 ( point, dir,
306 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
309 // short circut if this point is further away then a previous hit
310 tmp_dist = sgdDistanceSquaredVec3(point, orig );
311 if( tmp_dist > test_dist )
314 // place parametric point in world
315 sgdAddVec3 ( point, orig ) ;
317 if( fgdPointInTriangle( point, tri ) ) {
318 // transform point into passed coordinate frame
319 sgdXformPnt3( point, point, m );
320 add(leaf,n,point,plane);
321 test_dist = tmp_dist;
328 // ======================
329 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
330 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
333 // ======================
334 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
335 sgdVec3 orig, sgdVec3 dir )
337 /* the lookat vector and matrix in branch's coordinate frame
338 * but we won't determine these unless needed,
339 * This 'lazy evaluation' is a result of profiling data */
340 sgdVec3 orig_leaf, dir_leaf;
343 // 'lazy evaluation' flag
346 for ( ssgEntity *kid = branch->getKid( 0 );
348 kid = branch->getNextKid() )
350 if ( kid->getTraversalMask() & SSGTRAV_HOT )
354 kid->getBSphere()->getCenter()[0],
355 kid->getBSphere()->getCenter()[1],
356 kid->getBSphere()->getCenter()[2] );
357 sgdXformPnt3( center, m ) ;
359 // sgdClosestPointToLineDistSquared( center, orig, dir )
360 // inlined here because because of profiling results
362 sgdSubVec3(u, center, orig);
363 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
364 / sgdScalarProductVec3(dir,dir) );
365 sgdSubVec3(v, u, u1);
367 // double because of possible overflow
368 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
370 if ( kid->isAKindOf ( ssgTypeBranch() ) )
373 if ( kid->isA( ssgTypeTransform() ) )
376 ((ssgTransform *)kid)->getTransform( fxform );
377 sgMultMat4(m_new, m, fxform);
379 sgdCopyMat4(m_new, m);
381 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
383 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
386 sgdTransposeNegateMat4( m_leaf, m );
387 sgdXformPnt3( orig_leaf, orig, m_leaf );
388 sgdXformPnt3( dir_leaf, dir, m_leaf );
391 GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
392 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
395 } // branch not requested to be traversed
401 // ======================
402 // a temporary hack until we get everything rewritten with sgdVec3
403 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
405 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
409 // ======================
410 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
413 sgdMakeIdentMat4 ( m ) ;
414 IntersectBranch( scene, m, orig, dir );
417 // ======================
418 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
421 IntersectBranch( scene, m, orig, dir );
424 // ======================
425 // Need these because of mixed matrix types
426 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
428 for ( int j = 0 ; j < 4 ; j++ )
430 dst[0][j] = m2[0][0] * m1[0][j] +
431 m2[0][1] * m1[1][j] +
432 m2[0][2] * m1[2][j] +
433 m2[0][3] * m1[3][j] ;
435 dst[1][j] = m2[1][0] * m1[0][j] +
436 m2[1][1] * m1[1][j] +
437 m2[1][2] * m1[2][j] +
438 m2[1][3] * m1[3][j] ;
440 dst[2][j] = m2[2][0] * m1[0][j] +
441 m2[2][1] * m1[1][j] +
442 m2[2][2] * m1[2][j] +
443 m2[2][3] * m1[3][j] ;
445 dst[3][j] = m2[3][0] * m1[0][j] +
446 m2[3][1] * m1[1][j] +
447 m2[3][2] * m1[2][j] +
448 m2[3][3] * m1[3][j] ;
452 // ======================
453 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
456 Walk backwards up the tree, transforming the
457 vertex by all the matrices along the way.
459 Upwards recursion hurts my head.
465 If this node has a parent - get the composite
466 matrix for the parent.
468 if ( entity->getNumParents() > 0 )
469 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
471 sgMakeIdentMat4 ( mat ) ;
474 If this node has a transform - then concatenate it.
476 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
478 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
479 sgPostMultMat4 ( mat, this_mat ) ;
482 sgCopyMat4 ( m, mat ) ;
485 // ======================
486 // return the passed entitity's bsphere's center point radius and
487 // fully formed current model matrix for entity
488 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
490 sgSphere *bsphere = entity->getBSphere();
491 *radius = (double)bsphere->getRadius();
492 sgCopyVec3( center, bsphere->getCenter() );
493 sgMakeIdentMat4 ( m ) ;
494 ssgGetEntityTransform( entity, m );
498 // ======================
499 // Determine scenery altitude via ssg.
500 // returned results are in meters
501 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
503 double *terrain_elev, double *radius, double *normal)
506 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
509 sgdCopyVec3(orig, view_pos );
510 sgdCopyVec3(dir, abs_view_pos );
512 // !! why is terrain not globals->get_terrain()
513 hit_list->Intersect( terrain_branch, orig, dir );
517 double result = -9999;
518 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
520 // cout << "hits = ";
521 int hitcount = hit_list->num_hits();
522 for ( int i = 0; i < hitcount; ++i ) {
523 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
524 double lat_geod, alt, sea_level_r;
525 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
527 // cout << alt << " ";
528 if ( alt > result && alt < 10000 ) {
535 if ( result > -9000 ) {
536 *terrain_elev = result;
537 *radius = geoc.radius();
540 sgSetVec3(tmp, hit_list->get_normal(this_hit));
541 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
542 // << tmp[2] << endl;
543 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
544 sgXformVec3(tmp, tmp, TMP);
545 // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
546 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
547 /* ssgState *IntersectedLeafState =
548 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
551 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
553 float *up = globals->get_current_view()->get_world_up();
554 sgdSetVec3(normal, up[0], up[1], up[2]);
560 // ======================
561 // Determine scenery altitude via ssg.
562 // returned results are in meters
563 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
564 ssgTransform *terra_transform,
566 double *terrain_elev, double *radius, double *normal)
569 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
572 sgdCopyVec3(orig, view_pos );
574 sgdCopyVec3(dir, abs_view_pos );
575 sgdNormalizeVec3(dir);
578 sgMakeIdentMat4 ( fxform ) ;
579 ssgGetEntityTransform( terra_transform, fxform );
582 sgdSetMat4(xform,fxform);
583 hit_list->Intersect( terra_transform, xform, orig, dir );
587 double result = -9999;
588 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
590 int hitcount = hit_list->num_hits();
591 for ( int i = 0; i < hitcount; ++i ) {
592 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
593 double lat_geod, alt, sea_level_r;
594 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
596 if ( alt > result && alt < 20000 ) {
602 if ( result > -9000 ) {
603 *terrain_elev = result;
604 *radius = geoc.radius();
607 sgSetVec3(tmp, hit_list->get_normal(this_hit));
608 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
609 sgXformVec3(tmp, tmp, TMP);
610 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
611 /* ssgState *IntersectedLeafState =
612 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
615 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
616 return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
617 terrain_elev,radius,normal);