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 )
355 kid->getBSphere()->getCenter()[0],
356 kid->getBSphere()->getCenter()[1],
357 kid->getBSphere()->getCenter()[2] );
358 sgdXformPnt3( center, m ) ;
360 // sgdClosestPointToLineDistSquared( center, orig, dir )
361 // inlined here because because of profiling results
363 sgdSubVec3(u, center, orig);
364 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
365 / sgdScalarProductVec3(dir,dir) );
366 sgdSubVec3(v, u, u1);
368 // double because of possible overflow
369 if ( IN_RANGE( v, double(kid->getBSphere()->getRadius()) ) )
371 if ( kid->isAKindOf ( ssgTypeBranch() ) )
374 if ( kid->isA( ssgTypeTransform() ) )
377 ((ssgTransform *)kid)->getTransform( fxform );
378 sgMultMat4(m_new, m, fxform);
380 sgdCopyMat4(m_new, m);
382 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
384 else if ( kid->isAKindOf( ssgTypeLeaf() ) )
387 sgdTransposeNegateMat4( m_leaf, m );
388 sgdXformPnt3( orig_leaf, orig, m_leaf );
389 sgdXformPnt3( dir_leaf, dir, m_leaf );
392 GLenum primType = ((ssgLeaf *)kid)->getPrimitiveType();
393 IntersectLeaf( (ssgLeaf *)kid, m, orig_leaf, dir_leaf, primType );
396 } // branch not requested to be traversed
402 // ======================
403 // a temporary hack until we get everything rewritten with sgdVec3
404 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
406 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
410 // ======================
411 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
414 sgdMakeIdentMat4 ( m ) ;
415 IntersectBranch( scene, m, orig, dir );
418 // ======================
419 void FGHitList::Intersect( ssgBranch *scene, sgdMat4 m, sgdVec3 orig, sgdVec3 dir )
422 IntersectBranch( scene, m, orig, dir );
425 // ======================
426 // Need these because of mixed matrix types
427 static void sgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2)
429 for ( int j = 0 ; j < 4 ; j++ )
431 dst[0][j] = m2[0][0] * m1[0][j] +
432 m2[0][1] * m1[1][j] +
433 m2[0][2] * m1[2][j] +
434 m2[0][3] * m1[3][j] ;
436 dst[1][j] = m2[1][0] * m1[0][j] +
437 m2[1][1] * m1[1][j] +
438 m2[1][2] * m1[2][j] +
439 m2[1][3] * m1[3][j] ;
441 dst[2][j] = m2[2][0] * m1[0][j] +
442 m2[2][1] * m1[1][j] +
443 m2[2][2] * m1[2][j] +
444 m2[2][3] * m1[3][j] ;
446 dst[3][j] = m2[3][0] * m1[0][j] +
447 m2[3][1] * m1[1][j] +
448 m2[3][2] * m1[2][j] +
449 m2[3][3] * m1[3][j] ;
453 // ======================
454 static void ssgGetEntityTransform(ssgEntity *entity, sgMat4 m )
457 Walk backwards up the tree, transforming the
458 vertex by all the matrices along the way.
460 Upwards recursion hurts my head.
466 If this node has a parent - get the composite
467 matrix for the parent.
469 if ( entity->getNumParents() > 0 )
470 ssgGetEntityTransform ( entity->getParent(0), mat ) ;
472 sgMakeIdentMat4 ( mat ) ;
475 If this node has a transform - then concatenate it.
477 if ( entity -> isAKindOf ( ssgTypeTransform () ) ) {
479 ((ssgTransform *) entity) -> getTransform ( this_mat ) ;
480 sgPostMultMat4 ( mat, this_mat ) ;
483 sgCopyMat4 ( m, mat ) ;
486 // ======================
487 // return the passed entitity's bsphere's center point radius and
488 // fully formed current model matrix for entity
489 static void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center, float *radius, sgMat4 m )
491 sgSphere *bsphere = entity->getBSphere();
492 *radius = (double)bsphere->getRadius();
493 sgCopyVec3( center, bsphere->getCenter() );
494 sgMakeIdentMat4 ( m ) ;
495 ssgGetEntityTransform( entity, m );
499 // ======================
500 // Determine scenery altitude via ssg.
501 // returned results are in meters
502 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
504 double *terrain_elev, double *radius, double *normal)
507 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
510 sgdCopyVec3(orig, view_pos );
511 sgdCopyVec3(dir, abs_view_pos );
513 hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
518 double result = -9999;
519 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
521 // cout << "hits = ";
522 int hitcount = hit_list->num_hits();
523 for ( int i = 0; i < hitcount; ++i ) {
524 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
525 double lat_geod, alt, sea_level_r;
526 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
528 // cout << alt << " ";
529 if ( alt > result && alt < 10000 ) {
536 if ( result > -9000 ) {
537 *terrain_elev = result;
538 *radius = geoc.radius();
540 sgSetVec3(tmp, hit_list->get_normal(this_hit));
541 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
542 sgdSetVec3( normal, tmp );
543 // float *up = globals->get_current_view()->get_world_up();
544 // cout << "world_up : " << up[0] << " " << up[1] << " " << up[2] << endl;
545 /* ssgState *IntersectedLeafState =
546 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
549 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
551 float *up = globals->get_current_view()->get_world_up();
552 sgdSetVec3(normal, up[0], up[1], up[2]);
558 // ======================
559 // Determine scenery altitude via ssg.
560 // returned results are in meters
561 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
562 ssgTransform *terra_transform,
564 double *terrain_elev, double *radius, double *normal)
567 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
570 sgdCopyVec3(orig, view_pos );
572 sgdCopyVec3(dir, abs_view_pos );
573 sgdNormalizeVec3(dir);
576 sgMakeIdentMat4 ( fxform ) ;
577 ssgGetEntityTransform( terra_transform, fxform );
580 sgdSetMat4(xform,fxform);
581 hit_list->Intersect( terra_transform, xform, orig, dir );
585 double result = -9999;
586 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
588 int hitcount = hit_list->num_hits();
589 for ( int i = 0; i < hitcount; ++i ) {
590 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
591 double lat_geod, alt, sea_level_r;
592 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
594 if ( alt > result && alt < 20000 ) {
600 if ( result > -9000 ) {
601 *terrain_elev = result;
602 *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);