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>
20 #include <Scenery/scenery.hxx>
22 #include "hitlist.hxx"
24 // forward declaration of our helper/convenience functions
25 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2);
26 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m );
27 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m );
30 // ======================
31 // This is same as PLib's sgdIsectInfLinePlane()
32 // and can be replaced by it after the next PLib release
33 static int fgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
34 const sgdVec3 l_vec, const sgdVec4 plane )
36 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
38 /* Is line parallel to plane? */
40 if ( fabs ( tmp ) < FLT_EPSILON )
43 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
44 + plane[3] ) / tmp ) ;
45 sgdAddVec3 ( dst, l_org ) ;
50 // ======================
53 * Given a point and a triangle lying on the same plane
54 * check to see if the point is inside the triangle
56 // This is same as PLib's sgdPointInTriangle()
57 // and can be replaced by it after the next PLib release
58 static bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
63 // punt if outside bouding cube
64 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
65 if( (point[0] < min) || (point[0] > max) )
69 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
70 if( (point[1] < min) || (point[1] > max) )
74 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
75 if( (point[2] < min) || (point[2] > max) )
79 // drop the smallest dimension so we only have to work in 2d.
80 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
81 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
82 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
83 // x is the smallest dimension
92 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
93 // y is the smallest dimension
102 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
103 // z is the smallest dimension
113 // all dimensions are really small so lets call it close
114 // enough and return a successful match
118 // check if intersection point is on the same side of p1 <-> p2 as p3
119 SGDfloat tmp = (y2 - y3) / (x2 - x3);
120 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
121 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
122 if ( side1 != side2 ) {
123 // printf("failed side 1 check\n");
127 // check if intersection point is on correct side of p2 <-> p3 as p1
128 tmp = (y3 - ry) / (x3 - rx);
129 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
130 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
131 if ( side1 != side2 ) {
132 // printf("failed side 2 check\n");
136 // check if intersection point is on correct side of p1 <-> p3 as p2
137 tmp = (y2 - ry) / (x2 - rx);
138 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
139 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
140 if ( side1 != side2 ) {
141 // printf("failed side 3 check\n");
148 // ======================
150 inline static int isZeroAreaTri( sgdVec3 tri[3] )
152 return( sgdEqualVec3(tri[0], tri[1]) ||
153 sgdEqualVec3(tri[1], tri[2]) ||
154 sgdEqualVec3(tri[2], tri[0]) );
157 FGHitList::FGHitList() :
158 last(NULL), test_dist(DBL_MAX)
162 FGHitList::~FGHitList() {}
166 Find the intersection of an infinite line with a leaf
167 the line being defined by a point and direction.
171 ssgLeaf pointer -- leaf
172 qualified matrix -- m
174 line direction -- dir
176 result -- intersection point
177 normal -- intersected tri's normal
180 true if intersection found
184 If you need an exhaustive list of hitpoints YOU MUST use
185 the generic version of this function as the specialized
186 versions will do an early out of expensive tests if the point
187 can not be the closest one found
190 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
191 sgdVec3 orig, sgdVec3 dir )
196 for ( ; i < leaf->getNumTriangles(); ++i ) {
198 leaf->getTriangle( i, &i1, &i2, &i3 );
201 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
202 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
203 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
205 if( isZeroAreaTri( tri ) )
209 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
212 if( fgdIsectInfLinePlane( point, orig, dir, plane ) ) {
213 if( fgdPointInTriangle( point, tri ) ) {
214 // transform point into passed into desired coordinate frame
215 sgdXformPnt3( point, point, m );
216 sgdXformPnt4(plane,plane,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 sgdXformPnt4(plane,plane,m);
321 add(leaf,n,point,plane);
322 test_dist = tmp_dist;
329 // ======================
330 inline static bool IN_RANGE( sgdVec3 v, double radius ) {
331 return ( sgdScalarProductVec3(v, v) < (radius*radius) );
334 // ======================
335 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
336 sgdVec3 orig, sgdVec3 dir )
338 /* the lookat vector and matrix in branch's coordinate frame
339 * but we won't determine these unless needed,
340 * This 'lazy evaluation' is a result of profiling data */
341 sgdVec3 orig_leaf, dir_leaf;
344 // 'lazy evaluation' flag
347 for ( ssgEntity *kid = branch->getKid( 0 );
349 kid = branch->getNextKid() )
351 if ( kid->getTraversalMask() & SSGTRAV_HOT
352 && !kid->getBSphere()->isEmpty() )
356 kid->getBSphere()->getCenter()[0],
357 kid->getBSphere()->getCenter()[1],
358 kid->getBSphere()->getCenter()[2] );
359 sgdXformPnt3( center, m ) ;
361 // sgdClosestPointToLineDistSquared( center, orig, dir )
362 // inlined here because because of profiling results
364 sgdSubVec3(u, center, orig);
365 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
366 / sgdScalarProductVec3(dir,dir) );
367 sgdSubVec3(v, u, u1);
369 // double because of possible overflow
370 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
372 if ( kid->isAKindOf ( ssgTypeBranch() ) )
375 if ( kid->isA( ssgTypeTransform() ) )
378 ((ssgTransform *)kid)->getTransform( fxform );
379 sgMultMat4(m_new, m, fxform);
381 sgdCopyMat4(m_new, m);
383 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
385 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
388 sgdTransposeNegateMat4( m_leaf, m );
389 sgdXformPnt3( orig_leaf, orig, m_leaf );
390 sgdXformPnt3( dir_leaf, dir, m_leaf );
393 GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
394 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
397 } // branch not requested to be traversed
403 // ======================
404 // a temporary hack until we get everything rewritten with sgdVec3
405 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
407 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
411 // ======================
412 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
415 sgdMakeIdentMat4 ( m ) ;
416 IntersectBranch( scene, m, orig, dir );
419 // ======================
420 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
423 IntersectBranch( scene, m, orig, dir );
426 // ======================
427 // Need these because of mixed matrix types
428 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
430 for ( int j = 0 ; j < 4 ; j++ )
432 dst[0][j] = m2[0][0] * m1[0][j] +
433 m2[0][1] * m1[1][j] +
434 m2[0][2] * m1[2][j] +
435 m2[0][3] * m1[3][j] ;
437 dst[1][j] = m2[1][0] * m1[0][j] +
438 m2[1][1] * m1[1][j] +
439 m2[1][2] * m1[2][j] +
440 m2[1][3] * m1[3][j] ;
442 dst[2][j] = m2[2][0] * m1[0][j] +
443 m2[2][1] * m1[1][j] +
444 m2[2][2] * m1[2][j] +
445 m2[2][3] * m1[3][j] ;
447 dst[3][j] = m2[3][0] * m1[0][j] +
448 m2[3][1] * m1[1][j] +
449 m2[3][2] * m1[2][j] +
450 m2[3][3] * m1[3][j] ;
454 // ======================
455 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
458 Walk backwards up the tree, transforming the
459 vertex by all the matrices along the way.
461 Upwards recursion hurts my head.
467 If this node has a parent - get the composite
468 matrix for the parent.
470 if ( entity->getNumParents() > 0 )
471 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
473 sgMakeIdentMat4 ( mat ) ;
476 If this node has a transform - then concatenate it.
478 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
480 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
481 sgPostMultMat4 ( mat, this_mat ) ;
484 sgCopyMat4 ( m, mat ) ;
487 // ======================
488 // return the passed entitity's bsphere's center point radius and
489 // fully formed current model matrix for entity
490 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
492 sgSphere *bsphere = entity->getBSphere();
493 *radius = (double)bsphere->getRadius();
494 sgCopyVec3( center, bsphere->getCenter() );
495 sgMakeIdentMat4 ( m ) ;
496 ssgGetEntityTransform( entity, m );
500 // ======================
501 // Determine scenery altitude via ssg.
502 // returned results are in meters
503 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
505 double *terrain_elev, double *radius, double *normal)
508 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
511 sgdCopyVec3(orig, view_pos );
512 sgdCopyVec3(dir, abs_view_pos );
514 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
519 double result = -9999;
520 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
522 // cout << "hits = ";
523 int hitcount = hit_list->num_hits();
524 for ( int i = 0; i < hitcount; ++i ) {
525 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
526 double lat_geod, alt, sea_level_r;
527 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
529 // cout << alt << " ";
530 if ( alt > result && alt < 10000 ) {
537 if ( result > -9000 ) {
538 *terrain_elev = result;
539 *radius = geoc.radius();
541 sgSetVec3(tmp, hit_list->get_normal(this_hit));
542 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
543 sgdSetVec3( normal, tmp );
544 // float *up = globals->get_current_view()->get_world_up();
545 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
546 /* ssgState *IntersectedLeafState =
547 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
550 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
552 float *up = globals->get_current_view()->get_world_up();
553 sgdSetVec3(normal, up[0], up[1], up[2]);
559 // ======================
560 // Determine scenery altitude via ssg.
561 // returned results are in meters
562 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
563 ssgTransform *terra_transform,
565 double *terrain_elev, double *radius, double *normal)
568 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
571 sgdCopyVec3(orig, view_pos );
573 sgdCopyVec3(dir, abs_view_pos );
574 sgdNormalizeVec3(dir);
577 sgMakeIdentMat4 ( fxform ) ;
578 ssgGetEntityTransform( terra_transform, fxform );
581 sgdSetMat4(xform,fxform);
582 hit_list->Intersect( terra_transform, xform, orig, dir );
586 double result = -9999;
587 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
589 int hitcount = hit_list->num_hits();
590 for ( int i = 0; i < hitcount; ++i ) {
591 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
592 double lat_geod, alt, sea_level_r;
593 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
595 if ( alt > result && alt < 20000 ) {
601 if ( result > -9000 ) {
602 *terrain_elev = result;
603 *radius = geoc.radius();
605 sgSetVec3(tmp, hit_list->get_normal(this_hit));
606 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
607 sgdSetVec3( normal, tmp );
608 // float *up = globals->get_current_view()->get_world_up();
609 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
610 /* ssgState *IntersectedLeafState =
611 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
614 SG_LOG( SG_TERRAIN, SG_DEBUG, "DOING FULL TERRAIN INTERSECTION" );
615 return fgCurrentElev( abs_view_pos, scenery_center, hit_list,
616 terrain_elev,radius,normal);