17 #include <simgear/constants.h>
18 #include <simgear/sg_inlines.h>
19 #include <simgear/debug/logstream.hxx>
20 #include <simgear/math/point3d.hxx>
21 #include <simgear/math/sg_geodesy.hxx>
22 #include <simgear/math/vector.hxx>
24 #include <Main/globals.hxx>
25 #include <Main/viewer.hxx>
27 #include "hitlist.hxx"
30 extern ssgBranch *terrain_branch;
33 // check to see if the intersection point is
34 // actually inside this face
35 static bool pointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
37 double xmin, xmax, ymin, ymax, zmin, zmax;
39 // punt if outside bouding cube
40 if ( point[0] < (xmin = SG_MIN3 (tri[0][0], tri[1][0], tri[2][0])) ) {
42 } else if ( point[0] > (xmax = SG_MAX3 (tri[0][0], tri[1][0], tri[2][0])) )
45 } else if ( point[1] < (ymin = SG_MIN3 (tri[0][1], tri[1][1], tri[2][1])) )
48 } else if ( point[1] > (ymax = SG_MAX3 (tri[0][1], tri[1][1], tri[2][1])) )
51 } else if ( point[2] < (zmin = SG_MIN3 (tri[0][2], tri[1][2], tri[2][2])) )
54 } else if ( point[2] > (zmax = SG_MAX3 (tri[0][2], tri[1][2], tri[2][2])) )
59 // (finally) check to see if the intersection point is
60 // actually inside this face
62 //first, drop the smallest dimension so we only have to work
64 double dx = xmax - xmin;
65 double dy = ymax - ymin;
66 double dz = zmax - zmin;
67 double min_dim = SG_MIN3 (dx, dy, dz);
69 //first, drop the smallest dimension so we only have to work
71 double x1, y1, x2, y2, x3, y3, rx, ry;
72 if ( fabs(min_dim-dx) <= SG_EPSILON ) {
73 // x is the smallest dimension
82 } else if ( fabs(min_dim-dy) <= SG_EPSILON ) {
83 // y is the smallest dimension
92 } else if ( fabs(min_dim-dz) <= SG_EPSILON ) {
93 // z is the smallest dimension
103 // all dimensions are really small so lets call it close
104 // enough and return a successful match
108 // check if intersection point is on the same side of p1 <-> p2 as p3
109 double tmp = (y2 - y3) / (x2 - x3);
110 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
111 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
112 if ( side1 != side2 ) {
113 // printf("failed side 1 check\n");
117 // check if intersection point is on correct side of p2 <-> p3 as p1
118 tmp = (y3 - ry) / (x3 - rx);
119 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
120 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
121 if ( side1 != side2 ) {
122 // printf("failed side 2 check\n");
126 // check if intersection point is on correct side of p1 <-> p3 as p2
127 tmp = (y2 - ry) / (x2 - rx);
128 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
129 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
130 if ( side1 != side2 ) {
131 // printf("failed side 3 check\n");
138 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
139 const sgdVec3 l_vec, const sgdVec4 plane )
141 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
143 /* Is line parallel to plane? */
145 if ( fabs ( tmp ) < FLT_EPSILON )
148 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
149 + plane[3] ) / tmp ) ;
150 sgdAddVec3 ( dst, l_org ) ;
156 static void sgdXformPnt3 ( sgdVec3 dst, const sgVec3 src, const sgdMat4 mat )
158 SGDfloat t0 = src[ 0 ] ;
159 SGDfloat t1 = src[ 1 ] ;
160 SGDfloat t2 = src[ 2 ] ;
162 dst[0] = ( t0 * mat[ 0 ][ 0 ] +
167 dst[1] = ( t0 * mat[ 0 ][ 1 ] +
172 dst[2] = ( t0 * mat[ 0 ][ 2 ] +
181 Find the intersection of an infinite line with a plane
182 (the line being defined by a point and direction).
184 Norman Vine -- nhv@yahoo.com (with hacks by Steve)
187 int sgdIsectInfLinePlane( sgdVec3 dst, sgdVec3 l_org,
188 sgdVec3 l_vec, sgdVec4 plane )
190 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
192 /* Is line parallel to plane? */
194 if ( sgdAbs ( tmp ) < DBL_EPSILON )
197 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
198 + plane[3] ) / tmp ) ;
199 sgdAddVec3 ( dst, l_org ) ;
206 * Given a point and a triangle lying on the same plane
207 * check to see if the point is inside the triangle
209 bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
214 // punt if outside bouding cube
215 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
216 if( (point[0] < min) || (point[0] > max) )
220 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
221 if( (point[1] < min) || (point[1] > max) )
225 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
226 if( (point[2] < min) || (point[2] > max) )
230 // drop the smallest dimension so we only have to work in 2d.
231 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
232 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
233 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
234 // x is the smallest dimension
243 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
244 // y is the smallest dimension
253 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
254 // z is the smallest dimension
264 // all dimensions are really small so lets call it close
265 // enough and return a successful match
269 // check if intersection point is on the same side of p1 <-> p2 as p3
270 SGDfloat tmp = (y2 - y3) / (x2 - x3);
271 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
272 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
273 if ( side1 != side2 ) {
274 // printf("failed side 1 check\n");
278 // check if intersection point is on correct side of p2 <-> p3 as p1
279 tmp = (y3 - ry) / (x3 - rx);
280 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
281 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
282 if ( side1 != side2 ) {
283 // printf("failed side 2 check\n");
287 // check if intersection point is on correct side of p1 <-> p3 as p2
288 tmp = (y2 - ry) / (x2 - rx);
289 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
290 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
291 if ( side1 != side2 ) {
292 // printf("failed side 3 check\n");
301 Find the intersection of an infinite line with a leaf
302 the line being defined by a point and direction.
306 ssgLeaf pointer -- leaf
307 qualified matrix -- m
309 line direction -- dir
311 result -- intersection point
312 normal -- intersected tri's normal
315 true if intersection found
318 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
319 sgdVec3 orig, sgdVec3 dir )
322 for ( int i = 0; i < leaf->getNumTriangles(); ++i ) {
324 leaf->getTriangle( i, &i1, &i2, &i3 );
327 sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
328 sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
329 sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
331 //avoid division by zero when two points are the same
332 if ( sgdEqualVec3(tri[0], tri[1]) ||
333 sgdEqualVec3(tri[1], tri[2]) ||
334 sgdEqualVec3(tri[2], tri[0]) ) {
339 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
343 //inlined IsectInfLinePlane( point dst, orig, dir, plane )
344 SGDfloat tmp = sgdScalarProductVec3 ( dir, plane ) ;
346 /* Is line parallel to plane? */
347 if ( sgdAbs ( tmp ) < DBL_EPSILON )
350 sgdScaleVec3 ( point, dir,
351 -( sgdScalarProductVec3 ( orig, plane ) + plane[3] )
354 sgdAddVec3 ( point, orig ) ;
355 // end of inlined intersection
357 if( pointInTriangle( point, tri ) ) {
358 add(leaf,i,point,plane);
362 if( sgdPointInTriangle( point, tri ) ) {
363 // transform point into passed into desired coordinate frame
364 sgdXformPnt3( point, point, m );
365 add(leaf,i,point,plane);
373 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
374 sgdVec3 orig, sgdVec3 dir )
378 /* the lookat vector and matrix in branch's coordinate frame
379 * but we won't determine these unless needed to,
380 * This 'lazy evaluation' is a result of profiling data */
384 // 'lazy evaluation' flag
387 for ( ssgEntity *kid = branch->getKid( 0 );
389 kid = branch->getNextKid() )
391 if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
392 bsphere = kid->getBSphere();
395 bsphere->getCenter()[0],
396 bsphere->getCenter()[1],
397 bsphere->getCenter()[2] );
398 sgdXformPnt3( center, m ) ;
400 // sgdClosestPointToLineDistSquared( center, orig, dir )
401 // inlined here because because of profiling results
403 sgdSubVec3(u, center, orig);
404 sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
405 / sgdScalarProductVec3(dir,dir) );
406 sgdSubVec3(v, u, u1);
408 // doubles because of possible overflow
409 #define SQUARE(x) (x*x)
410 if( sgdScalarProductVec3(v, v)
411 < SQUARE( double(bsphere->getRadius()) ) )
413 // possible intersections
414 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
416 sgdCopyMat4(m_new, m);
417 if ( kid->isA( ssgTypeTransform() ) ) {
419 ((ssgTransform *)kid)->getTransform( fxform );
421 sgdSetMat4( xform, fxform );
422 sgdPreMultMat4( m_new, xform );
424 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
425 } else if ( kid->isAKindOf( ssgTypeLeaf() ) ) {
428 sgdTransposeNegateMat4( _m, m);
429 sgdXformPnt3( _orig, orig, _m );
430 sgdXformPnt3( _dir, dir, _m );
433 IntersectLeaf( (ssgLeaf *)kid, m, _orig, _dir );
436 // end of the line for this branch
439 // branch requested not to be traversed
445 // This expects the inital m to the identity transform
446 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
448 for ( ssgEntity *parent = branch->getParent(0);
450 parent = parent->getNextParent() )
452 // recurse till we are at the scene root
453 // then just unwinding the stack should
454 // give us our cumulative transform :-) NHV
455 ssgGetEntityTransform( parent, m );
456 if ( parent->isA( ssgTypeTransform() ) ) {
458 ((ssgTransform *)parent)->getTransform( xform );
459 sgPreMultMat4( m, xform );
465 // return the passed entitity's bsphere's center point radius and
466 // fully formed current model matrix for entity
467 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
468 float *radius, sgMat4 m )
470 sgSphere *bsphere = entity->getBSphere();
471 *radius = (double)bsphere->getRadius();
472 sgCopyVec3( center, bsphere->getCenter() );
473 sgMakeIdentMat4 ( m ) ;
474 ssgGetEntityTransform( entity, m );
478 void FGHitList::IntersectCachedLeaf( sgdMat4 m,
479 sgdVec3 orig, sgdVec3 dir)
485 // ssgEntity *ent = last_hit();
486 ssgGetCurrentBSphere( last_hit(), fcenter, &radius, fxform );
489 sgdSetMat4( m, fxform );
490 sgdXformPnt3( center, m );
492 if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
493 double(radius*radius) )
495 IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
501 void FGHitList::Intersect( ssgBranch *scene, sgdVec3 orig, sgdVec3 dir ) {
504 // #define USE_CACHED_HIT
506 #ifdef USE_CACHED_HIT
507 // This optimization gives a slight speedup
508 // but it precludes using the hitlist for dynamic
512 sgdMakeIdentMat4 ( m ) ;
513 IntersectCachedLeaf(m, orig, dir);
519 sgdMakeIdentMat4 ( m ) ;
520 IntersectBranch( scene, m, orig, dir);
522 #ifdef USE_CACHED_HIT
528 static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
530 sgSetVec3(tmp, src[0], src[1], src[2] );
532 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
533 sgXformVec3(tmp, tmp, TMP);
534 sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
538 // a temporary hack until we get everything rewritten with sgdVec3
539 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
541 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
545 // Determine scenery altitude via ssg. Normally this just happens
546 // when we render the scene, but we'd also like to be able to do this
547 // explicitely. lat & lon are in radians. view_pos in current world
548 // coordinate translated near (0,0,0) (in meters.) Returns result in
550 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
552 double *terrain_elev, double *radius, double *normal)
555 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
558 sgdCopyVec3(orig, view_pos );
559 sgdCopyVec3(dir, abs_view_pos );
561 hit_list->Intersect( terrain_branch, orig, dir );
565 double result = -9999;
566 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
568 int hitcount = hit_list->num_hits();
569 for ( int i = 0; i < hitcount; ++i ) {
570 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
571 double lat_geod, alt, sea_level_r;
572 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
574 if ( alt > result && alt < 10000 ) {
580 if ( result > -9000 ) {
581 *terrain_elev = result;
582 *radius = geoc.radius();
584 sgSetVec3(tmp, hit_list->get_normal(this_hit));
585 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
586 // << tmp[2] << endl;
587 /* ssgState *IntersectedLeafState =
588 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
589 CurrentNormalInLocalPlane(tmp, tmp);
590 sgdSetVec3( normal, tmp );
591 // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
594 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
596 float *up = globals->get_current_view()->get_world_up();
597 sgdSetVec3(normal, up[0], up[1], up[2]);