fly/flightgear
1
0
Fork 0
Code Activity

Mathias Frhlich:

Browse source 
2.
I made YASim query the the ground cache at the wrong place. This one fixed
this, one can now land the bo105 on top of the oracle buildings  :)

3.
Is a followup of the scenery center update code: Register the scenery center
transform at the time it is put into the scene graph not at creation time.

4.
I held that part back from the past hitlist patch, because I hoped that it
will be sufficient (and the last one was in fact the biggest part) without.
As some test cases from Melchior showed me, it is not. We have additionally
to the wrong computed transform from the prevous patch some roundoff
problems. This patch adds some small tolerance to for the point in triangle
test.
... may be one even needs to increase the eps value further if starting at
some tile boundaries still fails.

5.
That is a big chunk.
Tested now for two days while hunting the second patch  :) .
That is a partial rewrite of the groundcache to use its own datastructures for
that flat scenegraph in the cache. The basic advantage is, what Erik
suggested, to precompute some often used values of these triangles. Also
allmost all computations are now in double precision which should decrease
(hopefully fix), together with a similar tolerance for some point in triangle
tests, the problems with 'no ground below aircraft'.
I am playing with octrees for the groundcache, that will finally solve the
performance problem when high triangular count models end up in the
groundcache. This patch is also some prework for those octrees ...
This commit is contained in:
ehofman 2005-05-30 08:48:27 +00:00
parent 8ff709439b
commit 807d2cc3ca
5 changed files with 466 additions and 422 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/FDM/YASim/YASim.cxx
View file

@ -192,13 +192,13 @@ void YASim::update(double dt)
// ground. Calculate a cartesian coordinate for the ground under
// us, find the (geodetic) up vector normal to the ground, then
// use that to find the final (radius) term of the plane equation.
float v[3] = { get_uBody()*FT2M, get_vBody()*FT2M, get_wBody()*FT2M };
float v[3] = { get_uBody(), get_vBody(), get_wBody() };
float lat = get_Latitude(); float lon = get_Longitude();
double xyz[3];
sgGeodToCart(lat, lon, 0.0, xyz);
float alt = get_Altitude() * FT2M; double xyz[3];
sgGeodToCart(lat, lon, alt, xyz);
// build the environment cache.
float vr = _fdm->getVehicleRadius();
vr += 2.0*dt*Math::mag3(v);
vr += 2.0*FT2M*dt*Math::mag3(v);
prepare_ground_cache_m( 0.0, xyz, vr );
// Track time increments.

759
src/FDM/groundcache.cxx
View file

@ -25,6 +25,7 @@
#include <plib/sg.h>
#include <simgear/sg_inlines.h>
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/math/sg_geodesy.hxx>
@ -37,6 +38,147 @@
#include "flight.hxx"
#include "groundcache.hxx"
// Specialized version of sgMultMat4 needed because of mixed matrix
// types
static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
for ( int j = 0 ; j < 4 ; j++ ) {
dst[0][j] = m2[0][0] * m1[0][j] +
m2[0][1] * m1[1][j] +
m2[0][2] * m1[2][j] +
m2[0][3] * m1[3][j] ;
dst[1][j] = m2[1][0] * m1[0][j] +
m2[1][1] * m1[1][j] +
m2[1][2] * m1[2][j] +
m2[1][3] * m1[3][j] ;
dst[2][j] = m2[2][0] * m1[0][j] +
m2[2][1] * m1[1][j] +
m2[2][2] * m1[2][j] +
m2[2][3] * m1[3][j] ;
dst[3][j] = m2[3][0] * m1[0][j] +
m2[3][1] * m1[1][j] +
m2[3][2] * m1[2][j] +
m2[3][3] * m1[3][j] ;
}
}
static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
{
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-2;
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 - 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 - 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 - eps) || (point[2] > max + eps) )
return false;
dif[2] = max - min;
// drop the smallest dimension so we only have to work in 2d.
SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
// x is the smallest dimension
x1 = point[1];
y1 = point[2];
x2 = tri[0][1];
y2 = tri[0][2];
x3 = tri[1][1];
y3 = tri[1][2];
rx = tri[2][1];
ry = tri[2][2];
} else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
// y is the smallest dimension
x1 = point[0];
y1 = point[2];
x2 = tri[0][0];
y2 = tri[0][2];
x3 = tri[1][0];
y3 = tri[1][2];
rx = tri[2][0];
ry = tri[2][2];
} else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
// z is the smallest dimension
x1 = point[0];
y1 = point[1];
x2 = tri[0][0];
y2 = tri[0][1];
x3 = tri[1][0];
y3 = tri[1][1];
rx = tri[2][0];
ry = tri[2][1];
} else {
// all dimensions are really small so lets call it close
// enough and return a successful match
return true;
}
// check if intersection point is on the same side of p1 <-> p2 as p3
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 - side1*eps - y1) * 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);
tmpn = (x3 - rx);
side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
side2 = SG_SIGN (tmp * (x1 - rx) + (ry - side1*eps - y1) * 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);
tmpn = (x2 - rx);
side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
side2 = SG_SIGN (tmp * (x1 - rx) + (ry - side1*eps - y1) * tmpn);
if ( side1 != side2 ) {
// printf("failed side 3 check\n");
return false;
}
return true;
}
// Test if the line given by the point on the line pt_on_line and the
// line direction dir intersects the sphere sp.
// Adapted from plib.
static inline bool
fgdIsectSphereInfLine(const sgdSphere& sp,
const sgdVec3 pt_on_line, const sgdVec3 dir)
{
sgdVec3 r;
sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
projectedDistance * projectedDistance;
SGDfloat radius = sp.getRadius();
return dist < radius*radius;
}
FGGroundCache::FGGroundCache()
{
sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
@ -50,10 +192,9 @@ FGGroundCache::FGGroundCache()
FGGroundCache::~FGGroundCache()
{
cache_root.removeAllKids();
}
FGGroundCache::GroundProperty*
FGGroundCache::GroundProperty
FGGroundCache::extractGroundProperty( ssgLeaf* l )
{
// FIXME: Do more ...
@ -62,33 +203,33 @@ FGGroundCache::extractGroundProperty( ssgLeaf* l )
// from property tree or whatever ...
// Get ground dependent data.
GroundProperty *gp = new GroundProperty;
gp->wire_id = -1;
GroundProperty gp;
gp.wire_id = -1;
FGAICarrierHardware *ud =
dynamic_cast<FGAICarrierHardware*>(l->getUserData());
if (ud) {
switch (ud->type) {
case FGAICarrierHardware::Wire:
gp->type = FGInterface::Wire;
gp->wire_id = ud->id;
gp.type = FGInterface::Wire;
gp.wire_id = ud->id;
break;
case FGAICarrierHardware::Catapult:
gp->type = FGInterface::Catapult;
gp.type = FGInterface::Catapult;
break;
default:
gp->type = FGInterface::Solid;
gp.type = FGInterface::Solid;
break;
}
// Copy the velocity from the carrier class.
ud->carrier->getVelocityWrtEarth( gp->vel );
ud->carrier->getVelocityWrtEarth( gp.vel );
}
else {
// Initialize velocity field.
sgSetVec3( gp->vel, 0.0, 0.0, 0.0 );
sgSetVec3( gp.vel, 0.0, 0.0, 0.0 );
}
// Get the texture name and decide what ground type we have.
@ -100,208 +241,173 @@ FGGroundCache::extractGroundProperty( ssgLeaf* l )
SGPath dirPath(fullPath.dir());
string category = dirPath.file();
SG_LOG(SG_FLIGHT,SG_INFO,
"New triangle in cache: " << category << " " << file );
if (category == "Runway")
gp->type = FGInterface::Solid;
gp.type = FGInterface::Solid;
else {
if (file == "asphault.rgb" || file == "airport.rgb")
gp->type = FGInterface::Solid;
gp.type = FGInterface::Solid;
else if (file == "water.rgb" || file == "water-lake.rgb")
gp->type = FGInterface::Water;
gp.type = FGInterface::Water;
else if (file == "forest.rgb" || file == "cropwood.rgb")
gp->type = FGInterface::Forest;
gp.type = FGInterface::Forest;
}
}
return gp;
}
// Test if the line given by the point on the line pt_on_line and the
// line direction dir intersects the sphere sp.
// Adapted from plib.
static bool
sgIsectSphereInfLine(const sgSphere *sp,
const sgVec3 pt_on_line, const sgVec3 dir)
{
sgVec3 r ;
sgSubVec3 ( r, sp->getCenter(), pt_on_line ) ;
SGfloat projectedDistance = sgScalarProductVec3(r, dir);
SGfloat dist = sgScalarProductVec3 ( r, r ) -
projectedDistance * projectedDistance;
SGfloat radius = sp->getRadius();
return dist < radius*radius;
}
void
FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia,
const sgMat4 xform)
{
// Extract data from the leaf which is just copied.
ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices();
ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals();
// Create a new leaf.
ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 0, 0, ia );
// Clones data ...
vtxa->removeUnusedVertices();
// Apply transform. We won't store transforms in our cache.
vtxa->transform( xform );
// Check for magic texture names object names and such ...
GroundProperty *gp = extractGroundProperty( l );
// Assertation???
if ( !gp ) {
cerr << "Newly created cache leaf where userdata is not a Ground property!" << endl;
}
vtxa->setUserData( gp );
vtxa->setCullFace( l->getCullFace() );
// Finally append to cache.
cache_root.addKid((ssgEntity*)vtxa);
}
void
FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform,
FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
ssgLeaf *l)
{
ssgIndexArray *ia = 0;
GroundProperty gp = extractGroundProperty(l);
// Lines must have special meanings.
// Wires and catapults are done with lines.
int nl = l->getNumLines();
for (int i = 0; i < nl; ++i) {
sgSphere tmp;
sgdSphere sphere;
sphere.empty();
sgdVec3 ends[2];
short v[2];
l->getLine(i, v, v+1 );
for (int k=0; k<2; ++k)
tmp.extend( l->getVertex( v[k] ) );
tmp.orthoXform(xform);
if (wsp->intersects( &tmp )) {
if (ia == 0)
ia = new ssgIndexArray();
ia->add( v[0] );
ia->add( v[1] );
for (int k=0; k<2; ++k) {
sgdSetVec3(ends[k], l->getVertex(v[k]));
sgdXformPnt3(ends[k], xform);
sphere.extend(ends[k]);
}
if (wsp->intersects( &sphere )) {
if (gp.type == FGInterface::Wire) {
Wire wire;
sgdCopyVec3(wire.ends[0], ends[0]);
sgdCopyVec3(wire.ends[1], ends[1]);
sgdSetVec3(wire.velocity, gp.vel);
wire.wire_id = gp.wire_id;
wires.push_back(wire);
}
if (gp.type == FGInterface::Catapult) {
Catapult cat;
sgdCopyVec3(cat.start, ends[0]);
sgdCopyVec3(cat.end, ends[1]);
sgdSetVec3(cat.velocity, gp.vel);
catapults.push_back(cat);
}
}
}
if (!ia)
return;
addAndFlattenLeaf(GL_LINES, l, ia, xform);
}
void
FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform,
bool sphIsec, sgVec3 down, ssgLeaf *l)
FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
const sgdMat4 xform, bool sphIsec,
sgdVec3 down, ssgLeaf *l)
{
ssgIndexArray *ia = 0;
GroundProperty gp = extractGroundProperty(l);
int nt = l->getNumTriangles();
for (int i = 0; i < nt; ++i) {
// Build up a sphere around that particular triangle-
sgSphere tmp;
Triangle t;
t.sphere.empty();
short v[3];
l->getTriangle(i, v, v+1, v+2 );
for (int k=0; k<3; ++k)
tmp.extend( l->getVertex( v[k] ) );
tmp.orthoXform(xform);
l->getTriangle(i, &v[0], &v[1], &v[2]);
for (int k = 0; k < 3; ++k) {
sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
sgdXformPnt3(t.vertices[k], xform);
t.sphere.extend(t.vertices[k]);
}
sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
SGDfloat dot = sgdScalarProductVec3(down, t.plane);
if (dot > 0) {
if (!l->getCullFace()) {
// Surface points downwards, ignore for altitude computations.
continue;
} else
sgdScaleVec4( t.plane, -1 );
}
// Check if the sphere around the vehicle intersects the sphere
// around that triangle. If so, put that triangle into the cache.
if (sphIsec && sp->intersects( &tmp )) {
if (ia == 0)
ia = new ssgIndexArray();
ia->add( v[0] );
ia->add( v[1] );
ia->add( v[2] );
if (sphIsec && sp->intersects(&t.sphere)) {
sgdSetVec3(t.velocity, gp.vel);
t.type = gp.type;
triangles.push_back(t);
}
// In case the cache is empty, we still provide agl computations.
// But then we use the old way of having a fixed elevation value for
// the whole lifetime of this cache.
if ( sgIsectSphereInfLine(&tmp, sp->getCenter(), down) ) {
sgVec3 tri[3];
for (int k=0; k<3; ++k) {
sgCopyVec3( tri[k], l->getVertex( v[k] ) );
sgXformPnt3( tri[k], xform );
}
sgVec4 plane;
sgMakePlane( plane, tri[0], tri[1], tri[2] );
sgVec3 ac_cent;
sgCopyVec3(ac_cent, sp->getCenter());
sgVec3 dst;
sgIsectInfLinePlane( dst, ac_cent, down, plane );
if ( sgPointInTriangle ( dst, tri ) ) {
if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
sgdVec3 tmp;
sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
sgdVec3 isectpoint;
if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
fgdPointInTriangle( isectpoint, t.vertices ) ) {
found_ground = true;
sgdVec3 ddst;
sgdSetVec3(ddst, dst);
sgdAddVec3(ddst, cache_center);
double this_radius = sgdLengthVec3(ddst);
sgdAddVec3(isectpoint, cache_center);
double this_radius = sgdLengthVec3(isectpoint);
if (ground_radius < this_radius)
ground_radius = this_radius;
}
}
}
if (!ia)
return;
}
inline void
FGGroundCache::velocityTransformTriangle(double dt,
FGGroundCache::Triangle& dst,
const FGGroundCache::Triangle& src)
{
sgdCopyVec3(dst.vertices[0], src.vertices[0]);
sgdCopyVec3(dst.vertices[1], src.vertices[1]);
sgdCopyVec3(dst.vertices[2], src.vertices[2]);
sgdCopyVec4(dst.plane, src.plane);
addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform);
}
sgdCopyVec3(dst.sphere.center, src.sphere.center);
dst.sphere.radius = src.sphere.radius;
// Here is the point where rotation should be handled
void
FGGroundCache::extractCacheRelativeVertex(double t, ssgVtxArray *va,
GroundProperty *gp,
short i, sgVec3 rel_pos,
sgdVec3 wgs84_vel)
{
sgCopyVec3( rel_pos, va->getVertex( i ) );
sgAddScaledVec3( rel_pos, gp->vel, t );
sgdCopyVec3(dst.velocity, src.velocity);
// Set velocity.
sgdSetVec3( wgs84_vel, gp->vel );
dst.type = src.type;
if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
sgdAddScaledVec3(dst.vertices[0], src.velocity, dt);
sgdAddScaledVec3(dst.vertices[1], src.velocity, dt);
sgdAddScaledVec3(dst.vertices[2], src.velocity, dt);
dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, src.velocity);
sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
}
}
void
FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va,
GroundProperty *gp, short i,
sgdVec3 wgs84_pos, sgdVec3 wgs84_vel)
{
sgVec3 rel_pos;
extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel);
sgdSetVec3( wgs84_pos, rel_pos );
sgdAddVec3( wgs84_pos, cache_center );
}
void
FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
sgSphere* sp, sgVec3 down, sgSphere* wsp)
FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
{
// Travel through all kids.
ssgEntity *e;
for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
if ( !( e->getTraversalMask() & SSGTRAV_HOT) )
if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
continue;
if ( e->getBSphere()->isEmpty() )
continue;
// Wee need to check further if either the sphere around the branch
// We need to check further if either the sphere around the branch
// intersects the sphere around the aircraft or the line downwards from
// the aircraft intersects the branchs sphere.
sgSphere esphere = *(e->getBSphere());
sgdSphere esphere;
sgdSetVec3(esphere.center, e->getBSphere()->center);
esphere.radius = e->getBSphere()->radius;
esphere.orthoXform(xform);
bool wspIsec = wsp->intersects(&esphere);
bool downIsec = sgIsectSphereInfLine(&esphere, sp->getCenter(), down);
bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
if (!wspIsec && !downIsec)
continue;
// For branches collect up the transforms to reach that branch and
// call cache_fill recursively.
if ( e->isAKindOf( ssgTypeBranch() ) ) {
@ -313,8 +419,9 @@ FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
sgMakeIdentMat4( xform2 );
ssgTransform *t = (ssgTransform*)b;
t->getTransform( xform2 );
sgPostMultMat4( xform2, xform );
cache_fill( b, xform2, sp, down, wsp );
sgdMat4 xform3;
fgMultMat4(xform3, xform, xform2);
cache_fill( b, xform3, sp, down, wsp );
} else
cache_fill( b, xform, sp, down, wsp );
}
@ -340,16 +447,14 @@ FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
bool
FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
double rad)
double rad)
{
Point3D old_cntr = globals->get_scenery()->get_center();
Point3D cntr(pt[0], pt[1], pt[2]);
globals->get_scenery()->set_center( cntr );
// Empty cache.
cache_root.removeAllKids();
ground_radius = 0.0;
found_ground = false;
triangles.resize(0);
catapults.resize(0);
wires.resize(0);
// Store the parameters we used to build up that cache.
sgdCopyVec3(reference_wgs84_point, pt);
@ -357,35 +462,38 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
// Store the time reference used to compute movements of moving triangles.
cache_ref_time = ref_time;
// Decide where we put the scenery center.
Point3D old_cntr = globals->get_scenery()->get_center();
Point3D cntr(pt[0], pt[1], pt[2]);
// Only move the cache center if it is unaccaptable far away.
if (40*40 < old_cntr.distance3Dsquared(cntr))
globals->get_scenery()->set_center(cntr);
else
cntr = old_cntr;
// The center of the cache.
sgdCopyVec3(cache_center, pt);
sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
sgVec3 zero;
sgZeroVec3(zero);
sgdVec3 ptoff;
sgdSubVec3(ptoff, pt, cache_center);
// Prepare sphere around the aircraft.
sgSphere acSphere;
sgdSphere acSphere;
acSphere.setRadius(rad);
acSphere.setCenter(zero);
acSphere.setCenter(ptoff);
// Prepare bigger sphere around the aircraft.
// This one is required for reliably finding wires we have caught but
// have already left the hopefully smaller sphere for the ground reactions.
const double max_wire_dist = 300.0;
sgSphere wireSphere;
sgdSphere wireSphere;
wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
wireSphere.setCenter(zero);
wireSphere.setCenter(ptoff);
// Down vector. Is used for croase agl computations when we are far enough
// from ground that we have an empty cache.
sgVec3 down;
sgSetVec3(down, -pt[0], -pt[1], -pt[2]);
sgNormalizeVec3(down);
// We need the offset to the scenery scenery center.
sgdVec3 doffset;
Point3D psc = globals->get_scenery()->get_center();
sgdSetVec3(doffset, psc[0], psc[1], psc[2]);
sgdSubVec3(doffset, doffset, pt);
sgdVec3 down;
sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
sgdNormalizeVec3(down);
// We collaps all transforms we need to reach a particular leaf.
// The leafs itself will be then transformed later.
@ -394,10 +502,8 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
// we will later store a speed in the GroundType class. We can then apply
// some translations to that nodes according to the time which has passed
// compared to that snapshot.
sgVec3 offset;
sgSetVec3(offset, doffset[0], doffset[1], doffset[2]);
sgMat4 xform;
sgMakeTransMat4(xform, offset);
sgdMat4 xform;
sgdMakeIdentMat4( xform );
// Walk the scene graph and extract solid ground triangles and carrier data.
@ -406,7 +512,9 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
// some stats
SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad
<< ", # leafs = " << cache_root.getNumKids()
<< ", # triangles = " << triangles.size()
<< ", # wires = " << wires.size()
<< ", # catapults = " << catapults.size()
<< ", ground_radius = " << ground_radius );
// If the ground radius is still below 5e6 meters, then we do not yet have
@ -416,7 +524,8 @@ FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
"without any scenery below the aircraft" );
globals->get_scenery()->set_center( old_cntr );
if (cntr != old_cntr)
globals->get_scenery()->set_center(old_cntr);
return found_ground;
}
@ -432,7 +541,7 @@ FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
double
FGGroundCache::get_cat(double t, const double dpt[3],
double end[2][3], double vel[2][3])
double end[2][3], double vel[2][3])
{
// start with a distance of 1e10 meters...
double dist = 1e10;
@ -440,40 +549,29 @@ FGGroundCache::get_cat(double t, const double dpt[3],
// Time difference to the reference time.
t -= cache_ref_time;
// We know that we have a flat cache ...
ssgEntity *e;
for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
// We just know that, because we build that ourselfs ...
ssgVtxArray *va = (ssgVtxArray *)e;
// Only lines are interresting ...
if (va->getPrimitiveType() != GL_LINES)
continue;
GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
// Check if we have a catapult ...
if ( gp->type != FGInterface::Catapult )
continue;
size_t sz = catapults.size();
for (size_t i = 0; i < sz; ++i) {
sgdLineSegment3 ls;
sgdCopyVec3(ls.a, catapults[i].start);
sgdCopyVec3(ls.b, catapults[i].end);
int nl = va->getNumLines();
for (int i=0; i < nl; ++i) {
sgdLineSegment3 ls;
sgdVec3 lsVel[2];
short vi[2];
va->getLine(i, vi, vi+1 );
extractWgs84Vertex(t, va, gp, vi[0], ls.a, lsVel[0]);
extractWgs84Vertex(t, va, gp, vi[1], ls.b, lsVel[1]);
double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
if (this_dist < dist) {
dist = this_dist;
sgdAddVec3(ls.a, cache_center);
sgdAddVec3(ls.b, cache_center);
sgdAddScaledVec3(ls.a, catapults[i].velocity, t);
sgdAddScaledVec3(ls.b, catapults[i].velocity, t);
double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
if (this_dist < dist) {
SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
<< this_dist << " meters away");
dist = this_dist;
// end[0] is the end where the cat starts.
// end[1] is the end where the cat ends.
// The carrier code takes care of that ordering.
sgdCopyVec3( end[0], ls.a );
sgdCopyVec3( end[1], ls.b );
sgdCopyVec3( vel[0], lsVel[0] );
sgdCopyVec3( vel[1], lsVel[1] );
}
// The carrier code takes care of that ordering.
sgdCopyVec3( end[0], ls.a );
sgdCopyVec3( end[1], ls.b );
sgdCopyVec3( vel[0], catapults[i].velocity );
sgdCopyVec3( vel[1], catapults[i].velocity );
}
}
@ -501,85 +599,49 @@ FGGroundCache::get_agl(double t, const double dpt[3],
t -= cache_ref_time;
// The double valued point we start to search for intersection.
sgdVec3 tmp;
sgdSubVec3( tmp, dpt, cache_center );
sgVec3 pt;
sgSetVec3( pt, tmp );
sgdVec3 pt;
sgdSubVec3( pt, dpt, cache_center );
// The search direction
sgVec3 dir;
sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
sgdVec3 dir;
sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
// Initialize to something sensible
double sqdist = DBL_MAX;
// We know that we have a flat cache ...
// We just know that, because we build that ourselfs ...
ssgEntity *e;
for ( e = cache_root.getKid(0) ; e != NULL ; e = cache_root.getNextKid() ) {
// We just know that, because we build that ourselfs ...
ssgVtxArray *va = (ssgVtxArray *)e;
// AGL computations are done with triangle/surface leafs.
if (va->getPrimitiveType() != GL_TRIANGLES)
size_t sz = triangles.size();
for (size_t i = 0; i < sz; ++i) {
Triangle triangle;
velocityTransformTriangle(t, triangle, triangles[i]);
if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
continue;
ssgBase *gpb = va->getUserData();
// Assertation???
if ( !gpb ) {
cerr << "Found cache leaf without userdata!" << endl;
continue;
}
GroundProperty *gp = static_cast<GroundProperty*>(gpb);
int nt = va->getNumTriangles();
for (int i=0; i < nt; ++i) {
short vi[3];
va->getTriangle( i, vi, vi+1, vi+2 );
sgVec3 tri[3];
sgdVec3 dvel[3];
for (int k=0; k<3; ++k)
extractCacheRelativeVertex(t, va, gp, vi[k], tri[k], dvel[k]);
sgVec4 plane;
sgMakePlane( plane, tri[0], tri[1], tri[2] );
// Check for intersection.
sgVec3 isecpoint;
if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
sgPointInTriangle3( isecpoint, tri ) ) {
// Only accept surfaces with the normal pointing upwards.
// For double sided surfaces flip the normal in this case.
float dirDot = sgScalarProductVec3(plane, dir);
if ( dirDot >= 0 && va->getCullFace() == 1 ) {
sgScaleVec4( plane, -1 );
dirDot = -dirDot;
}
// Check for intersection.
sgdVec3 isecpoint;
if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
// Check for the closest intersection point.
// FIXME: is this the right one?
double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
if ( newSqdist < sqdist && dirDot < 0 ) {
sqdist = newSqdist;
ret = true;
// Save the new potential intersection point.
sgdSetVec3( contact, isecpoint );
sgdAddVec3( contact, cache_center );
// The first three values in the vector are the plane normal.
sgdSetVec3( normal, plane );
// Remormalize that as double, else it *can* have surprising effects
// when used as plane normal together with a 6000000m offset in a
// plane equation.
sgdNormalizeVec3( normal );
// The velocity wrt earth.
/// FIXME: only true for non rotating objects!!!!
sgdCopyVec3( vel, dvel[0] );
// Save the ground type.
*type = gp->type;
// FIXME: figure out how to get that sign ...
// Check for the closest intersection point.
// FIXME: is this the right one?
SGDfloat newSqdist = sgdDistanceSquaredVec3( isecpoint, pt );
if ( newSqdist < sqdist ) {
sqdist = newSqdist;
ret = true;
// Save the new potential intersection point.
sgdCopyVec3( contact, isecpoint );
sgdAddVec3( contact, cache_center );
// The first three values in the vector are the plane normal.
sgdCopyVec3( normal, triangle.plane );
// The velocity wrt earth.
/// FIXME: only true for non rotating objects!!!!
sgdCopyVec3( vel, triangle.velocity );
// Save the ground type.
*type = triangle.type;
// FIXME: figure out how to get that sign ...
// *agl = sqrt(sqdist);
*agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
*agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
// *loadCapacity = DBL_MAX;
// *frictionFactor = 1.0;
}
}
}
}
@ -607,75 +669,55 @@ FGGroundCache::get_agl(double t, const double dpt[3],
return ret;
}
bool FGGroundCache::caught_wire(double t, const double cpt[4][3])
bool FGGroundCache::caught_wire(double t, const double pt[4][3])
{
bool ret = false;
size_t sz = wires.size();
if (sz == 0)
return false;
// Time difference to the reference time.
t -= cache_ref_time;
bool firsttime = true;
sgVec4 plane[2];
sgVec3 tri[2][3];
// Build the two triangles spanning the area where the hook has moved
// during the past step.
sgdVec4 plane[2];
sgdVec3 tri[2][3];
sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
sgdCopyVec3( tri[0][0], pt[0] );
sgdCopyVec3( tri[0][1], pt[1] );
sgdCopyVec3( tri[0][2], pt[2] );
sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
sgdCopyVec3( tri[1][0], pt[0] );
sgdCopyVec3( tri[1][1], pt[2] );
sgdCopyVec3( tri[1][2], pt[3] );
// We know that we have a flat cache ...
ssgEntity *e;
for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
// We just know that, because we build that ourselfs ...
ssgVtxArray *va = (ssgVtxArray *)e;
// Only lines are interresting ...
if (va->getPrimitiveType() != GL_LINES)
continue;
GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
// Check if we have a catapult ...
if ( gp->type != FGInterface::Wire )
continue;
// Intersect the wire lines with each of these triangles.
// You have cautght a wire if they intersect.
for (size_t i = 0; i < sz; ++i) {
sgdVec3 le[2];
sgdCopyVec3(le[0], wires[i].ends[0]);
sgdCopyVec3(le[1], wires[i].ends[1]);
// Lazy compute the values required for intersectiion tests.
// Since we normally do not have wires in the cache this is a
// huge benefit.
if (firsttime) {
firsttime = false;
sgVec3 pt[4];
for (int k=0; k<4; ++k) {
sgdVec3 tmp;
sgdSubVec3( tmp, cpt[k], cache_center );
sgSetVec3( pt[k], tmp );
}
sgMakePlane( plane[0], pt[0], pt[1], pt[2] );
sgCopyVec3( tri[0][0], pt[0] );
sgCopyVec3( tri[0][1], pt[1] );
sgCopyVec3( tri[0][2], pt[2] );
sgMakePlane( plane[1], pt[0], pt[2], pt[3] );
sgCopyVec3( tri[1][0], pt[0] );
sgCopyVec3( tri[1][1], pt[2] );
sgCopyVec3( tri[1][2], pt[3] );
}
sgdAddVec3(le[0], cache_center);
sgdAddVec3(le[1], cache_center);
sgdAddScaledVec3(le[0], wires[i].velocity, t);
sgdAddScaledVec3(le[1], wires[i].velocity, t);
int nl = va->getNumLines();
for (int i=0; i < nl; ++i) {
short vi[2];
va->getLine(i, vi, vi+1 );
sgVec3 le[2];
sgdVec3 dummy;
extractCacheRelativeVertex(t, va, gp, vi[0], le[0], dummy);
extractCacheRelativeVertex(t, va, gp, vi[1], le[1], dummy);
for (int k=0; k<2; ++k) {
sgVec3 isecpoint;
float isecval = sgIsectLinesegPlane( isecpoint, le[0], le[1], plane[k] );
if ( 0.0 <= isecval && isecval <= 1.0 &&
sgPointInTriangle( isecpoint, tri[k] ) ) {
// Store the wire id.
wire_id = gp->wire_id;
ret = true;
}
for (int k=0; k<2; ++k) {
sgdVec3 isecpoint;
double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
if ( 0.0 <= isecval && isecval <= 1.0 &&
sgdPointInTriangle( isecpoint, tri[k] ) ) {
SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
// Store the wire id.
wire_id = wires[i].wire_id;
return true;
}
}
}
return ret;
return false;
}
bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
@ -684,36 +726,29 @@ bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
if (wire_id < 0)
return false;
bool ret = false;
// Time difference to th reference time.
// Time difference to the reference time.
t -= cache_ref_time;
// We know that we have a flat cache ...
ssgEntity *e;
for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
// We just know that, because we build that ourselfs ...
ssgVtxArray *va = (ssgVtxArray *)e;
// Only lines are interresting ...
if (va->getPrimitiveType() != GL_LINES)
continue;
GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
// Check if we have a catapult ...
if ( gp->type != FGInterface::Wire )
continue;
if ( gp->wire_id != wire_id )
continue;
// Get the line ends, that are the wire endpoints.
short vi[2];
va->getLine(0, vi, vi+1 );
extractWgs84Vertex(t, va, gp, vi[0], end[0], vel[0]);
extractWgs84Vertex(t, va, gp, vi[1], end[1], vel[1]);
ret = true;
// Search for the wire with the matching wire id.
size_t sz = wires.size();
for (size_t i = 0; i < sz; ++i) {
if (wires[i].wire_id == wire_id) {
sgdCopyVec3(end[0], wires[i].ends[0]);
sgdCopyVec3(end[1], wires[i].ends[1]);
sgdAddVec3(end[0], cache_center);
sgdAddVec3(end[1], cache_center);
sgdAddScaledVec3(end[0], wires[i].velocity, t);
sgdAddScaledVec3(end[1], wires[i].velocity, t);
sgdCopyVec3(vel[0], wires[i].velocity);
sgdCopyVec3(vel[1], wires[i].velocity);
return true;
}
}
return ret;
return false;
}
void FGGroundCache::release_wire(void)

70
src/FDM/groundcache.hxx
View file

@ -82,9 +82,30 @@ public:
void release_wire(void);
private:
// Holds the private ground triangle cache ...
// The surface cache itself.
ssgRoot cache_root;
struct Triangle {
// The edge vertices.
sgdVec3 vertices[3];
// The surface normal.
sgdVec4 plane;
// The bounding shpere.
sgdSphere sphere;
// The linear velocity.
sgdVec3 velocity;
// Ground type
int type;
};
struct Catapult {
sgdVec3 start;
sgdVec3 end;
sgdVec3 velocity;
};
struct Wire {
sgdVec3 ends[2];
sgdVec3 velocity;
int wire_id;
};
// The center of the cache.
sgdVec3 cache_center;
// Approximate ground radius.
@ -96,6 +117,11 @@ private:
// The wire identifier to track.
int wire_id;
// Containers which hold all the essential information about this cache.
std::vector<Triangle> triangles;
std::vector<Catapult> catapults;
std::vector<Wire> wires;
// The point and radius where the cache is built around.
// That are the arguments that were given to prepare_ground_cache.
sgdVec3 reference_wgs84_point;
@ -104,13 +130,18 @@ private:
// Fills the environment cache with everything inside the sphere sp.
void cache_fill(ssgBranch *branch, sgMat4 xform,
sgSphere* sp, sgVec3 down, sgSphere* wsp);
void cache_fill(ssgBranch *branch, sgdMat4 xform,
sgdSphere* sp, sgdVec3 down, sgdSphere* wsp);
// compute the ground property of this leaf.
void putSurfaceLeafIntoCache(const sgdSphere *sp, const sgdMat4 xform,
bool sphIsec, sgdVec3 down, ssgLeaf *l);
void putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
ssgLeaf *l);
// Helper class to hold some properties of the ground triangle.
class GroundProperty
: public ssgBase {
public:
struct GroundProperty {
GroundProperty() : type(0) {}
int type;
int wire_id;
@ -120,28 +151,11 @@ private:
};
// compute the ground property of this leaf.
static GroundProperty *extractGroundProperty( ssgLeaf* leaf );
static GroundProperty extractGroundProperty( ssgLeaf* leaf );
// compute the ground property of this leaf.
void putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform,
bool sphIsec, sgVec3 down, ssgLeaf *l);
void putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform,
ssgLeaf *l);
void addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia,
const sgMat4 xform);
void extractCacheRelativeVertex(double t, ssgVtxArray *va,
GroundProperty *gp,
short i, sgVec3 rel_pos,
sgdVec3 wgs84_vel);
void extractWgs84Vertex(double t, ssgVtxArray *va,
GroundProperty *gp, short i,
sgdVec3 wgs84_pos, sgdVec3 wgs84_vel);
static void velocityTransformTriangle(double dt, Triangle& dst,
const Triangle& src);
};
#endif

30
src/Scenery/hitlist.cxx
View file

@ -124,20 +124,23 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
{
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;
@ -181,27 +184,30 @@ static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
}
// 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 - side1*eps - y1) * 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 - side1*eps - y1) * 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 - side1*eps - y1) * tmpn);
if ( side1 != side2 ) {
// printf("failed side 3 check\n");
return false;

21
src/Scenery/tileentry.cxx
View file

@ -940,14 +940,6 @@ FGTileEntry::load( const string_list &path_list, bool is_base )
sgdSetVec3( sgdTrans, center.x(), center.y(), center.z() );
terra_transform->setTransform( sgdTrans );
sgdVec3 sgdCenter;
Point3D p = globals->get_scenery()->get_center();
sgdSetVec3( sgdCenter, p.x(), p.y(), p.z() );
terra_transform->setSceneryCenter( sgdCenter );
globals->get_scenery()->register_placement_transform(terra_transform);
// terrain->addKid( terra_transform );
// Add ground lights to scene graph if any exist
gnd_lights_transform = NULL;
gnd_lights_range = NULL;
@ -970,29 +962,21 @@ FGTileEntry::load( const string_list &path_list, bool is_base )
gnd_lights_range->addKid( gnd_lights_brightness );
gnd_lights_transform->addKid( gnd_lights_range );
gnd_lights_transform->setTransform( sgdTrans );
gnd_lights_transform->setSceneryCenter( sgdCenter );
globals->get_scenery()->register_placement_transform(gnd_lights_transform);
}
// Update vasi lights transform
if ( vasi_lights_transform->getNumKids() > 0 ) {
vasi_lights_transform->setTransform( sgdTrans );
vasi_lights_transform->setSceneryCenter( sgdCenter );
globals->get_scenery()->register_placement_transform(vasi_lights_transform);
}
// Update runway lights transform
if ( rwy_lights_transform->getNumKids() > 0 ) {
rwy_lights_transform->setTransform( sgdTrans );
rwy_lights_transform->setSceneryCenter( sgdCenter );
globals->get_scenery()->register_placement_transform(rwy_lights_transform);
}
// Update taxi lights transform
if ( taxi_lights_transform->getNumKids() > 0 ) {
taxi_lights_transform->setTransform( sgdTrans );
taxi_lights_transform->setSceneryCenter( sgdCenter );
globals->get_scenery()->register_placement_transform(taxi_lights_transform);
}
}
@ -1027,6 +1011,7 @@ FGTileEntry::add_ssg_nodes( ssgBranch *terrain_branch,
terra_transform->ref();
terrain_branch->addKid( terra_transform );
globals->get_scenery()->register_placement_transform(terra_transform);
SG_LOG( SG_TERRAIN, SG_DEBUG,
"connected a tile into scene graph. terra_transform = "
@ -1039,6 +1024,7 @@ FGTileEntry::add_ssg_nodes( ssgBranch *terrain_branch,
// having ssg try to free the memory.
gnd_lights_transform->ref();
gnd_lights_branch->addKid( gnd_lights_transform );
globals->get_scenery()->register_placement_transform(gnd_lights_transform);
}
if ( vasi_lights_transform != NULL ) {
@ -1046,6 +1032,7 @@ FGTileEntry::add_ssg_nodes( ssgBranch *terrain_branch,
// having ssg try to free the memory.
vasi_lights_selector->ref();
vasi_lights_selector->addKid( vasi_lights_transform );
globals->get_scenery()->register_placement_transform(vasi_lights_transform);
vasi_lights_branch->addKid( vasi_lights_selector );
}
@ -1054,6 +1041,7 @@ FGTileEntry::add_ssg_nodes( ssgBranch *terrain_branch,
// having ssg try to free the memory.
rwy_lights_selector->ref();
rwy_lights_selector->addKid( rwy_lights_transform );
globals->get_scenery()->register_placement_transform(rwy_lights_transform);
rwy_lights_branch->addKid( rwy_lights_selector );
}
@ -1062,6 +1050,7 @@ FGTileEntry::add_ssg_nodes( ssgBranch *terrain_branch,
// having ssg try to free the memory.
taxi_lights_selector->ref();
taxi_lights_selector->addKid( taxi_lights_transform );
globals->get_scenery()->register_placement_transform(taxi_lights_transform);
taxi_lights_branch->addKid( taxi_lights_selector );
}