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 // forward declaration of our helper/convenience functions
24 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2);
25 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m );
26 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m );
29 // ======================
30 // This is same as PLib's sgdIsectInfLinePlane()
31 // and can be replaced by it after the next PLib release
32 static int fgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
33 const sgdVec3 l_vec, const sgdVec4 plane )
35 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
37 /* Is line parallel to plane? */
39 if ( fabs ( tmp ) < FLT_EPSILON )
42 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
43 + plane[3] ) / tmp ) ;
44 sgdAddVec3 ( dst, l_org ) ;
49 // ======================
52 * Given a point and a triangle lying on the same plane
53 * check to see if the point is inside the triangle
55 // This is same as PLib's sgdPointInTriangle()
56 // and can be replaced by it after the next PLib release
57 static bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
62 // punt if outside bouding cube
63 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
64 if( (point[0] < min) || (point[0] > max) )
68 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
69 if( (point[1] < min) || (point[1] > max) )
73 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
74 if( (point[2] < min) || (point[2] > max) )
78 // drop the smallest dimension so we only have to work in 2d.
79 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
80 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
81 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
82 // x is the smallest dimension
91 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
92 // y is the smallest dimension
101 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
102 // z is the smallest dimension
112 // all dimensions are really small so lets call it close
113 // enough and return a successful match
117 // check if intersection point is on the same side of p1 <-> p2 as p3
118 SGDfloat tmp = (y2 - y3) / (x2 - x3);
119 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
120 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
121 if ( side1 != side2 ) {
122 // printf("failed side 1 check\n");
126 // check if intersection point is on correct side of p2 <-> p3 as p1
127 tmp = (y3 - ry) / (x3 - rx);
128 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
129 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
130 if ( side1 != side2 ) {
131 // printf("failed side 2 check\n");
135 // check if intersection point is on correct side of p1 <-> p3 as p2
136 tmp = (y2 - ry) / (x2 - rx);
137 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
138 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
139 if ( side1 != side2 ) {
140 // printf("failed side 3 check\n");
147 // ======================
149 inline static int isZeroAreaTri( sgdVec3 tri[3] )
151 return( sgdEqualVec3(tri[0], tri[1]) ||
152 sgdEqualVec3(tri[1], tri[2]) ||
153 sgdEqualVec3(tri[2], tri[0]) );
156 FGHitList::FGHitList() :
157 last(NULL), test_dist(DBL_MAX)
161 FGHitList::~FGHitList() {}
165 Find the intersection of an infinite line with a leaf
166 the line being defined by a point and direction.
170 ssgLeaf pointer -- leaf
171 qualified matrix -- m
173 line direction -- dir
175 result -- intersection point
176 normal -- intersected tri's normal
179 true if intersection found
183 If you need an exhaustive list of hitpoints YOU MUST use
184 the generic version of this function as the specialized
185 versions will do an early out of expensive tests if the point
186 can not be the closest one found
189 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
190 sgdVec3 orig, sgdVec3 dir )
195 for ( ; i < leaf->getNumTriangles(); ++i ) {
197 leaf->getTriangle( i, &i1, &i2, &i3 );
200 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
201 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
202 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
204 if( isZeroAreaTri( tri ) )
208 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
211 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
212 if( fgdPointInTriangle( point, tri ) ) {
213 // transform point into passed into desired coordinate frame
214 sgdXformPnt3( point, point, m );
215 sgdXformPnt4(plane,plane,m);
216 add(leaf,i,point,plane);
224 // ======================
226 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
227 sgdVec3 orig, sgdVec3 dir,
232 // number of hits but there could be more that
233 // were not found because of short circut switch !
234 // so you may want to use the unspecialized IntersectLeaf()
237 int ntri = leaf->getNumTriangles();
238 for ( n = 0; n < ntri; ++n )
245 case GL_TRIANGLE_FAN :
247 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
248 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
249 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
251 sgdCopyVec3( tri[1], tri[2] );
252 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
256 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
257 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
258 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
260 case GL_TRIANGLE_STRIP :
263 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
264 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
265 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
268 sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
269 sgdCopyVec3( tri[1], tri[2] );
270 sgdCopyVec3( tri[2], tri[1] );
273 sgdCopyVec3( tri[0], tri[1] );
274 sgdCopyVec3( tri[1], tri[2] );
275 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
280 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
281 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
282 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
285 return IntersectLeaf( leaf, m, orig, dir);
288 if( isZeroAreaTri( tri ) )
292 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
296 // find point of intersection of line from point org
297 // in direction dir with triangle's plane
298 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
299 /* Is line parallel to plane? */
300 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
303 // find parametric point
304 sgdScaleVec3 ( point, dir,
305 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
308 // short circut if this point is further away then a previous hit
309 tmp_dist = sgdDistanceSquaredVec3(point, orig );
310 if( tmp_dist > test_dist )
313 // place parametric point in world
314 sgdAddVec3 ( point, orig ) ;
316 if( fgdPointInTriangle( point, tri ) ) {
317 // transform point into passed coordinate frame
318 sgdXformPnt3( point, point, m );
319 sgdXformPnt4(plane,plane,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 hit_list->Intersect( globals->get_terrain_branch(), orig, dir );
516 double result = -9999;
517 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
519 // cout << "hits = ";
520 int hitcount = hit_list->num_hits();
521 for ( int i = 0; i < hitcount; ++i ) {
522 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
523 double lat_geod, alt, sea_level_r;
524 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
526 // cout << alt << " ";
527 if ( alt > result && alt < 10000 ) {
534 if ( result > -9000 ) {
535 *terrain_elev = result;
536 *radius = geoc.radius();
539 sgSetVec3(tmp, hit_list->get_normal(this_hit));
540 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
541 sgdSetVec3( normal, tmp );
542 // float *up = globals->get_current_view()->get_world_up();
543 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
544 /* ssgState *IntersectedLeafState =
545 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
548 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
550 float *up = globals->get_current_view()->get_world_up();
551 sgdSetVec3(normal, up[0], up[1], up[2]);
557 // ======================
558 // Determine scenery altitude via ssg.
559 // returned results are in meters
560 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
561 ssgTransform *terra_transform,
563 double *terrain_elev, double *radius, double *normal)
566 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
569 sgdCopyVec3(orig, view_pos );
571 sgdCopyVec3(dir, abs_view_pos );
572 sgdNormalizeVec3(dir);
575 sgMakeIdentMat4 ( fxform ) ;
576 ssgGetEntityTransform( terra_transform, fxform );
579 sgdSetMat4(xform,fxform);
580 hit_list->Intersect( terra_transform, xform, orig, dir );
584 double result = -9999;
585 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
587 int hitcount = hit_list->num_hits();
588 for ( int i = 0; i < hitcount; ++i ) {
589 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
590 double lat_geod, alt, sea_level_r;
591 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
593 if ( alt > result && alt < 20000 ) {
599 if ( result > -9000 ) {
600 *terrain_elev = result;
601 *radius = geoc.radius();
604 sgSetVec3(tmp, hit_list->get_normal(this_hit));
605 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
606 sgdSetVec3( normal, tmp );
607 // float *up = globals->get_current_view()->get_world_up();
608 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
609 /* ssgState *IntersectedLeafState =
610 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
613 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
614 return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
615 terrain_elev,radius,normal);