#include <simgear/sg_inlines.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/math/point3d.hxx>
+#include <simgear/math/polar3d.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/math/vector.hxx>
#include <simgear/timing/timestamp.hxx>
{
sgdVec3 dif;
+ // Some tolerance in meters we accept a point to be outside of the triangle
+ // and still return that it is inside.
+ SGDfloat eps = 1e-4;
SGDfloat min, max;
// punt if outside bouding cube
SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
- if( (point[0] < min) || (point[0] > max) )
+ if( (point[0] < min - eps) || (point[0] > max + eps) )
return false;
dif[0] = max - min;
SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
- if( (point[1] < min) || (point[1] > max) )
+ if( (point[1] < min - eps) || (point[1] > max + eps) )
return false;
dif[1] = max - min;
SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
- if( (point[2] < min) || (point[2] > max) )
+ if( (point[2] < min - eps) || (point[2] > max + eps) )
return false;
dif[2] = max - min;
}
// check if intersection point is on the same side of p1 <-> p2 as p3
- SGDfloat tmp = (y2 - y3) / (x2 - x3);
- int side1 = SG_SIGN (tmp * (rx - x3) + y3 - ry);
- int side2 = SG_SIGN (tmp * (x1 - x3) + y3 - y1);
+ SGDfloat tmp = (y2 - y3);
+ SGDfloat tmpn = (x2 - x3);
+ int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
+ int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
+ + side1 * eps * fabs(tmpn));
if ( side1 != side2 ) {
// printf("failed side 1 check\n");
return false;
}
// check if intersection point is on correct side of p2 <-> p3 as p1
- tmp = (y3 - ry) / (x3 - rx);
- side1 = SG_SIGN (tmp * (x2 - rx) + ry - y2);
- side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
+ tmp = (y3 - ry);
+ tmpn = (x3 - rx);
+ side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
+ side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
+ + side1 * eps * fabs(tmpn));
if ( side1 != side2 ) {
// printf("failed side 2 check\n");
return false;
}
// check if intersection point is on correct side of p1 <-> p3 as p2
- tmp = (y2 - ry) / (x2 - rx);
- side1 = SG_SIGN (tmp * (x3 - rx) + ry - y3);
- side2 = SG_SIGN (tmp * (x1 - rx) + ry - y1);
+ tmp = (y2 - ry);
+ tmpn = (x2 - rx);
+ side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
+ side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
+ + side1 * eps * fabs(tmpn));
if ( side1 != side2 ) {
// printf("failed side 3 check\n");
return false;
FGHitList::~FGHitList() {}
+// http://www.cs.lth.se/home/Tomas_Akenine_Moller/raytri/raytri.c
+// http://little3d.free.fr/ressources/jgt%20Fast,%20Minumum%20Storage%20Ray-Triangle%20Intersection.htm
+// http://www.acm.org/jgt/papers/MollerTrumbore97/
+
+/* Ray-Triangle Intersection Test Routines */
+/* Different optimizations of my and Ben Trumbore's */
+/* code from journals of graphics tools (JGT) */
+/* http://www.acm.org/jgt/ */
+/* by Tomas Moller, May 2000 */
+
+/* code rewritten to do tests on the sign of the determinant */
+/* the division is at the end in the code */
+// cosmetics change by H.J :
+// make u & v locals since we don't use them, use sg functions
+static bool intersect_triangle(const double orig[3], const double dir[3],
+ const double vert0[3], const double vert1[3], const double vert2[3],
+ double *t)
+{
+ double u, v;
+ double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
+
+ const SGDfloat eps = 1e-4;
+
+ /* find vectors for two edges sharing vert0 */
+ sgdSubVec3(edge1, vert1, vert0);
+ sgdSubVec3(edge2, vert2, vert0);
+
+ /* begin calculating determinant - also used to calculate U parameter */
+ sgdVectorProductVec3(pvec, dir, edge2);
+
+ /* if determinant is near zero, ray lies in plane of triangle */
+ double det = sgdScalarProductVec3(edge1, pvec);
+
+ if (det > eps)
+ {
+ /* calculate distance from vert0 to ray origin */
+ sgdSubVec3(tvec, orig, vert0);
+
+ /* calculate U parameter and test bounds */
+ u = sgdScalarProductVec3(tvec, pvec);
+ if (u < 0.0 || u > det)
+ return false;
+
+ /* prepare to test V parameter */
+ sgdVectorProductVec3(qvec, tvec, edge1);
+
+ /* calculate V parameter and test bounds */
+ v = sgdScalarProductVec3(dir, qvec);
+ if (v < 0.0 || u + v > det)
+ return false;
+
+ }
+ else if(det < -eps)
+ {
+ /* calculate distance from vert0 to ray origin */
+ sgdSubVec3(tvec, orig, vert0);
+
+ /* calculate U parameter and test bounds */
+ u = sgdScalarProductVec3(tvec, pvec);
+ if (u > 0.0 || u < det)
+ return false;
+
+ /* prepare to test V parameter */
+ sgdVectorProductVec3(qvec, tvec, edge1);
+
+ /* calculate V parameter and test bounds */
+ v = sgdScalarProductVec3(dir, qvec) ;
+ if (v > 0.0 || u + v < det)
+ return false;
+ }
+ else return false; /* ray is parallell to the plane of the triangle */
+
+ /* calculate t, ray intersects triangle */
+ *t = sgdScalarProductVec3(edge2, qvec) / det;
+
+ return true;
+}
+
+
/*
Find the intersection of an infinite line with a leaf the line being
defined by a point and direction.
sgdSetVec3( tri[0], leaf->getVertex( i1 ) );
sgdSetVec3( tri[1], leaf->getVertex( i2 ) );
sgdSetVec3( tri[2], leaf->getVertex( i3 ) );
-
+#if 1
+ sgdFloat t;
+ if( intersect_triangle( orig, dir, tri[0], tri[1], tri[2], &t) ) {
+ sgdVec4 plane;
+ sgdMakePlane( plane, tri[0], tri[1], tri[2] );
+ // t is the distance to the triangle plane
+ // so P = Orig + t*dir
+ sgdVec3 point;
+ sgdAddScaledVec3( point, orig, dir, t );
+ sgdXformPnt3( point, point, m );
+ sgdXformPnt4(plane,plane,m);
+ add(leaf,i,point,plane);
+ num_hits++;
+ }
+#else
if( isZeroAreaTri( tri ) )
continue;
if( fgdPointInTriangle( point, tri ) ) {
// transform point into passed into desired coordinate frame
sgdXformPnt3( point, point, m );
- sgdXformPnt4(plane,plane,m);
+ sgdXformPnt4(plane,plane,m);
add(leaf,i,point,plane);
num_hits++;
}
}
+#endif
}
return num_hits;
}
&& !kid->getBSphere()->isEmpty() )
{
sgdVec3 center;
+ const sgFloat *BSCenter = kid->getBSphere()->getCenter();
sgdSetVec3( center,
- kid->getBSphere()->getCenter()[0],
- kid->getBSphere()->getCenter()[1],
- kid->getBSphere()->getCenter()[2] );
+ BSCenter[0],
+ BSCenter[1],
+ BSCenter[2] );
sgdXformPnt3( center, m ) ;
// sgdClosestPointToLineDistSquared( center, orig, dir )
// inlined here because because of profiling results
sgdVec3 u, u1, v;
sgdSubVec3(u, center, orig);
- sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir)
- / sgdScalarProductVec3(dir,dir) );
+ sgdScaleVec3( u1, dir, sgdScalarProductVec3(u,dir) );
sgdSubVec3(v, u, u1);
// double because of possible overflow
sgdCopyVec3(orig, view_pos );
sgdCopyVec3(dir, abs_view_pos );
+ sgdNormaliseVec3( dir );
hit_list->Intersect( globals->get_scenery()->get_terrain_branch(),
orig, dir );
int this_hit = -1;
int max_hit = -1;
- Point3D geoc;
double hit_elev = -9999;
double max_elev = -9999;
Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
if ( hit_elev > -9000 ) {
*terrain_elev = hit_elev;
- *radius = geoc.radius();
+ *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
sgVec3 tmp;
sgSetVec3(tmp, hit_list->get_normal(this_hit));
// cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
int this_hit = -1;
int max_hit = -1;
- Point3D geoc;
double hit_elev = -9999;
double max_elev = -9999;
Point3D sc(scenery_center[0], scenery_center[1], scenery_center[2]) ;
max_hit = i;
}
}
+
if ( this_hit < 0 ) {
// no hits below us, take the max hit
if ( hit_elev > -9000 ) {
*terrain_elev = hit_elev;
- *radius = geoc.radius();
+ *radius = sgCartToPolar3d(sc + hit_list->get_point(this_hit)).radius();
+
sgVec3 tmp;
sgSetVec3(tmp, hit_list->get_normal(this_hit));
// cout << "cur_normal: " << tmp[0] << " " << tmp[1] << " " << tmp[2] << endl;
projects / flightgear.git / blobdiff