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 add(leaf,i,point,plane);
223 // ======================
225 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
226 sgdVec3 orig, sgdVec3 dir,
231 // number of hits but there could be more that
232 // were not found because of short circut switch !
233 // so you may want to use the unspecialized IntersectLeaf()
236 int ntri = leaf->getNumTriangles();
237 for ( n = 0; n < ntri; ++n )
244 case GL_TRIANGLE_FAN :
246 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
247 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
248 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
250 sgdCopyVec3( tri[1], tri[2] );
251 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
255 sgdSetVec3( tri[0], leaf->getVertex( short(n*3) ) );
256 sgdSetVec3( tri[1], leaf->getVertex( short(n*3+1) ) );
257 sgdSetVec3( tri[2], leaf->getVertex( short(n*3+2) ) );
259 case GL_TRIANGLE_STRIP :
262 sgdSetVec3( tri[0], leaf->getVertex( short(0) ) );
263 sgdSetVec3( tri[1], leaf->getVertex( short(1) ) );
264 sgdSetVec3( tri[2], leaf->getVertex( short(2) ) );
267 sgdSetVec3( tri[0], leaf->getVertex( short(n+2) ) );
268 sgdCopyVec3( tri[1], tri[2] );
269 sgdCopyVec3( tri[2], tri[1] );
272 sgdCopyVec3( tri[0], tri[1] );
273 sgdCopyVec3( tri[1], tri[2] );
274 sgdSetVec3( tri[2], leaf->getVertex( short(n+2) ) );
279 sgdSetVec3( tri[0], leaf->getVertex( short(n*2) ) );
280 sgdSetVec3( tri[1], leaf->getVertex( short(n*2+1) ) );
281 sgdSetVec3( tri[2], leaf->getVertex( short(n*2 + 2 - (n&1)*4) ) );
284 return IntersectLeaf( leaf, m, orig, dir);
287 if( isZeroAreaTri( tri ) )
291 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
295 // find point of intersection of line from point org
296 // in direction dir with triangle's plane
297 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
298 /* Is line parallel to plane? */
299 if ( sgdAbs ( tmp ) < FLT_EPSILON /*DBL_EPSILON*/ )
302 // find parametric point
303 sgdScaleVec3 ( point, dir,
304 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
307 // short circut if this point is further away then a previous hit
308 tmp_dist = sgdDistanceSquaredVec3(point, orig );
309 if( tmp_dist > test_dist )
312 // place parametric point in world
313 sgdAddVec3 ( point, orig ) ;
315 if( fgdPointInTriangle( point, tri ) ) {
316 // transform point into passed coordinate frame
317 sgdXformPnt3( point, point, m );
318 add(leaf,n,point,plane);
319 test_dist = tmp_dist;
326 // ======================
327 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
328 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
331 // ======================
332 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
333 sgdVec3 orig, sgdVec3 dir )
335 /* the lookat vector and matrix in branch's coordinate frame
336 * but we won't determine these unless needed,
337 * This 'lazy evaluation' is a result of profiling data */
338 sgdVec3 orig_leaf, dir_leaf;
341 // 'lazy evaluation' flag
344 for ( ssgEntity *kid = branch->getKid( 0 );
346 kid = branch->getNextKid() )
348 if ( kid->getTraversalMask() & SSGTRAV_HOT )
352 kid->getBSphere()->getCenter()[0],
353 kid->getBSphere()->getCenter()[1],
354 kid->getBSphere()->getCenter()[2] );
355 sgdXformPnt3( center, m ) ;
357 // sgdClosestPointToLineDistSquared( center, orig, dir )
358 // inlined here because because of profiling results
360 sgdSubVec3(u, center, orig);
361 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
362 / sgdScalarProductVec3(dir,dir) );
363 sgdSubVec3(v, u, u1);
365 // double because of possible overflow
366 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
368 if ( kid->isAKindOf ( ssgTypeBranch() ) )
371 if ( kid->isA( ssgTypeTransform() ) )
374 ((ssgTransform *)kid)->getTransform( fxform );
375 sgMultMat4(m_new, m, fxform);
377 sgdCopyMat4(m_new, m);
379 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
381 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
384 sgdTransposeNegateMat4( m_leaf, m );
385 sgdXformPnt3( orig_leaf, orig, m_leaf );
386 sgdXformPnt3( dir_leaf, dir, m_leaf );
389 GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
390 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
393 } // branch not requested to be traversed
399 // ======================
400 // a temporary hack until we get everything rewritten with sgdVec3
401 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
403 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
407 // ======================
408 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
411 sgdMakeIdentMat4 ( m ) ;
412 IntersectBranch( scene, m, orig, dir );
415 // ======================
416 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
419 IntersectBranch( scene, m, orig, dir );
422 // ======================
423 // Need these because of mixed matrix types
424 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
426 for ( int j = 0 ; j < 4 ; j++ )
428 dst[0][j] = m2[0][0] * m1[0][j] +
429 m2[0][1] * m1[1][j] +
430 m2[0][2] * m1[2][j] +
431 m2[0][3] * m1[3][j] ;
433 dst[1][j] = m2[1][0] * m1[0][j] +
434 m2[1][1] * m1[1][j] +
435 m2[1][2] * m1[2][j] +
436 m2[1][3] * m1[3][j] ;
438 dst[2][j] = m2[2][0] * m1[0][j] +
439 m2[2][1] * m1[1][j] +
440 m2[2][2] * m1[2][j] +
441 m2[2][3] * m1[3][j] ;
443 dst[3][j] = m2[3][0] * m1[0][j] +
444 m2[3][1] * m1[1][j] +
445 m2[3][2] * m1[2][j] +
446 m2[3][3] * m1[3][j] ;
450 // ======================
451 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
454 Walk backwards up the tree, transforming the
455 vertex by all the matrices along the way.
457 Upwards recursion hurts my head.
463 If this node has a parent - get the composite
464 matrix for the parent.
466 if ( entity->getNumParents() > 0 )
467 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
469 sgMakeIdentMat4 ( mat ) ;
472 If this node has a transform - then concatenate it.
474 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
476 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
477 sgPostMultMat4 ( mat, this_mat ) ;
480 sgCopyMat4 ( m, mat ) ;
483 // ======================
484 // return the passed entitity's bsphere's center point radius and
485 // fully formed current model matrix for entity
486 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
488 sgSphere *bsphere = entity->getBSphere();
489 *radius = (double)bsphere->getRadius();
490 sgCopyVec3( center, bsphere->getCenter() );
491 sgMakeIdentMat4 ( m ) ;
492 ssgGetEntityTransform( entity, m );
496 // ======================
497 // Determine scenery altitude via ssg.
498 // returned results are in meters
499 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
501 double *terrain_elev, double *radius, double *normal)
504 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
507 sgdCopyVec3(orig, view_pos );
508 sgdCopyVec3(dir, abs_view_pos );
510 hit_list->Intersect( globals->get_terrain_branch(), orig, dir );
514 double result = -9999;
515 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
517 // cout << "hits = ";
518 int hitcount = hit_list->num_hits();
519 for ( int i = 0; i < hitcount; ++i ) {
520 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
521 double lat_geod, alt, sea_level_r;
522 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
524 // cout << alt << " ";
525 if ( alt > result && alt < 10000 ) {
532 if ( result > -9000 ) {
533 *terrain_elev = result;
534 *radius = geoc.radius();
537 sgSetVec3(tmp, hit_list->get_normal(this_hit));
538 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
539 // << tmp[2] << endl;
540 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
541 sgXformVec3(tmp, tmp, TMP);
542 // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
543 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
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 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
606 sgXformVec3(tmp, tmp, TMP);
607 sgdSetVec3( normal, tmp[2], tmp[1], tmp[0] );
608 /* ssgState *IntersectedLeafState =
609 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
612 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
613 return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
614 terrain_elev,radius,normal);