//
// 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.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
#include <float.h>
#include <plib/sg.h>
+#include <osg/CullFace>
+#include <osg/Drawable>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/TriangleFunctor>
+#include <simgear/sg_inlines.h>
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/scene/material/mat.hxx>
+#include <simgear/scene/material/matlib.hxx>
+#include <simgear/scene/util/SGNodeMasks.hxx>
#include <Main/globals.hxx>
#include <Scenery/scenery.hxx>
#include "flight.hxx"
#include "groundcache.hxx"
-FGGroundCache::FGGroundCache()
+static inline bool
+fgdRayTriangle(SGVec3d& x, const SGVec3d& point, const SGVec3d& dir,
+ const SGVec3d v[3])
{
- sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
- ground_radius = 0.0;
- cache_ref_time = 0.0;
- wire_id = 0;
- sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
- reference_vehicle_radius = 0.0;
- found_ground = false;
+ double eps = 1e-4;
+ // Method based on the observation that we are looking for a
+ // point x that can be expressed in terms of the triangle points
+ // x = p_0 + \mu_1*(p_1 - p_0) + \mu_2*(p_2 - p_0)
+ // with 0 <= \mu_1, \mu_2 and \mu_1 + \mu_2 <= 1.
+ // OTOH it could be expressed in terms of the ray
+ // x = point + \lambda*dir
+ // Now we can compute \mu_i and \lambda.
+ // define
+ SGVec3d d1 = v[1] - v[0];
+ SGVec3d d2 = v[2] - v[0];
+ SGVec3d b = point - v[0];
+
+ // the vector in normal direction, but not normalized
+ SGVec3d d1crossd2 = cross(d1, d2);
+
+ double denom = -dot(dir, d1crossd2);
+ double signDenom = copysign(1, denom);
+ // return if paralell ??? FIXME what if paralell and in plane?
+ // may be we are ok below than anyway??
+ // if (SGMiscd::abs(denom) <= SGLimitsd::min())
+ // return false;
+
+ // Now \lambda would read
+ // lambda = 1/denom*dot(b, d1crossd2);
+ // To avoid an expensive division we multiply by |denom|
+ double lambdaDenom = signDenom*dot(b, d1crossd2);
+ if (lambdaDenom < 0)
+ return false;
+ // For line segment we would test against
+ // if (1 < lambda)
+ // return false;
+ // with the original lambda. The multiplied test would read
+ // if (absDenom < lambdaDenom)
+ // return false;
+
+ double absDenom = fabs(denom);
+ double absDenomEps = absDenom*eps;
+
+ SGVec3d bcrossr = cross(b, dir);
+ // double mu1 = 1/denom*dot(d2, bcrossr);
+ double mu1 = signDenom*dot(d2, bcrossr);
+ if (mu1 < -absDenomEps)
+ return false;
+ // double mu2 = -1/denom*dot(d1, bcrossr);
+ // if (mu2 < -eps)
+ // return false;
+ double mmu2 = signDenom*dot(d1, bcrossr);
+ if (mmu2 > absDenomEps)
+ return false;
+
+ if (mu1 - mmu2 > absDenom + absDenomEps)
+ return false;
+
+ x = point;
+ // if we have survived here it could only happen with denom == 0
+ // that the point is already in plane. Then return the origin ...
+ if (SGLimitsd::min() < absDenom)
+ x += (lambdaDenom/absDenom)*dir;
+
+ return true;
}
-FGGroundCache::~FGGroundCache()
+static inline bool
+fgdPointInTriangle( const SGVec3d& point, const SGVec3d tri[3] )
{
- cache_root.removeAllKids();
+ SGVec3d dif;
+
+ // Some tolerance in meters we accept a point to be outside of the triangle
+ // and still return that it is inside.
+ 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) )
+ 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) )
+ 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) )
+ 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 - 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 - 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 - y1) * tmpn);
+ if ( side1 != side2 ) {
+ // printf("failed side 3 check\n");
+ return false;
+ }
+
+ return true;
}
-FGGroundCache::GroundProperty*
-FGGroundCache::extractGroundProperty( ssgLeaf* l )
+// 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 SGVec3d& sphereCenter, double radius,
+ const SGVec3d& pt_on_line, const SGVec3d& dir)
{
- // 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<FGAICarrierHardware*>(l->getUserData());
- if (ud) {
- switch (ud->type) {
- case FGAICarrierHardware::Wire:
- gp->type = FGInterface::Wire;
- gp->wire_id = ud->id;
+ SGVec3d r = sphereCenter - pt_on_line;
+ double projectedDistance = dot(r, dir);
+ double dist = dot(r, r) - projectedDistance * projectedDistance;
+ return dist < radius*radius;
+}
+
+template<typename T>
+class SGExtendedTriangleFunctor : public osg::TriangleFunctor<T> {
+public:
+ // Ok, to be complete we should also implement the indexed variants
+ // For now this one appears to be enough ...
+ void drawArrays(GLenum mode, GLint first, GLsizei count)
+ {
+ if (_vertexArrayPtr==0 || count==0) return;
+
+ const osg::Vec3* vlast;
+ const osg::Vec3* vptr;
+ switch(mode) {
+ case(GL_LINES):
+ vlast = &_vertexArrayPtr[first+count];
+ for(vptr=&_vertexArrayPtr[first];vptr<vlast;vptr+=2)
+ this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
break;
- case FGAICarrierHardware::Catapult:
- gp->type = FGInterface::Catapult;
+ case(GL_LINE_STRIP):
+ vlast = &_vertexArrayPtr[first+count-1];
+ for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
+ this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
+ break;
+ case(GL_LINE_LOOP):
+ vlast = &_vertexArrayPtr[first+count-1];
+ for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
+ this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
+ this->operator()(_vertexArrayPtr[first+count-1],
+ _vertexArrayPtr[first],_treatVertexDataAsTemporary);
break;
default:
- gp->type = FGInterface::Solid;
+ osg::TriangleFunctor<T>::drawArrays(mode, first, count);
break;
}
-
- // Copy the velocity from the carrier class.
- ud->carrier->getVelocityWrtEarth( gp->vel );
}
+protected:
+ using osg::TriangleFunctor<T>::_vertexArrayPtr;
+ using osg::TriangleFunctor<T>::_treatVertexDataAsTemporary;
+};
- else {
-
- // Initialize velocity field.
- sgSetVec3( gp->vel, 0.0, 0.0, 0.0 );
- }
+class GroundCacheFillVisitor : public osg::NodeVisitor {
+public:
- // 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();
+ /// class to just redirect triangles to the GroundCacheFillVisitor
+ class GroundCacheFill {
+ public:
+ void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv)
+ { mGroundCacheFillVisitor = gcfv; }
- SG_LOG(SG_FLIGHT,SG_INFO,
- "New triangle in cache: " << category << " " << file );
+ void operator () (const osg::Vec3& v1, const osg::Vec3& v2,
+ const osg::Vec3& v3, bool)
+ { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); }
+
+ void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool)
+ { mGroundCacheFillVisitor->addLine(v1, v2); }
- 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;
+ private:
+ GroundCacheFillVisitor* mGroundCacheFillVisitor;
+ };
+
+
+ GroundCacheFillVisitor(FGGroundCache* groundCache,
+ const SGVec3d& down,
+ const SGVec3d& cacheReference,
+ double cacheRadius,
+ double wireCacheRadius) :
+ osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
+ mGroundCache(groundCache)
+ {
+ setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
+ mDown = down;
+ mLocalDown = down;
+ sphIsec = true;
+ mBackfaceCulling = false;
+ mCacheReference = cacheReference;
+ mLocalCacheReference = cacheReference;
+ mCacheRadius = cacheRadius;
+ mWireCacheRadius = wireCacheRadius;
+
+ mTriangleFunctor.setGroundCacheFillVisitor(this);
+
+ mGroundProperty.wire_id = -1;
+ mGroundProperty.vel = SGVec3d(0, 0, 0);
+ mGroundProperty.rot = SGVec3d(0, 0, 0);
+ mGroundProperty.pivot = SGVec3d(0, 0, 0);
+ }
+
+ void updateCullMode(osg::StateSet* stateSet)
+ {
+ if (!stateSet)
+ return;
+
+ osg::StateAttribute* stateAttribute;
+ stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE);
+ if (!stateAttribute)
+ return;
+ osg::CullFace* cullFace = static_cast<osg::CullFace*>(stateAttribute);
+ mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK;
+ }
+
+ bool enterBoundingSphere(const osg::BoundingSphere& bs)
+ {
+ if (!bs.valid())
+ return false;
+
+ SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal);
+ double rc = bs.radius() + mCacheRadius;
+ // Ok, this node might intersect the cache. Visit it in depth.
+ double centerDist2 = distSqr(mCacheReference, cntr);
+ if (centerDist2 < rc*rc) {
+ sphIsec = true;
+ } else {
+ // Check if the down direction touches the bounding sphere of the node
+ // if so, do at least croase agl computations.
+ // Ther other thing is that we must check if we are in range of
+ // cats or wires
+ double rw = bs.radius() + mWireCacheRadius;
+ if (rw*rw < centerDist2 &&
+ !fgdIsectSphereInfLine(cntr, bs.radius(), mCacheReference, mDown))
+ return false;
+ sphIsec = false;
}
+
+ return true;
}
+
+ bool enterNode(osg::Node& node)
+ {
+ if (!enterBoundingSphere(node.getBound()))
+ return false;
+
+ updateCullMode(node.getStateSet());
+
+ FGGroundCache::GroundProperty& gp = mGroundProperty;
+ // get some material information for use in the gear model
+ gp.material = globals->get_matlib()->findMaterial(&node);
+ if (gp.material) {
+ gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
+ return true;
+ }
+ gp.type = FGInterface::Unknown;
+ osg::Referenced* base = node.getUserData();
+ if (!base)
+ return true;
+ FGAICarrierHardware *ud =
+ dynamic_cast<FGAICarrierHardware*>(base);
+ if (!ud)
+ return true;
+
+ 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, gp.rot, gp.pivot);
- return gp;
-}
+ 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 bool
-sgIsectSphereInfLine(const sgSphere *sp,
- const sgVec3 pt_on_line, const sgVec3 dir)
-{
- sgVec3 r ;
- sgSubVec3 ( r, sp->getCenter(), pt_on_line ) ;
+ void fillWith(osg::Drawable* drawable)
+ {
+ bool oldSphIsec = sphIsec;
+ if (!enterBoundingSphere(drawable->getBound()))
+ return;
- SGfloat projectedDistance = sgScalarProductVec3(r, dir);
-
- SGfloat dist = sgScalarProductVec3 ( r, r ) -
- projectedDistance * projectedDistance;
+ bool oldBackfaceCulling = mBackfaceCulling;
+ updateCullMode(drawable->getStateSet());
- SGfloat radius = sp->getRadius();
- return dist < radius*radius;
-}
+ drawable->accept(mTriangleFunctor);
-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;
+ mBackfaceCulling = oldBackfaceCulling;
+ sphIsec = oldSphIsec;
}
- 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,
- 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] );
- }
+ virtual void apply(osg::Geode& geode)
+ {
+ bool oldBackfaceCulling = mBackfaceCulling;
+ bool oldSphIsec = sphIsec;
+ FGGroundCache::GroundProperty oldGp = mGroundProperty;
+ if (!enterNode(geode))
+ return;
+
+ for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
+ fillWith(geode.getDrawable(i));
+ sphIsec = oldSphIsec;
+ mGroundProperty = oldGp;
+ mBackfaceCulling = oldBackfaceCulling;
}
- 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;
+ virtual void apply(osg::Group& group)
+ {
+ bool oldBackfaceCulling = mBackfaceCulling;
+ bool oldSphIsec = sphIsec;
+ FGGroundCache::GroundProperty oldGp = mGroundProperty;
+ if (!enterNode(group))
+ return;
+ traverse(group);
+ sphIsec = oldSphIsec;
+ mBackfaceCulling = oldBackfaceCulling;
+ mGroundProperty = oldGp;
+ }
+
+ virtual void apply(osg::Transform& transform)
+ {
+ if (!enterNode(transform))
+ return;
+ bool oldBackfaceCulling = mBackfaceCulling;
+ bool oldSphIsec = sphIsec;
+ FGGroundCache::GroundProperty oldGp = mGroundProperty;
+ /// transform the caches center to local coords
+ osg::Matrix oldLocalToGlobal = mLocalToGlobal;
+ osg::Matrix oldGlobalToLocal = mGlobalToLocal;
+ transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
+ transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
+
+ SGVec3d oldLocalCacheReference = mLocalCacheReference;
+ mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
+ SGVec3d oldLocalDown = mLocalDown;
+ mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
+
+ // walk the children
+ traverse(transform);
+
+ // Restore that one
+ mLocalDown = oldLocalDown;
+ mLocalCacheReference = oldLocalCacheReference;
+ mLocalToGlobal = oldLocalToGlobal;
+ mGlobalToLocal = oldGlobalToLocal;
+ sphIsec = oldSphIsec;
+ mBackfaceCulling = oldBackfaceCulling;
+ mGroundProperty = oldGp;
+ }
- 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] );
+ void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
+ const osg::Vec3& v3)
+ {
+ SGVec3d v[3] = {
+ SGVec3d(v1),
+ SGVec3d(v2),
+ SGVec3d(v3)
+ };
+
+ // a bounding sphere in the node local system
+ SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
+#if 0
+ double boundRadius = std::max(norm1(v[0] - boundCenter),
+ norm1(v[1] - boundCenter));
+ boundRadius = std::max(boundRadius, norm1(v[2] - boundCenter));
+ // Ok, we take the 1-norm instead of the expensive 2 norm.
+ // Therefore we need that scaling factor - roughly sqrt(3)
+ boundRadius = 1.733*boundRadius;
+#else
+ double boundRadius = std::max(distSqr(v[0], boundCenter),
+ distSqr(v[1], boundCenter));
+ boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
+ boundRadius = sqrt(boundRadius);
+#endif
+
+ // if we are not in the downward cylinder bail out
+ if (!fgdIsectSphereInfLine(boundCenter, boundRadius + mCacheRadius,
+ mLocalCacheReference, mLocalDown))
+ return;
+
+
+ // The normal and plane in the node local coordinate system
+ SGVec3d n = normalize(cross(v[1] - v[0], v[2] - v[0]));
+ if (0 < dot(mLocalDown, n)) {
+ if (mBackfaceCulling) {
+ // Surface points downwards, ignore for altitude computations.
+ return;
+ } else {
+ n = -n;
+ std::swap(v[1], v[2]);
+ }
+ }
+
+ // Only check if the triangle is in the cache sphere if the plane
+ // containing the triangle is near enough
+ if (sphIsec && fabs(dot(n, v[0] - mLocalCacheReference)) < mCacheRadius) {
+ // Check if the sphere around the vehicle intersects the sphere
+ // around that triangle. If so, put that triangle into the cache.
+ double r2 = boundRadius + mCacheRadius;
+ if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
+ FGGroundCache::Triangle t;
+ for (unsigned i = 0; i < 3; ++i)
+ t.vertices[i].osg() = v[i].osg()*mLocalToGlobal;
+ t.boundCenter.osg() = boundCenter.osg()*mLocalToGlobal;
+ t.boundRadius = boundRadius;
+
+ SGVec3d tmp;
+ tmp.osg() = osg::Matrixd::transform3x3(n.osg(), mLocalToGlobal);
+ t.plane = SGVec4d(tmp[0], tmp[1], tmp[2], -dot(tmp, t.vertices[0]));
+ t.velocity = mGroundProperty.vel;
+ t.rotation = mGroundProperty.rot;
+ t.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
+ t.type = mGroundProperty.type;
+ t.material = mGroundProperty.material;
+ mGroundCache->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 ) ) {
- 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;
+ SGVec4d plane = SGVec4d(n[0], n[1], n[2], -dot(n, v[0]));
+ SGVec3d isectpoint;
+
+ if (fgdRayTriangle(isectpoint, mLocalCacheReference, mLocalDown, v)) {
+ mGroundCache->found_ground = true;
+ isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
+ isectpoint += mGroundCache->cache_center;
+ double this_radius = length(isectpoint);
+ if (mGroundCache->ground_radius < this_radius) {
+ mGroundCache->ground_radius = this_radius;
+ mGroundCache->_type = mGroundProperty.type;
+ mGroundCache->_material = mGroundProperty.material;
}
}
}
- if (!ia)
- return;
- addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform);
-}
+ void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
+ {
+ SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
+ SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
+
+ SGVec3d boundCenter = 0.5*(gv1 + gv2);
+ double boundRadius = length(gv1 - boundCenter);
+
+ if (distSqr(boundCenter, mCacheReference)
+ < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
+ if (mGroundProperty.type == FGInterface::Wire) {
+ FGGroundCache::Wire wire;
+ wire.ends[0] = gv1;
+ wire.ends[1] = gv2;
+ wire.velocity = mGroundProperty.vel;
+ wire.rotation = mGroundProperty.rot;
+ wire.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
+ wire.wire_id = mGroundProperty.wire_id;
+
+ mGroundCache->wires.push_back(wire);
+ }
+ if (mGroundProperty.type == FGInterface::Catapult) {
+ FGGroundCache::Catapult cat;
+ // Trick to get the ends in the right order.
+ // Use the x axis in the original coordinate system. Choose the
+ // most negative x-axis as the one pointing forward
+ if (v1[0] > v2[0]) {
+ cat.start = gv1;
+ cat.end = gv2;
+ } else {
+ cat.start = gv2;
+ cat.end = gv1;
+ }
+ cat.velocity = mGroundProperty.vel;
+ cat.rotation = mGroundProperty.rot;
+ cat.rotation_pivot = mGroundProperty.pivot - mGroundCache->cache_center;
-// 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 );
+ mGroundCache->catapults.push_back(cat);
+ }
+ }
+ }
- // Set velocity.
- sgdSetVec3( wgs84_vel, gp->vel );
-}
+ SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
+ FGGroundCache* mGroundCache;
+ SGVec3d mCacheReference;
+ double mCacheRadius;
+ double mWireCacheRadius;
+ osg::Matrix mLocalToGlobal;
+ osg::Matrix mGlobalToLocal;
+ SGVec3d mDown;
+ SGVec3d mLocalDown;
+ SGVec3d mLocalCacheReference;
+ bool sphIsec;
+ bool mBackfaceCulling;
+ FGGroundCache::GroundProperty mGroundProperty;
+};
-void
-FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va,
- GroundProperty *gp, short i,
- sgdVec3 wgs84_pos, sgdVec3 wgs84_vel)
+FGGroundCache::FGGroundCache()
{
- sgVec3 rel_pos;
- extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel);
- sgdSetVec3( wgs84_pos, rel_pos );
- sgdAddVec3( wgs84_pos, cache_center );
+ cache_center = SGVec3d(0, 0, 0);
+ ground_radius = 0.0;
+ cache_ref_time = 0.0;
+ wire_id = 0;
+ reference_wgs84_point = SGVec3d(0, 0, 0);
+ reference_vehicle_radius = 0.0;
+ found_ground = false;
}
+FGGroundCache::~FGGroundCache()
+{
+}
-void
-FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
- sgSphere* sp, sgVec3 down, sgSphere* wsp)
+inline void
+FGGroundCache::velocityTransformTriangle(double dt,
+ FGGroundCache::Triangle& dst,
+ const FGGroundCache::Triangle& src)
{
- // 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);
- }
+ dst = src;
+
+ if (fabs(dt*dot(src.velocity, src.velocity)) < SGLimitsd::epsilon())
+ return;
+
+ for (int i = 0; i < 3; ++i) {
+ SGVec3d pivotoff = src.vertices[i] - src.rotation_pivot;
+ dst.vertices[i] += dt*(src.velocity + cross(src.rotation, pivotoff));
}
+
+ // Transform the plane equation
+ SGVec3d pivotoff, vel;
+ sgdSubVec3(pivotoff.sg(), dst.plane.sg(), src.rotation_pivot.sg());
+ vel = src.velocity + cross(src.rotation, pivotoff);
+ dst.plane[3] += dt*sgdScalarProductVec3(dst.plane.sg(), vel.sg());
+
+ dst.boundCenter += dt*src.velocity;
}
bool
-FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
- double rad)
+FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
+ double rad)
{
// 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);
+ reference_wgs84_point = pt;
reference_vehicle_radius = rad;
// Store the time reference used to compute movements of moving triangles.
cache_ref_time = ref_time;
+ // Get a normalized down vector valid for the whole cache
+ SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
+ down = hlToEc.rotate(SGVec3d(0, 0, 1));
+
+ // Decide where we put the scenery center.
+ SGVec3d old_cntr = globals->get_scenery()->get_center();
+ SGVec3d cntr(pt);
+ // Only move the cache center if it is unacceptable far away.
+ if (40*40 < distSqr(old_cntr, cntr))
+ globals->get_scenery()->set_center(cntr);
+ else
+ cntr = old_cntr;
+
// The center of the cache.
- sgdCopyVec3(cache_center, pt);
+ cache_center = cntr;
- sgVec3 zero;
- sgZeroVec3(zero);
// Prepare sphere around the aircraft.
- sgSphere acSphere;
- acSphere.setRadius(rad);
- acSphere.setCenter(zero);
+ SGVec3d ptoff = pt - cache_center;
+ double cacheRadius = rad;
// 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(zero);
-
- // 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_tile_mgr()->get_current_center();
- sgdSetVec3(doffset, psc[0], psc[1], psc[2]);
- sgdSubVec3(doffset, doffset, pt);
-
- // 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.
- sgVec3 offset;
- sgSetVec3(offset, doffset[0], doffset[1], doffset[2]);
- sgMat4 xform;
- sgMakeTransMat4(xform, offset);
-
-
- // Walk the terrain branch for now.
- ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
- cache_fill(terrain, xform, &acSphere, down, &wireSphere);
+ double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
+
+ // Walk the scene graph and extract solid ground triangles and carrier data.
+ GroundCacheFillVisitor gcfv(this, down, ptoff, cacheRadius, wireCacheRadius);
+ globals->get_scenery()->get_scene_graph()->accept(gcfv);
// some stats
- SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad
- << ", # leafs = " << cache_root.getNumKids()
+ SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
+ << ", # 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
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
"without any scenery below the aircraft" );
+ if (cntr != old_cntr)
+ globals->get_scenery()->set_center(old_cntr);
+
return found_ground;
}
bool
-FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
+FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
{
- sgdCopyVec3(pt, reference_wgs84_point);
- *rad = reference_vehicle_radius;
- *ref_time = cache_ref_time;
+ 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])
+FGGroundCache::get_cat(double t, const SGVec3d& dpt,
+ SGVec3d end[2], SGVec3d vel[2])
{
// 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 = static_cast<GroundProperty*>(va->getUserData());
- // 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;
+ size_t sz = catapults.size();
+ for (size_t i = 0; i < sz; ++i) {
+ SGVec3d pivotoff, rvel[2];
+ pivotoff = catapults[i].start - catapults[i].rotation_pivot;
+ rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
+ pivotoff = catapults[i].end - catapults[i].rotation_pivot;
+ rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
+
+ SGVec3d thisEnd[2];
+ thisEnd[0] = cache_center + catapults[i].start + t*rvel[0];
+ thisEnd[1] = cache_center + catapults[i].end + t*rvel[1];
+
+ sgdLineSegment3 ls;
+ sgdCopyVec3(ls.a, thisEnd[0].sg());
+ sgdCopyVec3(ls.b, thisEnd[1].sg());
+ double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt.sg() );
+
+ 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] );
- }
+ end[0] = thisEnd[0];
+ end[1] = thisEnd[1];
+ vel[0] = rvel[0];
+ vel[1] = rvel[1];
}
}
}
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)
+FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
+ SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
+ int *type, const SGMaterial** material, 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 );
+ if (material)
+ *material = 0;
+ vel = SGVec3d(0, 0, 0);
+ contact = SGVec3d(0, 0, 0);
+ normal = SGVec3d(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] );
+ SGVec3d pt = dpt - cache_center;
+ // shift the start of our ray by maxaltoff upwards
+ SGVec3d raystart = pt - max_altoff*down;
// 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;
- ssgBase *gpb = va->getUserData();
- // Assertation???
- if ( !gpb ) {
- cerr << "Found cache leaf without userdata!" << endl;
+ double current_radius = 0.0;
+
+ size_t sz = triangles.size();
+ for (size_t i = 0; i < sz; ++i) {
+ Triangle triangle;
+ velocityTransformTriangle(t, triangle, triangles[i]);
+ if (!fgdIsectSphereInfLine(triangle.boundCenter, triangle.boundRadius, pt, down))
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 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 ...
-// *agl = sqrt(sqdist);
- *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
-// *loadCapacity = DBL_MAX;
-// *frictionFactor = 1.0;
- }
+ // Check for intersection.
+ SGVec3d isecpoint;
+ if (fgdRayTriangle(isecpoint, raystart, down, triangle.vertices)) {
+ // Compute the vector from pt to the intersection point ...
+ SGVec3d off = isecpoint - pt;
+ // ... and check if it is too high or not
+ // Transform to the wgs system
+ isecpoint += cache_center;
+ // compute the radius, good enough approximation to take the geocentric radius
+ double radius = dot(isecpoint, isecpoint);
+ if (current_radius < radius) {
+ current_radius = radius;
+ ret = true;
+ // Save the new potential intersection point.
+ contact = isecpoint;
+ // The first three values in the vector are the plane normal.
+ sgdCopyVec3( normal.sg(), triangle.plane.sg() );
+ // The velocity wrt earth.
+ SGVec3d pivotoff = pt - triangle.rotation_pivot;
+ vel = triangle.velocity + cross(triangle.rotation, pivotoff);
+ // Save the ground type.
+ *type = triangle.type;
+ *agl = dot(down, contact - dpt);
+ if (material)
+ *material = triangle.material;
}
}
}
// 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 );
+ double r = length(dpt);
+ contact = dpt;
+ contact *= ground_radius/r;
+ normal = -down;
+ vel = SGVec3d(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;
+ *agl = dot(down, contact - dpt);
+ *type = _type;
+ if (material)
+ *material = _material;
return ret;
}
-bool FGGroundCache::caught_wire(double t, const double cpt[4][3])
+bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
{
- 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];
-
- // 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;
-
- // 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] );
+ // Build the two triangles spanning the area where the hook has moved
+ // during the past step.
+ SGVec4d plane[2];
+ SGVec3d tri[2][3];
+ sgdMakePlane( plane[0].sg(), pt[0].sg(), pt[1].sg(), pt[2].sg() );
+ tri[0][0] = pt[0];
+ tri[0][1] = pt[1];
+ tri[0][2] = pt[2];
+ sgdMakePlane( plane[1].sg(), pt[0].sg(), pt[2].sg(), pt[3].sg() );
+ tri[1][0] = pt[0];
+ tri[1][1] = pt[2];
+ tri[1][2] = pt[3];
+
+ // Intersect the wire lines with each of these triangles.
+ // You have caught a wire if they intersect.
+ for (size_t i = 0; i < sz; ++i) {
+ SGVec3d le[2];
+ for (int k = 0; k < 2; ++k) {
+ le[k] = wires[i].ends[k];
+ SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
+ SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
+ le[k] += t*vel + cache_center;
}
- 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) {
+ SGVec3d isecpoint;
+ double isecval = sgdIsectLinesegPlane(isecpoint.sg(), le[0].sg(),
+ le[1].sg(), plane[k].sg());
+ if ( 0.0 <= isecval && isecval <= 1.0 &&
+ fgdPointInTriangle( 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])
+bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
{
// 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.
+ // 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) {
+ for (size_t k = 0; k < 2; ++k) {
+ SGVec3d pivotoff = end[k] - wires[i].rotation_pivot;
+ vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
+ end[k] = cache_center + wires[i].ends[k] + t*vel[k];
+ }
+ return true;
+ }
}
- return ret;
+ return false;
}
void FGGroundCache::release_wire(void)
projects / flightgear.git / blobdiff