15 #include <simgear/constants.h>
16 #include <simgear/sg_inlines.h>
17 #include <simgear/debug/logstream.hxx>
18 #include <simgear/math/point3d.hxx>
19 #include <simgear/math/sg_geodesy.hxx>
20 #include <simgear/math/vector.hxx>
22 #include <Main/globals.hxx>
24 #include "hitlist.hxx"
27 extern ssgBranch *terrain;
30 // check to see if the intersection point is
31 // actually inside this face
32 static bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
34 double xmin, xmax, ymin, ymax, zmin, zmax;
36 // punt if outside bouding cube
37 if ( point[0] < (xmin = SG_MIN3 (tri[0][0], tri[1][0], tri[2][0])) ) {
39 } else if ( point[0] > (xmax = SG_MAX3 (tri[0][0], tri[1][0], tri[2][0])) ) {
41 } else if ( point[1] < (ymin = SG_MIN3 (tri[0][1], tri[1][1], tri[2][1])) ) {
43 } else if ( point[1] > (ymax = SG_MAX3 (tri[0][1], tri[1][1], tri[2][1])) ) {
45 } else if ( point[2] < (zmin = SG_MIN3 (tri[0][2], tri[1][2], tri[2][2])) ) {
47 } else if ( point[2] > (zmax = SG_MAX3 (tri[0][2], tri[1][2], tri[2][2])) ) {
51 // (finally) check to see if the intersection point is
52 // actually inside this face
54 //first, drop the smallest dimension so we only have to work
56 double dx = xmax - xmin;
57 double dy = ymax - ymin;
58 double dz = zmax - zmin;
59 double min_dim = SG_MIN3 (dx, dy, dz);
61 //first, drop the smallest dimension so we only have to work
63 double x1, y1, x2, y2, x3, y3, rx, ry;
64 if ( fabs(min_dim-dx) <= SG_EPSILON ) {
65 // x is the smallest dimension
74 } else if ( fabs(min_dim-dy) <= SG_EPSILON ) {
75 // y is the smallest dimension
84 } else if ( fabs(min_dim-dz) <= SG_EPSILON ) {
85 // z is the smallest dimension
95 // all dimensions are really small so lets call it close
96 // enough and return a successful match
100 // check if intersection point is on the same side of p1 <-> p2 as p3
101 double tmp = (y2 - y3) / (x2 - x3);
102 int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
103 int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
104 if ( side1 != side2 ) {
105 // printf("failed side 1 check\n");
109 // check if intersection point is on correct side of p2 <-> p3 as p1
110 tmp = (y3 - ry) / (x3 - rx);
111 side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
112 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
113 if ( side1 != side2 ) {
114 // printf("failed side 2 check\n");
118 // check if intersection point is on correct side of p1 <-> p3 as p2
119 tmp = (y2 - ry) / (x2 - rx);
120 side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
121 side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
122 if ( side1 != side2 ) {
123 // printf("failed side 3 check\n");
130 static int sgdIsectInfLinePlane( sgdVec3 dst, const sgdVec3 l_org,
131 const sgdVec3 l_vec, const sgdVec4 plane )
133 SGDfloat tmp = sgdScalarProductVec3 ( l_vec, plane ) ;
135 /* Is line parallel to plane? */
137 if ( fabs ( tmp ) < FLT_EPSILON )
140 sgdScaleVec3 ( dst, l_vec, -( sgdScalarProductVec3 ( l_org, plane )
141 + plane[3] ) / tmp ) ;
142 sgdAddVec3 ( dst, l_org ) ;
148 static void sgdXformPnt3 ( sgdVec3 dst, const sgVec3 src, const sgdMat4 mat )
150 SGDfloat t0 = src[ 0 ] ;
151 SGDfloat t1 = src[ 1 ] ;
152 SGDfloat t2 = src[ 2 ] ;
154 dst[0] = ( t0 * mat[ 0 ][ 0 ] +
159 dst[1] = ( t0 * mat[ 0 ][ 1 ] +
164 dst[2] = ( t0 * mat[ 0 ][ 2 ] +
171 Find the intersection of an infinite line with a leaf
172 the line being defined by a point and direction.
176 ssgLeaf pointer -- leaf
177 qualified matrix -- m
179 line direction -- dir
181 result -- intersection point
182 normal -- intersected tri's normal
185 true if intersection found
188 int FGHitList::IntersectLeaf( ssgLeaf *leaf, sgdMat4 m,
189 sgdVec3 orig, sgdVec3 dir )
192 for ( int i = 0; i < leaf->getNumTriangles(); ++i ) {
194 leaf->getTriangle( i, &i1, &i2, &i3 );
197 sgdXformPnt3( tri[0], leaf->getVertex( i1 ), m );
198 sgdXformPnt3( tri[1], leaf->getVertex( i2 ), m );
199 sgdXformPnt3( tri[2], leaf->getVertex( i3 ), m );
202 sgdMakePlane( plane, tri[0], tri[1], tri[2] );
205 if( sgdIsectInfLinePlane( point, orig, dir, plane ) ) {
206 if( sgdPointInTriangle( point, tri ) ) {
207 add(leaf,i,point,plane);
215 void FGHitList::IntersectBranch( ssgBranch *branch, sgdMat4 m,
216 sgdVec3 orig, sgdVec3 dir )
219 for ( ssgEntity *kid = branch->getKid( 0 );
221 kid = branch->getNextKid() )
223 if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
224 bsphere = kid->getBSphere();
226 sgCopyVec3( fcenter, bsphere->getCenter() );
228 center[0] = fcenter[0];
229 center[1] = fcenter[1];
230 center[2] = fcenter[2];
231 sgdXformPnt3( center, m ) ;
232 double radius_sqd = bsphere->getRadius() * bsphere->getRadius();
233 if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
236 // possible intersections
237 if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
239 sgdCopyMat4(m_new, m);
240 if ( kid->isA( ssgTypeTransform() ) ) {
242 ((ssgTransform *)kid)->getTransform( fxform );
244 sgdSetMat4( xform, fxform );
245 sgdPreMultMat4( m_new, xform );
247 IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
248 } else if ( kid->isAKindOf ( ssgTypeLeaf() ) ) {
249 IntersectLeaf( (ssgLeaf *)kid, m, orig, dir );
252 // end of the line for this branch
255 // branch requested not to be traversed
261 // This expects the inital m to the identity transform
262 void ssgGetEntityTransform(ssgEntity *branch, sgMat4 m )
264 for ( ssgEntity *parent = branch->getParent(0);
266 parent = parent->getNextParent() )
268 // recurse till we are at the scene root
269 // then just unwinding the stack should
270 // give us our cumulative transform :-) NHV
271 ssgGetEntityTransform( parent, m );
272 if ( parent->isA( ssgTypeTransform() ) ) {
274 ((ssgTransform *)parent)->getTransform( xform );
275 sgPreMultMat4( m, xform );
281 // return the passed entitity's bsphere's center point radius and
282 // fully formed current model matrix for entity
283 void ssgGetCurrentBSphere( ssgEntity *entity, sgVec3 center,
284 float *radius, sgMat4 m )
286 sgSphere *bsphere = entity->getBSphere();
287 *radius = (double)bsphere->getRadius();
288 sgCopyVec3( center, bsphere->getCenter() );
289 sgMakeIdentMat4 ( m ) ;
290 ssgGetEntityTransform( entity, m );
294 void FGHitList::IntersectCachedLeaf( sgdMat4 m,
295 sgdVec3 orig, sgdVec3 dir)
301 // ssgEntity *ent = last_hit();
302 ssgGetCurrentBSphere( last_hit(), fcenter, &radius, fxform );
305 sgdSetMat4( m, fxform );
306 sgdXformPnt3( center, m );
308 if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
311 IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
317 static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
319 sgSetVec3(tmp, src[0], src[1], src[2] );
321 sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
322 sgXformVec3(tmp, tmp, TMP);
323 sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
327 // a temporary hack until we get everything rewritten with sgdVec3
328 static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
330 return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
334 // Determine scenery altitude via ssg. Normally this just happens
335 // when we render the scene, but we'd also like to be able to do this
336 // explicitely. lat & lon are in radians. view_pos in current world
337 // coordinate translated near (0,0,0) (in meters.) Returns result in
339 bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
341 double *terrain_elev, double *radius, double *normal)
344 sgdSubVec3( view_pos, abs_view_pos, scenery_center );
347 sgdCopyVec3(orig, view_pos );
348 sgdCopyVec3(dir, abs_view_pos );
350 hit_list->Intersect( terrain, orig, dir );
354 double result = -9999;
355 Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
357 int hitcount = hit_list->num_hits();
358 for ( int i = 0; i < hitcount; ++i ) {
359 geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
360 double lat_geod, alt, sea_level_r;
361 sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
363 if ( alt > result && alt < 10000 ) {
369 if ( result > -9000 ) {
370 *terrain_elev = result;
371 *radius = geoc.radius();
373 sgSetVec3(tmp, hit_list->get_normal(this_hit));
374 // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
375 // << tmp[2] << endl;
376 /* ssgState *IntersectedLeafState =
377 ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
378 CurrentNormalInLocalPlane(tmp, tmp);
379 sgdSetVec3( normal, tmp );
380 // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
383 SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
385 float *up = globals->get_current_view()->get_world_up();
386 sgdSetVec3(normal, up[0], up[1], up[2]);