From 7159e318e10427d9f61556b2fe965d445496ea4e Mon Sep 17 00:00:00 2001 From: ehofman Date: Mon, 22 Nov 2004 10:10:33 +0000 Subject: [PATCH] =?utf8?q?Mathias=20Fr=F6hlich:?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit I have now split out the ground cache functions into src/FDM/groundcache.[ch]xx Attached are the two files and the patch to integrate that cache into FGInterface. The code is nowhere used at the moment, the fdm's need to be updated to use that ground cache. The JSBSim-dropin.tar.gz from Martins ftp server does this for example. The carrier's scenegraph is not yet processed to be visible for ground intersection testing. So the only benefit up to now is that the api is set up. Using this I can put the changes to make JSBSim work with that into JSBSim's cvs. Also I aim to provide Andy a patch to make use of that with YASim. --- src/FDM/Makefile.am | 1 + src/FDM/SP/.cvsignore | 3 + src/FDM/flight.cxx | 125 +++++++- src/FDM/flight.hxx | 79 ++++- src/FDM/groundcache.cxx | 682 ++++++++++++++++++++++++++++++++++++++++ src/FDM/groundcache.hxx | 144 +++++++++ src/Scenery/tilemgr.cxx | 3 + src/Scenery/tilemgr.hxx | 5 + 8 files changed, 1031 insertions(+), 11 deletions(-) create mode 100644 src/FDM/SP/.cvsignore create mode 100644 src/FDM/groundcache.cxx create mode 100644 src/FDM/groundcache.hxx diff --git a/src/FDM/Makefile.am b/src/FDM/Makefile.am index e27dfc431..dfb7f278e 100644 --- a/src/FDM/Makefile.am +++ b/src/FDM/Makefile.am @@ -12,6 +12,7 @@ noinst_LIBRARIES = libFlight.a libFlight_a_SOURCES = \ Balloon.cxx Balloon.h \ flight.cxx flight.hxx \ + groundcache.cxx groundcache.hxx \ MagicCarpet.cxx MagicCarpet.hxx \ UFO.cxx UFO.hxx \ NullFDM.cxx NullFDM.hxx diff --git a/src/FDM/SP/.cvsignore b/src/FDM/SP/.cvsignore new file mode 100644 index 000000000..e99558847 --- /dev/null +++ b/src/FDM/SP/.cvsignore @@ -0,0 +1,3 @@ +.deps +Makefile +Makefile.in diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index a81ed437c..86324a831 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -32,9 +32,9 @@ #include #include -#include #include
#include
+#include #include "flight.hxx" @@ -789,6 +789,129 @@ void FGInterface::_busdump(void) { SG_LOG(SG_FLIGHT,SG_INFO,"altitude_agl: " << altitude_agl ); } +bool +FGInterface::prepare_ground_cache_m(double ref_time, const double pt[3], + double rad) +{ + return ground_cache.prepare_ground_cache(ref_time, pt, rad); +} + +bool FGInterface::prepare_ground_cache_ft(double ref_time, const double pt[3], + double rad) +{ + // Convert units and do the real work. + sgdVec3 pt_ft; + sgdScaleVec3( pt_ft, pt, SG_FEET_TO_METER ); + return ground_cache.prepare_ground_cache(ref_time, pt_ft, rad*SG_FEET_TO_METER); +} + +bool +FGInterface::is_valid_m(double *ref_time, double pt[3], double *rad) +{ + return ground_cache.is_valid(ref_time, pt, rad); +} + +bool FGInterface::is_valid_ft(double *ref_time, double pt[3], double *rad) +{ + // Convert units and do the real work. + bool found_ground = ground_cache.is_valid(ref_time, pt, rad); + sgdScaleVec3(pt, SG_METER_TO_FEET); + *rad *= SG_METER_TO_FEET; + return found_ground; +} + +double +FGInterface::get_cat_m(double t, const double pt[3], + double end[2][3], double vel[2][3]) +{ + return ground_cache.get_cat(t, pt, end, vel); +} + +double +FGInterface::get_cat_ft(double t, const double pt[3], + double end[2][3], double vel[2][3]) +{ + // Convert units and do the real work. + sgdVec3 pt_m; + sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER ); + double dist = ground_cache.get_cat(t, pt_m, end, vel); + for (int k=0; k<2; ++k) { + sgdScaleVec3( end[k], SG_METER_TO_FEET ); + sgdScaleVec3( vel[k], SG_METER_TO_FEET ); + } + return dist*SG_METER_TO_FEET; +} + +bool +FGInterface::get_agl_m(double t, const double pt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) +{ + return ground_cache.get_agl(t, pt, contact, normal, vel, type, + loadCapacity, frictionFactor, agl); +} + +bool +FGInterface::get_agl_ft(double t, const double pt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) +{ + // Convert units and do the real work. + sgdVec3 pt_m; + sgdScaleVec3( pt_m, pt, SG_FEET_TO_METER ); + bool ret = ground_cache.get_agl(t, pt_m, contact, normal, vel, + type, loadCapacity, frictionFactor, agl); + // Convert units back ... + sgdScaleVec3( contact, SG_METER_TO_FEET ); + sgdScaleVec3( vel, SG_METER_TO_FEET ); + *agl *= SG_METER_TO_FEET; + // FIXME: scale the load limit to something in the english unit system. + // Be careful with the DBL_MAX which is returned by default. + return ret; +} + +bool +FGInterface::caught_wire_m(double t, const double pt[4][3]) +{ + return ground_cache.caught_wire(t, pt); +} + +bool +FGInterface::caught_wire_ft(double t, const double pt[4][3]) +{ + // Convert units and do the real work. + double pt_m[4][3]; + for (int i=0; i<4; ++i) + sgdScaleVec3(pt_m[i], pt[i], SG_FEET_TO_METER); + + return ground_cache.caught_wire(t, pt_m); +} + +bool +FGInterface::get_wire_ends_m(double t, double end[2][3], double vel[2][3]) +{ + return ground_cache.get_wire_ends(t, end, vel); +} + +bool +FGInterface::get_wire_ends_ft(double t, double end[2][3], double vel[2][3]) +{ + // Convert units and do the real work. + bool ret = ground_cache.get_wire_ends(t, end, vel); + for (int k=0; k<2; ++k) { + sgdScaleVec3( end[k], SG_METER_TO_FEET ); + sgdScaleVec3( vel[k], SG_METER_TO_FEET ); + } + return ret; +} + +void +FGInterface::release_wire(void) +{ + ground_cache.release_wire(); +} void fgToggleFDMdataLogging(void) { cur_fdm_state->ToggleDataLogging(); diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index 3c200a295..36df52e42 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -90,14 +90,12 @@ #include #include #include +#include SG_USING_STD(list); SG_USING_STD(vector); SG_USING_STD(string); -class FGAircraftModel; - - typedef double FG_VECTOR_3[3]; // This is based heavily on LaRCsim/ls_generic.h @@ -223,6 +221,9 @@ private: // SGTimeStamp valid_stamp; // time this record is valid // SGTimeStamp next_stamp; // time this record is valid + // the ground cache object itself. + FGGroundCache ground_cache; + protected: int _calc_multiloop (double dt); @@ -1080,19 +1081,77 @@ public: // Note that currently this is the "same" value runway altitude... inline double get_ground_elev_ft() const { return runway_altitude; } -}; - -typedef list < FGInterface > fdm_state_list; -typedef fdm_state_list::iterator fdm_state_list_iterator; -typedef fdm_state_list::const_iterator const_fdm_state_list_iterator; + ////////////////////////////////////////////////////////////////////////// + // Ground handling routines + ////////////////////////////////////////////////////////////////////////// + enum GroundType { + Unknown = 0, //?? + Solid, // Whatever we will roll on with infinite load factor. + Forest, // Ground unsuitable for taxiing. + Water, // For the beaver ... + Catapult, // Carrier cats. + Wire // Carrier wires. + }; -extern FGInterface * cur_fdm_state; + // Prepare the ground cache for the wgs84 position pt_*. + // That is take all vertices in the ball with radius rad around the + // position given by the pt_* and store them in a local scene graph. + bool prepare_ground_cache_m(double ref_time, const double pt[3], + double rad); + bool prepare_ground_cache_ft(double ref_time, const double pt[3], + double rad); + + + // Returns true if the cache is valid. + // Also the reference time, point and radius values where the cache + // is valid for are returned. + bool is_valid_m(double *ref_time, double pt[3], double *rad); + bool is_valid_ft(double *ref_time, double pt[3], double *rad); + + // Return the nearest catapult to the given point + // pt in wgs84 coordinates. + double get_cat_m(double t, const double pt[3], + double end[2][3], double vel[2][3]); + double get_cat_ft(double t, const double pt[3], + double end[2][3], double vel[2][3]); + + // Return the altitude above ground below the wgs84 point pt + // Search for the nearest triangle to pt. + // Return ground properties like the ground type, the maximum load + // this kind kind of ground can carry, the friction factor between + // 0 and 1 which can be used to model lower friction with wet runways + // and finally the altitude above ground. + bool get_agl_m(double t, const double pt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl); + bool get_agl_ft(double t, const double pt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl); + + + // Return 1 if the hook intersects with a wire. + // That test is done by checking if the quad spanned by the points pt* + // intersects with the line representing the wire. + // If the wire is caught, the cache will trace this wires endpoints until + // the FDM calls release_wire(). + bool caught_wire_m(double t, const double pt[4][3]); + bool caught_wire_ft(double t, const double pt[4][3]); + + // Return the location and speed of the wire endpoints. + bool get_wire_ends_m(double t, double end[2][3], double vel[2][3]); + bool get_wire_ends_ft(double t, double end[2][3], double vel[2][3]); -// General interface to the flight model routines + // Tell the cache code that it does no longer need to care for + // the wire end position. + void release_wire(void); +}; +extern FGInterface * cur_fdm_state; // Toggle data logging on/off void fgToggleFDMdataLogging(void); diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx new file mode 100644 index 000000000..50eee1e60 --- /dev/null +++ b/src/FDM/groundcache.cxx @@ -0,0 +1,682 @@ +// groundcache.cxx -- carries a small subset of the scenegraph near the vehicle +// +// Written by Mathias Froehlich, started Nov 2004. +// +// Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#include + +#include + +#include +#include +#include + +#include
+#include +#include +#include + +#include "flight.hxx" +#include "groundcache.hxx" + +FGGroundCache::GroundProperty* +FGGroundCache::extractGroundProperty( ssgLeaf* l ) +{ + // FIXME: Do more ... + // Idea: have a get_globals() function which knows about that stuff. + // Or most propably read that from a configuration file, + // from property tree or whatever ... + + // Get ground dependent data. + GroundProperty *gp = new GroundProperty; + gp->wire_id = -1; + +// FGAICarrierHardware *ud = +// dynamic_cast(l->getUserData()); +// if (ud) { +// switch (ud->type) { +// case FGAICarrierHardware::Wire: +// gp->type = FGInterface::Wire; +// gp->wire_id = ud->id; +// break; +// case FGAICarrierHardware::Catapult: +// gp->type = FGInterface::Catapult; +// break; +// default: +// gp->type = FGInterface::Solid; +// break; +// } + +// // Copy the velocity from the carrier class. +// ud->carrier->getVelocityWrtEarth( gp->vel ); +// } + +// else { + + // Initialize velocity field. + sgSetVec3( gp->vel, 0.0, 0.0, 0.0 ); +// } + + // Get the texture name and decide what ground type we have. + ssgState *st = l->getState(); + if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) { + ssgSimpleState *ss = (ssgSimpleState*)st; + SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" ); + string file = fullPath.file(); + 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; + else { + if (file == "asphault.rgb" || file == "airport.rgb") + gp->type = FGInterface::Solid; + else if (file == "water.rgb" || file == "water-lake.rgb") + gp->type = FGInterface::Water; + else if (file == "forest.rgb" || file == "cropwood.rgb") + 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(); + // Create a new leaf. + ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 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 ... + vtxa->setUserData( extractGroundProperty( l ) ); + // Finally append to cache. + cache_root.addKid((ssgEntity*)vtxa); +} + +void +FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform, + ssgLeaf *l) +{ + ssgIndexArray *ia = 0; + + // 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; + 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] ); + } + } + if (!ia) + return; + + addAndFlattenLeaf(GL_LINES, l, ia, xform); +} + +void +FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform, + bool sphIsec, sgVec3 down, ssgLeaf *l) +{ + ssgIndexArray *ia = 0; + + int nt = l->getNumTriangles(); + for (int i = 0; i < nt; ++i) { + // Build up a sphere around that particular triangle- + sgSphere tmp; + 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); + + // 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] ); + } + + // 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 ) ) { + found_ground = true; + sgdVec3 ddst; + sgdSetVec3(ddst, dst); + sgdAddVec3(ddst, cache_center); + double this_radius = sgdLengthVec3(ddst); + if (ground_radius < this_radius) + ground_radius = this_radius; + } + } + } + if (!ia) + return; + + addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform); +} + +// 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 ); + + // Set velocity. + sgdSetVec3( wgs84_vel, gp->vel ); +} + +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) +{ + // Travel through all kids. + ssgEntity *e; + for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) { + if ( !( e->getTraversalMask() & SSGTRAV_HOT) ) + continue; + if ( e->getBSphere()->isEmpty() ) + continue; + + // Wee 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()); + esphere.orthoXform(xform); + bool wspIsec = wsp->intersects(&esphere); + bool downIsec = sgIsectSphereInfLine(&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() ) ) { + ssgBranch *b = (ssgBranch *)e; + if ( b->isAKindOf( ssgTypeTransform() ) ) { + // Collect up the transfors required to reach that part of + // the branch. + sgMat4 xform2; + sgMakeIdentMat4( xform2 ); + ssgTransform *t = (ssgTransform*)b; + t->getTransform( xform2 ); + sgPostMultMat4( xform2, xform ); + cache_fill( b, xform2, sp, down, wsp ); + } else + cache_fill( b, xform, sp, down, wsp ); + } + + // For leafs, check each triangle for intersection. + // This will minimize the number of vertices/triangles in the cache. + else if (e->isAKindOf(ssgTypeLeaf())) { + // Since we reach that leaf if we have an intersection with the + // most propably bigger wire/catapult cache sphere, we need to check + // that here, if the smaller cache for the surface has a chance for hits. + // Also, if the spheres do not intersect compute a croase agl value + // by following the line downwards originating at the aircraft. + bool spIsec = sp->intersects(&esphere); + putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e); + + // If we are here, we need to put all special hardware here into + // the cache. + if (wspIsec) + putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e); + } + } +} + +bool +FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3], + double rad) +{ + // Empty cache. + cache_root.removeAllKids(); + ground_radius = 0.0; + found_ground = false; + + // Store the parameters we used to build up that cache. + sgdCopyVec3(reference_wgs84_point, pt); + reference_vehicle_radius = rad; + // Store the time reference used to compute movements of moving triangles. + cache_ref_time = ref_time; + + // The center of the cache. + Point3D psc = globals->get_tile_mgr()->get_current_center(); + sgdSetVec3(cache_center, psc[0], psc[1], psc[2]); + + // Prepare sphere around the aircraft. + sgSphere acSphere; + acSphere.setRadius(rad); + + // Compute the postion of the aircraft relative to the scenery center. + sgdVec3 doffset; + sgdSubVec3( doffset, pt, cache_center ); + sgVec3 offset; + sgSetVec3( offset, doffset[0], doffset[1], doffset[2] ); + acSphere.setCenter( offset ); + + // 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; + wireSphere.setRadius( max_wire_dist < rad ? rad : max_wire_dist ); + wireSphere.setCenter( offset ); + + // 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 collaps all transforms we need to reach a particular leaf. + // The leafs itself will be then transformed later. + // So our cache is just flat. + // For leafs which are moving (carriers surface, etc ...) + // 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. + sgMat4 xform; + sgMakeIdentMat4(xform); + + // Walk the terrain branch for now. + ssgBranch *terrain = globals->get_scenery()->get_scene_graph(); + cache_fill(terrain, xform, &acSphere, down, &wireSphere); + + // some stats + SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad + << ", # leafs = " << cache_root.getNumKids() + << ", ground_radius = " << ground_radius ); + + // If the ground radius is still below 5e6 meters, then we do not yet have + // any scenery. + found_ground = found_ground && 5e6 < ground_radius; + if (!found_ground) + SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache " + "without any scenery below the aircraft" ); + + return found_ground; +} + +bool +FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad) +{ + sgdCopyVec3(pt, reference_wgs84_point); + *rad = reference_vehicle_radius; + *ref_time = cache_ref_time; + return found_ground; +} + +double +FGGroundCache::get_cat(double t, const double dpt[3], + double end[2][3], double vel[2][3]) +{ + // start with a distance of 1e10 meters... + double dist = 1e10; + + // 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 = dynamic_cast(va->getUserData()); + // Assertation??? + if ( !gp ) + continue; + // Check if we have a catapult ... + if ( gp->type != FGInterface::Catapult ) + continue; + + 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; + + // 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] ); + } + } + } + + // At the end take the root, we only computed squared distances ... + return sqrt(dist); +} + +bool +FGGroundCache::get_agl(double t, const double dpt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl) +{ + bool ret = false; + + *type = FGInterface::Unknown; +// *agl = 0.0; + *loadCapacity = DBL_MAX; + *frictionFactor = 1.0; + sgdSetVec3( vel, 0.0, 0.0, 0.0 ); + sgdSetVec3( contact, 0.0, 0.0, 0.0 ); + sgdSetVec3( normal, 0.0, 0.0, 0.0 ); + + // Time difference to th reference time. + 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 ); + + // The search direction + sgVec3 dir; + sgSetVec3( 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) + continue; + GroundProperty *gp = dynamic_cast(va->getUserData()); + // Assertation??? + if ( !gp ) + continue; + + 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 ) ) { + // Check for the closest intersection point. + // FIXME: is this the right one? + double newSqdist = sgDistanceSquaredVec3( isecpoint, pt ); + if ( newSqdist < sqdist ) { + 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 ); + // 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 ... +// *agl = sqrt(sqdist); + *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); +// *loadCapacity = DBL_MAX; +// *frictionFactor = 1.0; + } + } + } + } + + if (ret) + return true; + + // Whenever we did not have a ground triangle for the requested point, + // take the ground level we found during the current cache build. + // This is as good as what we had before for agl. + double r = sgdLengthVec3( dpt ); + sgdCopyVec3( contact, dpt ); + sgdScaleVec3( contact, ground_radius/r ); + sgdCopyVec3( normal, dpt ); + sgdNormaliseVec3( normal ); + sgdSetVec3( vel, 0.0, 0.0, 0.0 ); + + // The altitude is the distance of the requested point from the + // contact point. + *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact ); + *type = FGInterface::Unknown; + *loadCapacity = DBL_MAX; + *frictionFactor = 1.0; + + return ret; +} + +bool FGGroundCache::caught_wire(double t, const double cpt[4][3]) +{ + bool ret = false; + + // Time difference to the reference time. + t -= cache_ref_time; + + bool firsttime = true; + sgVec4 plane[2]; + sgVec3 tri[2][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 = dynamic_cast(va->getUserData()); + // Assertation??? + if ( !gp ) + continue; + // Check if we have a catapult ... + if ( gp->type != FGInterface::Wire ) + continue; + + // 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] ); + } + + 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; + } + } + } + } + + return ret; +} + +bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3]) +{ + // Fast return if we do not have an active wire. + if (wire_id < 0) + return false; + + bool ret = false; + + // Time difference to th 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 = dynamic_cast(va->getUserData()); + // Assertation??? + if ( !gp ) + continue; + // 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; + } + + return ret; +} + +void FGGroundCache::release_wire(void) +{ + wire_id = -1; +} diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx new file mode 100644 index 000000000..be4293db5 --- /dev/null +++ b/src/FDM/groundcache.hxx @@ -0,0 +1,144 @@ +// groundcache.hxx -- carries a small subset of the scenegraph near the vehicle +// +// Written by Mathias Froehlich, started Nov 2004. +// +// Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +#ifndef _GROUNDCACHE_HXX +#define _GROUNDCACHE_HXX + +#include +#include +#include +#include + +class FGGroundCache { +public: + ////////////////////////////////////////////////////////////////////////// + // Ground handling routines + ////////////////////////////////////////////////////////////////////////// + + // Prepare the ground cache for the wgs84 position pt_*. + // That is take all vertices in the ball with radius rad around the + // position given by the pt_* and store them in a local scene graph. + bool prepare_ground_cache(double ref_time, const double pt[3], + double rad); + + // Returns true if the cache is valid. + // Also the reference time, point and radius values where the cache + // is valid for are returned. + bool is_valid(double *ref_time, double pt[3], double *rad); + + + // Return the nearest catapult to the given point + // pt in wgs84 coordinates. + double get_cat(double t, const double pt[3], + double end[2][3], double vel[2][3]); + + + // Return the altitude above ground below the wgs84 point pt + // Search for the nearest triangle to pt. + // Return ground properties like the ground type, the maximum load + // this kind kind of ground can carry, the friction factor between + // 0 and 1 which can be used to model lower friction with wet runways + // and finally the altitude above ground. + bool get_agl(double t, const double pt[3], + double contact[3], double normal[3], double vel[3], + int *type, double *loadCapacity, + double *frictionFactor, double *agl); + + + // Return 1 if the hook intersects with a wire. + // That test is done by checking if the quad spanned by the points pt* + // intersects with the line representing the wire. + // If the wire is caught, the cache will trace this wires endpoints until + // the FDM calls release_wire(). + bool caught_wire(double t, const double pt[4][3]); + + // Return the location and speed of the wire endpoints. + bool get_wire_ends(double t, double end[2][3], double vel[2][3]); + + // Tell the cache code that it does no longer need to care for + // the wire end position. + void release_wire(void); + +private: + // Holds the private ground triangle cache ... + // The surface cache itself. + ssgRoot cache_root; + // The center of the cache. + sgdVec3 cache_center; + // Approximate ground radius. + // In case the aircraft is too high above ground. + double ground_radius; + // The time reference for later call to intersection test routines. + // Is required since we will have moving triangles in carriers. + double cache_ref_time; + // The wire identifier to track. + int wire_id; + + // 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; + double reference_vehicle_radius; + bool found_ground; + + + // Fills the environment cache with everything inside the sphere sp. + void cache_fill(ssgBranch *branch, sgMat4 xform, + sgSphere* sp, sgVec3 down, sgSphere* wsp); + + // Helper class to hold some properties of the ground triangle. + class GroundProperty + : public ssgBase { + public: + GroundProperty() : type(0) {} + int type; + int wire_id; + sgVec3 vel; + // not yet implemented ... +// double loadCapacity; + }; + + // compute the ground property of this 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); +}; + +#endif diff --git a/src/Scenery/tilemgr.cxx b/src/Scenery/tilemgr.cxx index ea10fd646..b53b7d8cf 100644 --- a/src/Scenery/tilemgr.cxx +++ b/src/Scenery/tilemgr.cxx @@ -515,6 +515,9 @@ void FGTileMgr::prep_ssg_nodes( SGLocation *location, float vis ) { // selector and transform Point3D center = location->get_tile_center(); + if (center == Point3D(0.0)) + return; + current_center = center; float *up = location->get_world_up(); FGTileEntry *e; diff --git a/src/Scenery/tilemgr.hxx b/src/Scenery/tilemgr.hxx index 6e4b30f34..4b2405f1f 100644 --- a/src/Scenery/tilemgr.hxx +++ b/src/Scenery/tilemgr.hxx @@ -103,6 +103,10 @@ private: * tile cache */ FGNewCache tile_cache; + /** + * and its center + */ + Point3D current_center; /** * Queue tiles for loading. @@ -188,6 +192,7 @@ public: // based on current visibilty void prep_ssg_nodes( float // visibility_meters ); void prep_ssg_nodes( SGLocation *location, float visibility_meters ); + const Point3D get_current_center(void) const { return current_center; } // Set flag with event manager so that non-moving view refreshes // tiles... -- 2.39.5