#include <math.h>
#include <GL/glut.h>
+#include <GL/gl.h>
#include <simgear/constants.h>
-#include <simgear/inlines.h>
+#include <simgear/sg_inlines.h>
+#include <simgear/debug/logstream.hxx>
+#include <simgear/math/point3d.hxx>
+#include <simgear/math/sg_geodesy.hxx>
#include <simgear/math/vector.hxx>
-#include <simgear/xgl/xgl.h>
+
+#include <Main/globals.hxx>
+#include <Main/viewer.hxx>
#include "hitlist.hxx"
+extern ssgBranch *terrain;
+
+
// check to see if the intersection point is
// actually inside this face
static bool sgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
//first, drop the smallest dimension so we only have to work
//in 2d.
double x1, y1, x2, y2, x3, y3, rx, ry;
- if ( fabs(min_dim-dx) <= FG_EPSILON ) {
+ if ( fabs(min_dim-dx) <= SG_EPSILON ) {
// x is the smallest dimension
x1 = point[1];
y1 = point[2];
y3 = tri[1][2];
rx = tri[2][1];
ry = tri[2][2];
- } else if ( fabs(min_dim-dy) <= FG_EPSILON ) {
+ } else if ( fabs(min_dim-dy) <= SG_EPSILON ) {
// y is the smallest dimension
x1 = point[0];
y1 = point[2];
y3 = tri[1][2];
rx = tri[2][0];
ry = tri[2][2];
- } else if ( fabs(min_dim-dz) <= FG_EPSILON ) {
+ } else if ( fabs(min_dim-dz) <= SG_EPSILON ) {
// z is the smallest dimension
x1 = point[0];
y1 = point[1];
sgdXformPnt3( tri[1], leaf->getVertex( i2 ), m );
sgdXformPnt3( tri[2], leaf->getVertex( i3 ), m );
+ //avoid division by zero when two points are the same
+ if ( sgdEqualVec3(tri[0], tri[1]) ||
+ sgdEqualVec3(tri[1], tri[2]) ||
+ sgdEqualVec3(tri[2], tri[0]) ) {
+ continue;
+ }
+
sgdVec4 plane;
sgdMakePlane( plane, tri[0], tri[1], tri[2] );
kid = branch->getNextKid() )
{
if ( kid->getTraversalMask() & SSGTRAV_HOT ) {
- bsphere = kid->getBSphere();
+ bsphere = kid->getBSphere();
sgVec3 fcenter;
sgCopyVec3( fcenter, bsphere->getCenter() );
sgdVec3 center;
center[1] = fcenter[1];
center[2] = fcenter[2];
sgdXformPnt3( center, m ) ;
- double radius_sqd = bsphere->getRadius() * bsphere->getRadius();
- if ( sgdPointLineDistSquared( center, orig, dir ) < radius_sqd ) {
+ // watch out for overflow
+ if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
+ double(bsphere->getRadius() * bsphere->getRadius()) )
+ {
// possible intersections
if ( kid->isAKindOf ( ssgTypeBranch() ) ) {
sgdMat4 m_new;
sgdCopyMat4(m_new, m);
if ( kid->isA( ssgTypeTransform() ) ) {
- sgMat4 fxform;
- ((ssgTransform *)kid)->getTransform( fxform );
- sgdMat4 xform;
- sgdSetMat4( xform, fxform );
- sgdPreMultMat4( m_new, xform );
+ sgMat4 fxform;
+ ((ssgTransform *)kid)->getTransform( fxform );
+ sgdMat4 xform;
+ sgdSetMat4( xform, fxform );
+ sgdPreMultMat4( m_new, xform );
}
IntersectBranch( (ssgBranch *)kid, m_new, orig, dir );
} else if ( kid->isAKindOf ( ssgTypeLeaf() ) ) {
sgdSetMat4( m, fxform );
sgdXformPnt3( center, m );
- if ( sgdPointLineDistSquared( center, orig, dir ) < radius*radius ) {
+ if ( sgdClosestPointToLineDistSquared( center, orig, dir ) <
+ double(radius*radius) )
+ {
IntersectLeaf( (ssgLeaf *)last_hit(), m, orig, dir );
}
}
}
+
+
+static void CurrentNormalInLocalPlane(sgVec3 dst, sgVec3 src) {
+ sgVec3 tmp;
+ sgSetVec3(tmp, src[0], src[1], src[2] );
+ sgMat4 TMP;
+ sgTransposeNegateMat4 ( TMP, globals->get_current_view()->get_UP() ) ;
+ sgXformVec3(tmp, tmp, TMP);
+ sgSetVec3(dst, tmp[2], tmp[1], tmp[0] );
+}
+
+
+// a temporary hack until we get everything rewritten with sgdVec3
+static inline Point3D operator + (const Point3D& a, const sgdVec3 b)
+{
+ return Point3D(a.x()+b[0], a.y()+b[1], a.z()+b[2]);
+}
+
+
+// Determine scenery altitude via ssg. Normally this just happens
+// when we render the scene, but we'd also like to be able to do this
+// explicitely. lat & lon are in radians. view_pos in current world
+// coordinate translated near (0,0,0) (in meters.) Returns result in
+// meters.
+bool fgCurrentElev( sgdVec3 abs_view_pos, sgdVec3 scenery_center,
+ FGHitList *hit_list,
+ double *terrain_elev, double *radius, double *normal)
+{
+ sgdVec3 view_pos;
+ sgdSubVec3( view_pos, abs_view_pos, scenery_center );
+
+ sgdVec3 orig, dir;
+ sgdCopyVec3(orig, view_pos );
+ sgdCopyVec3(dir, abs_view_pos );
+
+ hit_list->Intersect( terrain, orig, dir );
+
+ int this_hit=0;
+ Point3D geoc;
+ double result = -9999;
+ Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
+
+ int hitcount = hit_list->num_hits();
+ for ( int i = 0; i < hitcount; ++i ) {
+ geoc = sgCartToPolar3d( sc + hit_list->get_point(i) );
+ double lat_geod, alt, sea_level_r;
+ sgGeocToGeod(geoc.lat(), geoc.radius(), &lat_geod,
+ &alt, &sea_level_r);
+ if ( alt > result && alt < 10000 ) {
+ result = alt;
+ this_hit = i;
+ }
+ }
+
+ if ( result > -9000 ) {
+ *terrain_elev = result;
+ *radius = geoc.radius();
+ sgVec3 tmp;
+ sgSetVec3(tmp, hit_list->get_normal(this_hit));
+ // cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " "
+ // << tmp[2] << endl;
+ /* ssgState *IntersectedLeafState =
+ ((ssgLeaf*)hit_list->get_entity(this_hit))->getState(); */
+ CurrentNormalInLocalPlane(tmp, tmp);
+ sgdSetVec3( normal, tmp );
+ // cout << "NED: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
+ return true;
+ } else {
+ SG_LOG( SG_TERRAIN, SG_INFO, "no terrain intersection" );
+ *terrain_elev = 0.0;
+ float *up = globals->get_current_view()->get_world_up();
+ sgdSetVec3(normal, up[0], up[1], up[2]);
+ return false;
+ }
+}
projects / flightgear.git / blobdiff