From: frohlich Date: Sun, 1 Mar 2009 16:22:04 +0000 (+0000) Subject: Make initial use of the boundingvolumes for the ground cache. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=1708e43a39ce96b703a4aa21a8455964415dd499;p=flightgear.git Make initial use of the boundingvolumes for the ground cache. Modified Files: src/AIModel/AICarrier.cxx src/AIModel/AICarrier.hxx src/FDM/flight.hxx src/FDM/groundcache.cxx src/FDM/groundcache.hxx --- diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index 182bcad87..ff0d5441f 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -25,12 +25,19 @@ #include #include +#include +#include +#include #include +#include #include #include #include #include +#include +#include +#include #include #include
@@ -38,63 +45,133 @@ #include "AICarrier.hxx" -class FGCarrierVisitor : public osg::NodeVisitor { +/// Hmm: move that kind of configuration into the model file??? +class LineCollector : public osg::NodeVisitor { + struct LinePrimitiveFunctor { + LinePrimitiveFunctor() : _lineCollector(0) + { } + void operator() (const osg::Vec3&, bool) + { } + void operator() (const osg::Vec3& v1, const osg::Vec3& v2, bool) + { if (_lineCollector) _lineCollector->addLine(v1, v2); } + void operator() (const osg::Vec3&, const osg::Vec3&, const osg::Vec3&, + bool) + { } + void operator() (const osg::Vec3&, const osg::Vec3&, const osg::Vec3&, + const osg::Vec3&, bool) + { } + LineCollector* _lineCollector; + }; + public: - FGCarrierVisitor(FGAICarrier* carrier, - const std::list& wireObjects, - const std::list& catapultObjects, - const std::list& solidObjects) : - osg::NodeVisitor(osg::NodeVisitor::NODE_VISITOR, - osg::NodeVisitor::TRAVERSE_ALL_CHILDREN), - mWireObjects(wireObjects), - mCatapultObjects(catapultObjects), - mSolidObjects(solidObjects), - mFoundHot(false), - mCarrier(carrier) - { } - virtual void apply(osg::Node& node) - { - osg::ref_ptr oldUserData = mUserData; - bool oldFoundHot = mFoundHot; - mFoundHot = false; - - if (std::find(mWireObjects.begin(), mWireObjects.end(), node.getName()) - != mWireObjects.end()) { - mFoundHot = true; - mUserData = FGAICarrierHardware::newWire(mCarrier); + LineCollector() : + osg::NodeVisitor(osg::NodeVisitor::NODE_VISITOR, + osg::NodeVisitor::TRAVERSE_ALL_CHILDREN) + { } + virtual void apply(osg::Geode& geode) + { + osg::TemplatePrimitiveFunctor pf; + pf._lineCollector = this; + for (unsigned i = 0; i < geode.getNumDrawables(); ++i) { + geode.getDrawable(i)->accept(pf); + } } - if (std::find(mCatapultObjects.begin(), mCatapultObjects.end(), node.getName()) - != mCatapultObjects.end()) { - mFoundHot = true; - mUserData = FGAICarrierHardware::newCatapult(mCarrier); + virtual void apply(osg::Node& node) + { + traverse(node); } - if (std::find(mSolidObjects.begin(), mSolidObjects.end(), node.getName()) - != mSolidObjects.end()) { - mFoundHot = true; - mUserData = FGAICarrierHardware::newSolid(mCarrier); - //SG_LOG(SG_GENERAL, SG_ALERT, "AICarrierVisitor::apply() solidObject" ); + virtual void apply(osg::Transform& transform) + { + osg::Matrix matrix = _matrix; + if (transform.computeLocalToWorldMatrix(_matrix, this)) + traverse(transform); + _matrix = matrix; } - node.setUserData(mUserData.get()); + + const std::vector& getLineSegments() const + { return _lineSegments; } + + void addLine(const osg::Vec3& v1, const osg::Vec3& v2) + { + // 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 + SGVec3f tv1(_matrix.preMult(v1)); + SGVec3f tv2(_matrix.preMult(v2)); + if (tv1[0] > tv2[0]) + _lineSegments.push_back(SGLineSegmentf(tv1, tv2)); + else + _lineSegments.push_back(SGLineSegmentf(tv2, tv1)); + } + + void addBVHElements(osg::Node& node, simgear::BVHLineGeometry::Type type) + { + if (_lineSegments.empty()) + return; - traverse(node); + SGSceneUserData* userData; + userData = SGSceneUserData::getOrCreateSceneUserData(&node); - mFoundHot = oldFoundHot || mFoundHot; + simgear::BVHNode* bvNode = userData->getBVHNode(); + if (!bvNode && _lineSegments.size() == 1) { + simgear::BVHLineGeometry* bvLine; + bvLine = new simgear::BVHLineGeometry(_lineSegments.front(), type); + userData->setBVHNode(bvLine); + return; + } - if (mFoundHot) { - node.setNodeMask(node.getNodeMask() | SG_NODEMASK_TERRAIN_BIT); - } else - node.setNodeMask(node.getNodeMask() & ~SG_NODEMASK_TERRAIN_BIT); + simgear::BVHGroup* group = new simgear::BVHGroup; + if (bvNode) + group->addChild(bvNode); - mUserData = oldUserData; - } - + for (unsigned i = 0; i < _lineSegments.size(); ++i) { + simgear::BVHLineGeometry* bvLine; + bvLine = new simgear::BVHLineGeometry(_lineSegments[i], type); + group->addChild(bvLine); + } + userData->setBVHNode(group); + } + private: - std::list mWireObjects; - std::list mCatapultObjects; - std::list mSolidObjects; - bool mFoundHot; - FGAICarrier* mCarrier; - osg::ref_ptr mUserData; + osg::Matrix _matrix; + std::vector _lineSegments; +}; + +class FGCarrierVisitor : public osg::NodeVisitor { +public: + FGCarrierVisitor(FGAICarrier* carrier, + const std::list& wireObjects, + const std::list& catapultObjects) : + osg::NodeVisitor(osg::NodeVisitor::NODE_VISITOR, + osg::NodeVisitor::TRAVERSE_ALL_CHILDREN), + mWireObjects(wireObjects), + mCatapultObjects(catapultObjects) + { } + virtual void apply(osg::Node& node) + { + if (std::find(mWireObjects.begin(), mWireObjects.end(), node.getName()) + != mWireObjects.end()) { + LineCollector lineCollector; + node.accept(lineCollector); + simgear::BVHLineGeometry::Type type; + type = simgear::BVHLineGeometry::CarrierWire; + lineCollector.addBVHElements(node, type); + } + if (std::find(mCatapultObjects.begin(), mCatapultObjects.end(), + node.getName()) != mCatapultObjects.end()) { + LineCollector lineCollector; + node.accept(lineCollector); + simgear::BVHLineGeometry::Type type; + type = simgear::BVHLineGeometry::CarrierCatapult; + lineCollector.addBVHElements(node, type); + } + + traverse(node); + } + +private: + std::list mWireObjects; + std::list mCatapultObjects; }; FGAICarrier::FGAICarrier() : FGAIShip(otCarrier) { @@ -145,13 +222,6 @@ void FGAICarrier::readFromScenario(SGPropertyNode* scFileNode) { catapult_objects.push_back(s); } - props = scFileNode->getChildren("solid"); - for (it = props.begin(); it != props.end(); ++it) { - std::string s = (*it)->getStringValue(); - if (!s.empty()) - solid_objects.push_back(s); - } - props = scFileNode->getChildren("parking-pos"); for (it = props.begin(); it != props.end(); ++it) { string name = (*it)->getStringValue("name", "unnamed"); @@ -199,12 +269,6 @@ void FGAICarrier::setTACANChannelID(const string& id) { TACAN_channel_id = id; } -void FGAICarrier::getVelocityWrtEarth(SGVec3d& v, SGVec3d& omega, SGVec3d& pivot) { - v = vel_wrt_earth; - omega = rot_wrt_earth; - pivot = rot_pivot_wrt_earth; -} - void FGAICarrier::update(double dt) { // For computation of rotation speeds we just use finite differences here. // That is perfectly valid since this thing is not driven by accelerations @@ -219,13 +283,9 @@ void FGAICarrier::update(double dt) { SGQuatd ec2body = ec2hl*hl2body; // The cartesian position of the carrier in the wgs84 world SGVec3d cartPos = SGVec3d::fromGeod(pos); - // Store for later use by the groundcache - rot_pivot_wrt_earth = cartPos; - // Compute the velocity in m/s in the earth centered coordinate system axis - double v_north = 0.51444444*speed*cos(hdg * SGD_DEGREES_TO_RADIANS); - double v_east = 0.51444444*speed*sin(hdg * SGD_DEGREES_TO_RADIANS); - vel_wrt_earth = ec2hl.backTransform(SGVec3d(v_north, v_east, 0)); + // Compute the velocity in m/s in the body frame + aip.setBodyLinearVelocity(SGVec3d(0.51444444*speed, 0, 0)); // Now update the position and heading. This will compute new hdg and // roll values required for the rotation speed computation. @@ -242,9 +302,7 @@ void FGAICarrier::update(double dt) { } // Only change these values if we are able to compute them safely - if (dt < DBL_MIN) - rot_wrt_earth = SGVec3d::zeros(); - else { + if (SGLimits::min() < dt) { // Now here is the finite difference ... // Transform that one to the horizontal local coordinate system. @@ -258,13 +316,7 @@ void FGAICarrier::update(double dt) { // divided by the time difference provides a rotation speed vector dOrAngleAxis /= dt; - // now rotate the rotation speed vector back into the - // earth centered frames coordinates - dOrAngleAxis = ec2body.backTransform(dOrAngleAxis); -// dOrAngleAxis = hl2body.backTransform(dOrAngleAxis); -// dOrAngleAxis(1) = 0; -// dOrAngleAxis = ec2hl.backTransform(dOrAngleAxis); - rot_wrt_earth = dOrAngleAxis; + aip.setBodyAngularVelocity(dOrAngleAxis); } UpdateWind(dt); @@ -351,19 +403,9 @@ void FGAICarrier::initModel(osg::Node *node) FGAIShip::initModel(node); // process the 3d model here // mark some objects solid, mark the wires ... - - // The model should be used for altitude computations. - // To avoid that every detail in a carrier 3D model will end into - // the aircraft local cache, only set the HOT traversal bit on - // selected objects. - - // Clear the HOT traversal flag - // Selectively set that flag again for wires/cats/solid objects. - // Attach a pointer to this carrier class to those objects. - // SG_LOG(SG_GENERAL, SG_BULK, "AICarrier::initModel() visit" ); - FGCarrierVisitor carrierVisitor(this, wire_objects, catapult_objects, solid_objects); + FGCarrierVisitor carrierVisitor(this, wire_objects, catapult_objects); model->accept(carrierVisitor); -// model->setNodeMask(node->getNodeMask() & SG_NODEMASK_TERRAIN_BIT | model->getNodeMask()); + model->setNodeMask(model->getNodeMask() | SG_NODEMASK_TERRAIN_BIT); } void FGAICarrier::bind() { @@ -694,7 +736,3 @@ void FGAICarrier::UpdateJBD(double dt, double jbd_transition_time) { return; } // end UpdateJBD - - -int FGAICarrierHardware::unique_id = 1; - diff --git a/src/AIModel/AICarrier.hxx b/src/AIModel/AICarrier.hxx index c359c3fcb..082e11153 100644 --- a/src/AIModel/AICarrier.hxx +++ b/src/AIModel/AICarrier.hxx @@ -40,42 +40,6 @@ using std::list; class FGAIManager; class FGAICarrier; -class FGAICarrierHardware : public osg::Referenced { -public: - - enum Type { Catapult, Wire, Solid }; - - FGAICarrier *carrier; - int id; - Type type; - - static FGAICarrierHardware* newCatapult(FGAICarrier *c) { - FGAICarrierHardware* ch = new FGAICarrierHardware; - ch->carrier = c; - ch->type = Catapult; - ch->id = unique_id++; - return ch; - } - static FGAICarrierHardware* newWire(FGAICarrier *c) { - FGAICarrierHardware* ch = new FGAICarrierHardware; - ch->carrier = c; - ch->type = Wire; - ch->id = unique_id++; - return ch; - } - static FGAICarrierHardware* newSolid(FGAICarrier *c) { - FGAICarrierHardware* ch = new FGAICarrierHardware; - ch->carrier = c; - ch->type = Solid; - ch->id = unique_id++; - return ch; - } - -private: - static int unique_id; -}; - - class FGAICarrier : public FGAIShip { public: @@ -87,7 +51,6 @@ public: void setSign(const string& ); void setTACANChannelID(const string &); - void getVelocityWrtEarth(SGVec3d& v, SGVec3d& omega, SGVec3d& pivot); virtual void bind(); virtual void unbind(); void UpdateWind ( double dt ); @@ -133,17 +96,11 @@ private: double rel_wind_from_deg; - list solid_objects; // List of solid object names list wire_objects; // List of wire object names list catapult_objects; // List of catapult object names list ppositions; // List of positions where an aircraft can start. string sign; // The sign of this carrier. - // Velocity wrt earth. - SGVec3d vel_wrt_earth; - SGVec3d rot_wrt_earth; - SGVec3d rot_pivot_wrt_earth; - // these describe the flols SGVec3d flols_off; diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx index 017ab0a55..cd953167e 100644 --- a/src/FDM/flight.hxx +++ b/src/FDM/flight.hxx @@ -601,11 +601,11 @@ public: ////////////////////////////////////////////////////////////////////////// enum GroundType { + /// I sused at least in YAsim. Deprecate this ad get it from the + /// Material instead Unknown = 0, //?? Solid, // Whatever we will roll on with infinite load factor. - Water, // For the beaver ... - Catapult, // Carrier cats. - Wire // Carrier wires. + Water // For the beaver ... }; // Prepare the ground cache for the wgs84 position pt_*. diff --git a/src/FDM/groundcache.cxx b/src/FDM/groundcache.cxx index 90e79b864..f1643df02 100644 --- a/src/FDM/groundcache.cxx +++ b/src/FDM/groundcache.cxx @@ -24,144 +24,226 @@ # include "config.h" #endif -#include - #include -#include #include #include #include -#include -#include - -#include +#include +#include +#include +#include +#include #include #include #include #include #include -#include -#include #include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include
#include #include -#include #include "flight.hxx" #include "groundcache.hxx" -using namespace osg; -using namespace osgUtil; using namespace simgear; -void makePolytopeShaft(Polytope& polyt, const Vec3d& refPoint, - const Vec3d& direction, double radius) -{ - polyt.clear(); - // Choose best principal axis to start making orthogonal axis. - Vec3d majorAxis; - if (fabs(direction.x()) <= fabs(direction.y())) { - if (fabs(direction.z()) <= fabs(direction.x())) - majorAxis = Vec3d(0.0, 0.0, 1.0); - else - majorAxis = Vec3d(1.0, 0.0, 0.0); - } else { - if (fabs(direction.z()) <= fabs(direction.y())) - majorAxis = Vec3d(0.0, 0.0, 1.0); - else - majorAxis = Vec3d(0.0, 1.0, 0.0); - } - Vec3d axis1 = majorAxis ^ direction; - axis1.normalize(); - Vec3d axis2 = direction ^ axis1; - - polyt.add(Plane(-axis1, refPoint + axis1 * radius)); - polyt.add(Plane(axis1, refPoint - axis1 * radius)); - polyt.add(Plane(-axis2, refPoint + axis2 * radius)); - polyt.add(Plane(axis2 , refPoint - axis2 * radius)); -} - -void makePolytopeBox(Polytope& polyt, const Vec3d& center, - const Vec3d& direction, double radius) +static FGInterface::GroundType +materialToGroundType(const SGMaterial* material) { - makePolytopeShaft(polyt, center, direction, radius); - polyt.add(Plane(-direction, center + direction * radius)); - polyt.add(Plane(direction, center - direction * radius)); + if (!material) + return FGInterface::Solid; + if (material->get_solid()) + return FGInterface::Solid; + return FGInterface::Water; } -// Intersector for finding catapults and wires - -class WireIntersector : public PolytopeIntersector -{ +class FGGroundCache::CacheFill : public osg::NodeVisitor { public: - WireIntersector(const Polytope& polytope) - : PolytopeIntersector(polytope), depth(0) + CacheFill(const SGVec3d& center, const double& radius, + const double& startTime, const double& endTime) : + osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN), + _center(center), + _radius(radius), + _startTime(startTime), + _endTime(endTime) + { + setTraversalMask(SG_NODEMASK_TERRAIN_BIT); + } + virtual void apply(osg::Node& node) { - setDimensionMask(DimOne); + if (!testBoundingSphere(node.getBound())) + return; + + addBoundingVolume(node); } + + virtual void apply(osg::Group& group) + { + if (!testBoundingSphere(group.getBound())) + return; - bool enter(const osg::Node& node) + simgear::BVHSubTreeCollector::NodeList parentNodeList; + mSubTreeCollector.pushNodeList(parentNodeList); + + traverse(group); + addBoundingVolume(group); + + mSubTreeCollector.popNodeList(parentNodeList); + } + + virtual void apply(osg::Transform& transform) + { handleTransform(transform); } + virtual void apply(osg::Camera& camera) { - if (!PolytopeIntersector::enter(node)) - return false; - const Referenced* base = node.getUserData(); - if (base) { - const FGAICarrierHardware *ud - = dynamic_cast(base); - if (ud) - carriers.push_back(depth); + if (camera.getRenderOrder() != osg::Camera::NESTED_RENDER) + return; + handleTransform(camera); + } + virtual void apply(osg::CameraView& transform) + { handleTransform(transform); } + virtual void apply(osg::MatrixTransform& transform) + { handleTransform(transform); } + virtual void apply(osg::PositionAttitudeTransform& transform) + { handleTransform(transform); } + + void handleTransform(osg::Transform& transform) + { + // Hmm, may be this needs to be refined somehow ... + if (transform.getReferenceFrame() != osg::Transform::RELATIVE_RF) + return; + + if (!testBoundingSphere(transform.getBound())) + return; + + osg::Matrix inverseMatrix; + if (!transform.computeWorldToLocalMatrix(inverseMatrix, this)) + return; + osg::Matrix matrix; + if (!transform.computeLocalToWorldMatrix(matrix, this)) + return; + + // Look for a velocity note + const SGSceneUserData::Velocity* velocity = getVelocity(transform); + // ... no velocity of there is only zero velocity + if (velocity && velocity->linear == SGVec3d::zeros() && + velocity->angular == SGVec3d::zeros()) + velocity = 0; + + SGVec3d center = _center; + _center = SGVec3d(inverseMatrix.preMult(_center.osg())); + double radius = _radius; + if (velocity) + _radius += (_endTime - _startTime)*norm(velocity->linear); + + simgear::BVHSubTreeCollector::NodeList parentNodeList; + mSubTreeCollector.pushNodeList(parentNodeList); + + addBoundingVolume(transform); + traverse(transform); + + if (mSubTreeCollector.haveChildren()) { + if (velocity) { + simgear::BVHMotionTransform* bvhTransform; + bvhTransform = new simgear::BVHMotionTransform; + bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr())); + bvhTransform->setLinearVelocity(velocity->linear); + bvhTransform->setAngularVelocity(velocity->angular); + bvhTransform->setReferenceTime(_startTime); + bvhTransform->setEndTime(_endTime); + + mSubTreeCollector.popNodeList(parentNodeList, bvhTransform); + } else { + simgear::BVHTransform* bvhTransform; + bvhTransform = new simgear::BVHTransform; + bvhTransform->setToWorldTransform(SGMatrixd(matrix.ptr())); + + mSubTreeCollector.popNodeList(parentNodeList, bvhTransform); + } + } else { + mSubTreeCollector.popNodeList(parentNodeList); } - depth++; - return true; + _center = center; + _radius = radius; } - void leave() + const SGSceneUserData::Velocity* getVelocity(osg::Node& node) { - depth--; - if (!carriers.empty() && depth == carriers.back()) - carriers.pop_back(); + SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node); + if (!userData) + return 0; + return userData->getVelocity(); } - - void intersect(IntersectionVisitor& iv, Drawable* drawable) + simgear::BVHNode* getNodeBoundingVolume(osg::Node& node) { - if (carriers.empty()) - return; - PolytopeIntersector::intersect(iv, drawable); + SGSceneUserData* userData = SGSceneUserData::getSceneUserData(&node); + if (!userData) + return 0; + return userData->getBVHNode(); } + void addBoundingVolume(osg::Node& node) + { + simgear::BVHNode* bvNode = getNodeBoundingVolume(node); + if (!bvNode) + return; - void reset() + // Get that part of the local bv tree that intersects our sphere + // of interrest. + mSubTreeCollector.setSphere(SGSphered(_center, _radius)); + bvNode->accept(mSubTreeCollector); + } + + bool testBoundingSphere(const osg::BoundingSphere& bound) const { - carriers.clear(); + if (!bound.valid()) + return false; + + double maxDist = bound._radius + _radius; + return distSqr(SGVec3d(bound._center), _center) <= maxDist*maxDist; } - std::vector carriers; - int depth; -}; + + SGSharedPtr getBVHNode() const + { return mSubTreeCollector.getNode(); } + +private: + + SGVec3d _center; + double _radius; + double _startTime; + double _endTime; -/// Ok, variant that uses a infinite line istead of the ray. -/// also not that this only works if the ray direction is normalized. -static inline bool -intersectsInf(const SGRayd& ray, const SGSphered& sphere) -{ - SGVec3d r = sphere.getCenter() - ray.getOrigin(); - double projectedDistance = dot(r, ray.getDirection()); - double dist = dot(r, r) - projectedDistance * projectedDistance; - return dist < sphere.getRadius2(); -} + simgear::BVHSubTreeCollector mSubTreeCollector; +}; FGGroundCache::FGGroundCache() : - ground_radius(0.0), - _type(0), - _material(0), - cache_ref_time(0.0), - wire_id(0), - reference_wgs84_point(SGVec3d(0, 0, 0)), - reference_vehicle_radius(0.0), - down(0.0, 0.0, 0.0), - found_ground(false) + _altitude(0), + _type(0), + _material(0), + cache_ref_time(0), + _wire(0), + reference_wgs84_point(SGVec3d(0, 0, 0)), + reference_vehicle_radius(0), + down(0.0, 0.0, 0.0), + found_ground(false) { } @@ -169,507 +251,516 @@ FGGroundCache::~FGGroundCache() { } -inline void -FGGroundCache::velocityTransformTriangle(double dt, - SGTriangled& dst, SGSphered& sdst, - const FGGroundCache::Triangle& src) +bool +FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, + double rad) { - dst = src.triangle; - sdst = src.sphere; - - if (dt*dt*dot(src.gp.vel, src.gp.vel) < SGLimitsd::epsilon()) - return; - - SGVec3d baseVert = dst.getBaseVertex(); - SGVec3d pivotoff = baseVert - src.gp.pivot; - baseVert += dt*(src.gp.vel + cross(src.gp.rot, pivotoff)); - dst.setBaseVertex(baseVert); - dst.setEdge(0, dst.getEdge(0) + dt*cross(src.gp.rot, dst.getEdge(0))); - dst.setEdge(1, dst.getEdge(1) + dt*cross(src.gp.rot, dst.getEdge(1))); + // Empty cache. + found_ground = false; + + SGGeod geodPt = SGGeod::fromCart(pt); + // Don't blow away the cache ground_radius and stuff if there's no + // scenery + if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(), + geodPt.getLongitudeDeg(), + rad)) + return false; + _altitude = 0; + + // If we have an active wire, get some more area into the groundcache + if (_wire) + rad = SGMiscd::max(200, rad); + + // Store the parameters we used to build up that cache. + 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(geodPt); + down = hlToEc.rotate(SGVec3d(0, 0, 1)); + + // Get the ground cache, that is a local collision tree of the environment + double endTime = cache_ref_time + 1; //FIXME?? + CacheFill subtreeCollector(pt, rad, cache_ref_time, endTime); + globals->get_scenery()->get_scene_graph()->accept(subtreeCollector); + _localBvhTree = subtreeCollector.getBVHNode(); + + // Try to get a croase altitude value for the ground cache + SGLineSegmentd line(pt, pt + 1e4*down); + simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, ref_time); + if (_localBvhTree) + _localBvhTree->accept(lineSegmentVisitor); + + // If this is successful, store this altitude for croase altitude values + if (!lineSegmentVisitor.empty()) { + SGGeod geodPt = SGGeod::fromCart(lineSegmentVisitor.getPoint()); + _altitude = geodPt.getElevationM(); + _material = lineSegmentVisitor.getMaterial(); + _type = materialToGroundType(_material); + found_ground = true; + } else { + // Else do a crude scene query for the current point + found_ground = globals->get_scenery()-> + get_cart_elevation_m(pt, rad, _altitude, &_material); + _type = materialToGroundType(_material); + } + + // Still not sucessful?? + 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; } -void FGGroundCache::getGroundProperty(Drawable* drawable, - const NodePath& nodePath, - FGGroundCache::GroundProperty& gp, - bool& backfaceCulling) +bool +FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) { - gp.type = FGInterface::Unknown; - gp.wire_id = 0; - gp.vel = SGVec3d(0.0, 0.0, 0.0); - gp.rot = SGVec3d(0.0, 0.0, 0.0); - gp.pivot = SGVec3d(0.0, 0.0, 0.0); - gp.material = 0; - backfaceCulling = false; - // XXX state set might be higher up in scene graph - gp.material = SGMaterialLib::findMaterial(drawable->getStateSet()); - if (gp.material) - gp.type = (gp.material->get_solid() ? FGInterface::Solid - : FGInterface::Water); - for (NodePath::const_iterator iter = nodePath.begin(), e = nodePath.end(); - iter != e; - ++iter) { - Node* node = *iter; - StateSet* stateSet = node->getStateSet(); - StateAttribute* stateAttribute = 0; - if (stateSet && (stateAttribute - = stateSet->getAttribute(StateAttribute::CULLFACE))) { - backfaceCulling - = (static_cast(stateAttribute)->getMode() - == CullFace::BACK); - } - - // get some material information for use in the gear model - Referenced* base = node->getUserData(); - if (!base) - continue; - FGAICarrierHardware *ud = dynamic_cast(base); - if (!ud) - continue; - 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); - break; - } + pt = reference_wgs84_point; + rad = reference_vehicle_radius; + ref_time = cache_ref_time; + return found_ground; } -void FGGroundCache::getTriIntersectorResults(PolytopeIntersector* triInt) -{ - const PolytopeIntersector::Intersections& intersections - = triInt->getIntersections(); - Drawable* lastDrawable = 0; - RefMatrix* lastMatrix = 0; - Matrix worldToLocal; - GroundProperty gp; - bool backfaceCulling = false; - for (PolytopeIntersector::Intersections::const_iterator - itr = intersections.begin(), e = intersections.end(); - itr != e; - ++itr) { - const PolytopeIntersector::Intersection& intr = *itr; - if (intr.drawable.get() != lastDrawable) { - getGroundProperty(intr.drawable.get(), intr.nodePath, gp, - backfaceCulling); - lastDrawable = intr.drawable.get(); - } - Primitive triPrim = getPrimitive(intr.drawable, intr.primitiveIndex); - if (triPrim.numVerts != 3) - continue; - SGVec3d v[3] = { SGVec3d(triPrim.vertices[0]), - SGVec3d(triPrim.vertices[1]), - SGVec3d(triPrim.vertices[2]) - }; - RefMatrix* mat = intr.matrix.get(); - // If the drawable is the same then the intersection model - // matrix should be the same, because it is only set by nodes - // in the scene graph. However, do an extra test in case - // something funny is going on with the drawable. - if (mat != lastMatrix) { - lastMatrix = mat; - worldToLocal = Matrix::inverse(*mat); - } - SGVec3d localCacheReference; - localCacheReference.osg() = reference_wgs84_point.osg() * worldToLocal; - SGVec3d localDown; - localDown.osg() = Matrixd::transform3x3(down.osg(), worldToLocal); - // a bounding sphere in the node local system - SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]); - double boundRadius = std::max(distSqr(v[0], boundCenter), - distSqr(v[1], boundCenter)); - boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter)); - boundRadius = sqrt(boundRadius); - SGRayd ray(localCacheReference, localDown); - SGTriangled triangle(v); - // The normal and plane in the node local coordinate system - SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1)); - if (0 < dot(localDown, n)) { - if (backfaceCulling) { - // Surface points downwards, ignore for altitude computations. - continue; - } else { - triangle.flip(); - } - } +class FGGroundCache::CatapultFinder : public BVHVisitor { +public: + CatapultFinder(const SGSphered& sphere, const double& t) : + _sphere(sphere), + _time(t), + _haveLineSegment(false) + { } - // Only check if the triangle is in the cache sphere if the plane - // containing the triangle is near enough - double d = dot(n, v[0] - localCacheReference); - if (d*d < reference_vehicle_radius*dot(n, n)) { - // Check if the sphere around the vehicle intersects the sphere - // around that triangle. If so, put that triangle into the cache. - double r2 = boundRadius + reference_vehicle_radius; - if (distSqr(boundCenter, localCacheReference) < r2*r2) { - FGGroundCache::Triangle t; - t.triangle.setBaseVertex(SGVec3d(v[0].osg() * *mat)); - t.triangle.setEdge(0, SGVec3d(Matrixd:: - transform3x3(triangle - .getEdge(0).osg(), - *mat))); - t.triangle.setEdge(1, SGVec3d(Matrixd:: - transform3x3(triangle - .getEdge(1).osg(), - *mat))); - t.sphere.setCenter(SGVec3d(boundCenter.osg()* *mat)); - t.sphere.setRadius(boundRadius); - t.gp = gp; - 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. - SGVec3d isectpoint; - if (intersects(isectpoint, triangle, ray, 1e-4)) { - found_ground = true; - isectpoint.osg() = isectpoint.osg() * *mat; - double this_radius = length(isectpoint); - if (ground_radius < this_radius) { - ground_radius = this_radius; - _type = gp.type; - _material = gp.material; - } - } + virtual void apply(BVHGroup& leaf) + { + if (!intersects(_sphere, leaf.getBoundingSphere())) + return; + leaf.traverse(*this); } -} - -void FGGroundCache::getWireIntersectorResults(WireIntersector* wireInt, - double wireCacheRadius) -{ - const WireIntersector::Intersections& intersections - = wireInt->getIntersections(); - Drawable* lastDrawable = 0; - GroundProperty gp; - bool backfaceCulling = false; - for (PolytopeIntersector::Intersections::const_iterator - itr = intersections.begin(), e = intersections.end(); - itr != e; - ++itr) { - if (itr->drawable.get() != lastDrawable) { - getGroundProperty(itr->drawable.get(), itr->nodePath, gp, - backfaceCulling); - lastDrawable = itr->drawable.get(); - } - Primitive linePrim = getPrimitive(itr->drawable, itr->primitiveIndex); - if (linePrim.numVerts != 2) - continue; - RefMatrix* mat = itr->matrix.get(); - SGVec3d gv1(osg::Vec3d(linePrim.vertices[0]) * *mat); - SGVec3d gv2(osg::Vec3d(linePrim.vertices[1]) * *mat); - - SGVec3d boundCenter = 0.5*(gv1 + gv2); - double boundRadius = length(gv1 - boundCenter); - - if (distSqr(boundCenter, reference_wgs84_point) - < (boundRadius + wireCacheRadius)*(boundRadius + wireCacheRadius)) { - if (gp.type == FGInterface::Wire) { - FGGroundCache::Wire wire; - wire.ends[0] = gv1; - wire.ends[1] = gv2; - wire.gp = gp; - wires.push_back(wire); - } else if (gp.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 (linePrim.vertices[0][0] > linePrim.vertices[1][0]) { - cat.start = gv1; - cat.end = gv2; - } else { - cat.start = gv2; - cat.end = gv1; - } - cat.gp = gp; - catapults.push_back(cat); - } + virtual void apply(BVHTransform& transform) + { + if (!intersects(_sphere, transform.getBoundingSphere())) + return; + + SGSphered sphere = _sphere; + _sphere = transform.sphereToLocal(sphere); + bool haveLineSegment = _haveLineSegment; + _haveLineSegment = false; + + transform.traverse(*this); + + if (_haveLineSegment) { + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); } + _haveLineSegment |= haveLineSegment; + _sphere.setCenter(sphere.getCenter()); + } + virtual void apply(BVHMotionTransform& transform) + { + if (!intersects(_sphere, transform.getBoundingSphere())) + return; + SGSphered sphere = _sphere; + _sphere = transform.sphereToLocal(sphere, _time); + bool haveLineSegment = _haveLineSegment; + _haveLineSegment = false; + + transform.traverse(*this); + + if (_haveLineSegment) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + _haveLineSegment |= haveLineSegment; + _sphere.setCenter(sphere.getCenter()); } -} + virtual void apply(BVHLineGeometry& node) + { + if (node.getType() != BVHLineGeometry::CarrierCatapult) + return; -bool -FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt, - double rad) -{ - // Empty cache. - found_ground = false; - SGGeod geodPt = SGGeod::fromCart(pt); - // Don't blow away the cache ground_radius and stuff if there's no - // scenery - if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(), - geodPt.getLongitudeDeg(), - rad)) - return false; - ground_radius = 0.0; - triangles.resize(0); - catapults.resize(0); - wires.resize(0); - - // Store the parameters we used to build up that cache. - 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(geodPt); - down = hlToEc.rotate(SGVec3d(0, 0, 1)); - - // Prepare sphere around the aircraft. - 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; - double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist; - - Polytope triPolytope; - makePolytopeShaft(triPolytope, pt.osg(), down.osg(), cacheRadius); - ref_ptr triIntersector - = new PolytopeIntersector(triPolytope); - triIntersector->setDimensionMask(PolytopeIntersector::DimTwo); - Polytope wirePolytope; - makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius); - ref_ptr wireIntersector = new WireIntersector(wirePolytope); - wireIntersector->setDimensionMask(PolytopeIntersector::DimOne); - ref_ptr intersectors = new IntersectorGroup; - intersectors->addIntersector(triIntersector.get()); - intersectors->addIntersector(wireIntersector.get()); - - // Walk the scene graph and extract solid ground triangles and - // carrier data. - IntersectionVisitor iv(intersectors); - iv.setTraversalMask(SG_NODEMASK_TERRAIN_BIT); - globals->get_scenery()->get_scene_graph()->accept(iv); - getTriIntersectorResults(triIntersector.get()); - getWireIntersectorResults(wireIntersector.get(), wireCacheRadius); - - // some stats - 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 - // 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; -} + SGLineSegmentd lineSegment(node.getLineSegment()); + if (!intersects(_sphere, lineSegment)) + return; -bool -FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad) -{ - pt = reference_wgs84_point; - rad = reference_vehicle_radius; - ref_time = cache_ref_time; - return found_ground; -} + _lineSegment = lineSegment; + double dist = distSqr(lineSegment, getSphere().getCenter()); + _sphere.setRadius(sqrt(dist)); + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _haveLineSegment = true; + } + virtual void apply(BVHStaticGeometry& node) + { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + void setSphere(const SGSphered& sphere) + { _sphere = sphere; } + const SGSphered& getSphere() const + { return _sphere; } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + + bool getHaveLineSegment() const + { return _haveLineSegment; } + +protected: + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _haveLineSegment; + + SGSphered _sphere; + double _time; +}; double -FGGroundCache::get_cat(double t, const SGVec3d& dpt, +FGGroundCache::get_cat(double t, const SGVec3d& pt, 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; - - size_t sz = catapults.size(); - for (size_t i = 0; i < sz; ++i) { - SGVec3d pivotoff, rvel[2]; - pivotoff = catapults[i].start - catapults[i].gp.pivot; - rvel[0] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff); - pivotoff = catapults[i].end - catapults[i].gp.pivot; - rvel[1] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff); - - SGVec3d thisEnd[2]; - thisEnd[0] = catapults[i].start + t*rvel[0]; - thisEnd[1] = catapults[i].end + t*rvel[1]; - - double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt); - if (this_dist < dist) { - SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult " - << this_dist << " meters away"); - dist = this_dist; - - end[0] = thisEnd[0]; - end[1] = thisEnd[1]; - vel[0] = rvel[0]; - vel[1] = rvel[1]; - } - } + double maxDistance = 1000; - // At the end take the root, we only computed squared distances ... - return sqrt(dist); + // Get the wire in question + CatapultFinder catapultFinder(SGSphered(pt, maxDistance), t); + if (_localBvhTree) + _localBvhTree->accept(catapultFinder); + + if (!catapultFinder.getHaveLineSegment()) + return maxDistance; + + // prepare the returns + end[0] = catapultFinder.getLineSegment().getStart(); + end[1] = catapultFinder.getLineSegment().getEnd(); + + // The linear velocity is the one at the start of the line segment ... + vel[0] = catapultFinder.getLinearVelocity(); + // ... so the end point has the additional cross product. + vel[1] = catapultFinder.getLinearVelocity(); + vel[1] += cross(catapultFinder.getAngularVelocity(), + catapultFinder.getLineSegment().getDirection()); + + // Return the distance to the cat + return sqrt(distSqr(catapultFinder.getLineSegment(), pt)); } bool -FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff, +FGGroundCache::get_agl(double t, const SGVec3d& pt, 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; - 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. - SGVec3d pt = dpt; - // shift the start of our ray by maxaltoff upwards - SGRayd ray(pt - max_altoff*down, down); - - // Initialize to something sensible - double current_radius = 0.0; - - size_t sz = triangles.size(); - for (size_t i = 0; i < sz; ++i) { - SGSphered sphere; - SGTriangled triangle; - velocityTransformTriangle(t, triangle, sphere, triangles[i]); - if (!intersectsInf(ray, sphere)) - continue; - - // Check for intersection. - SGVec3d isecpoint; - if (intersects(isecpoint, triangle, ray, 1e-4)) { - // Compute the vector from pt to the intersection point ... - SGVec3d off = isecpoint - pt; - // ... and check if it is too high or not - - // 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. - normal = triangle.getNormal(); - // The velocity wrt earth. - SGVec3d pivotoff = pt - triangles[i].gp.pivot; - vel = triangles[i].gp.vel + cross(triangles[i].gp.rot, pivotoff); - // Save the ground type. - *type = triangles[i].gp.type; - *agl = dot(down, contact - dpt); + // Just set up a ground intersection query for the given point + SGLineSegmentd line(pt - max_altoff*down, pt + 1e4*down); + simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t); + if (_localBvhTree) + _localBvhTree->accept(lineSegmentVisitor); + + if (!lineSegmentVisitor.empty()) { + // Have an intersection + contact = lineSegmentVisitor.getPoint(); + normal = lineSegmentVisitor.getNormal(); + if (0 < dot(normal, down)) + normal = -normal; + *agl = dot(down, contact - pt); + vel = lineSegmentVisitor.getLinearVelocity(); + // correct the linear velocity, since the line intersector delivers + // values for the start point and the get_agl function should + // traditionally deliver for the contact point + vel += cross(lineSegmentVisitor.getAngularVelocity(), + contact - line.getStart()); + *type = materialToGroundType(lineSegmentVisitor.getMaterial()); if (material) - *material = triangles[i].gp.material; - } - } - } + *material = lineSegmentVisitor.getMaterial(); - if (ret) - return true; + return true; + } else { + // 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. + SGGeod geodPt = SGGeod::fromCart(pt); + *agl = geodPt.getElevationM() - _altitude; + geodPt.setElevationM(_altitude); + contact = SGVec3d::fromGeod(geodPt); + normal = -down; + vel = SGVec3d(0, 0, 0); + *type = _type; + if (material) + *material = _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 = 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 = dot(down, contact - dpt); - *type = _type; - if (material) - *material = _material; - - return ret; + return found_ground; + } } -bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4]) -{ - size_t sz = wires.size(); - if (sz == 0) - return false; - - // Time difference to the reference time. - t -= cache_ref_time; - - // Build the two triangles spanning the area where the hook has moved - // during the past step. - SGTriangled triangle[2]; - triangle[0].set(pt[0], pt[1], pt[2]); - triangle[1].set(pt[0], pt[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].gp.pivot; - SGVec3d vel = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff); - le[k] += t*vel; +class FGGroundCache::WireIntersector : public BVHVisitor { +public: + WireIntersector(const SGVec3d pt[4], const double& t) : + _linearVelocity(SGVec3d::zeros()), + _angularVelocity(SGVec3d::zeros()), + _wire(0), + _time(t) + { + // Build the two triangles spanning the area where the hook has moved + // during the past step. + _triangles[0].set(pt[0], pt[1], pt[2]); + _triangles[1].set(pt[0], pt[2], pt[3]); + } + + virtual void apply(BVHGroup& leaf) + { + if (!_intersects(leaf.getBoundingSphere())) + return; + + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (!_intersects(transform.getBoundingSphere())) + return; + + SGTriangled triangles[2] = { _triangles[0], _triangles[1] }; + _triangles[0] = triangles[0].transform(transform.getToLocalTransform()); + _triangles[1] = triangles[1].transform(transform.getToLocalTransform()); + + transform.traverse(*this); + + if (_wire) { + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + } + _triangles[0] = triangles[0]; + _triangles[1] = triangles[1]; } - SGLineSegmentd lineSegment(le[0], le[1]); + virtual void apply(BVHMotionTransform& transform) + { + if (!_intersects(transform.getBoundingSphere())) + return; + + SGMatrixd toLocal = transform.getToLocalTransform(_time); + + SGTriangled triangles[2] = { _triangles[0], _triangles[1] }; + _triangles[0] = triangles[0].transform(toLocal); + _triangles[1] = triangles[1].transform(toLocal); + + transform.traverse(*this); + + if (_wire) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + _triangles[0] = triangles[0]; + _triangles[1] = triangles[1]; + } + virtual void apply(BVHLineGeometry& node) + { + if (node.getType() != BVHLineGeometry::CarrierWire) + return; + SGLineSegmentd lineSegment(node.getLineSegment()); + if (!_intersects(lineSegment)) + return; + + _lineSegment = lineSegment; + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _wire = &node; + } + virtual void apply(BVHStaticGeometry& node) + { } - for (int k=0; k<2; ++k) { - if (intersects(triangle[k], lineSegment)) { - SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire"); - // Store the wire id. - wire_id = wires[i].gp.wire_id; - return true; - } + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + bool _intersects(const SGSphered& sphere) const + { + if (_wire) + return false; + if (intersects(_triangles[0], sphere)) + return true; + if (intersects(_triangles[1], sphere)) + return true; + return false; } - } + bool _intersects(const SGLineSegmentd& lineSegment) const + { + if (_wire) + return false; + if (intersects(_triangles[0], lineSegment)) + return true; + if (intersects(_triangles[1], lineSegment)) + return true; + return false; + } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + + const BVHLineGeometry* getWire() const + { return _wire; } + +private: + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + const BVHLineGeometry* _wire; + + SGTriangled _triangles[2]; + double _time; +}; - return false; +bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4]) +{ + // Get the wire in question + WireIntersector wireIntersector(pt, t); + if (_localBvhTree) + _localBvhTree->accept(wireIntersector); + + _wire = wireIntersector.getWire(); + return _wire; } +class FGGroundCache::WireFinder : public BVHVisitor { +public: + WireFinder(const BVHLineGeometry* wire, const double& t) : + _wire(wire), + _time(t), + _lineSegment(SGVec3d::zeros(), SGVec3d::zeros()), + _linearVelocity(SGVec3d::zeros()), + _angularVelocity(SGVec3d::zeros()), + _haveLineSegment(false) + { } + + virtual void apply(BVHGroup& leaf) + { + if (_haveLineSegment) + return; + leaf.traverse(*this); + } + virtual void apply(BVHTransform& transform) + { + if (_haveLineSegment) + return; + + transform.traverse(*this); + + if (_haveLineSegment) { + _linearVelocity = transform.vecToWorld(_linearVelocity); + _angularVelocity = transform.vecToWorld(_angularVelocity); + _lineSegment = transform.lineSegmentToWorld(_lineSegment); + } + } + virtual void apply(BVHMotionTransform& transform) + { + if (_haveLineSegment) + return; + + transform.traverse(*this); + + if (_haveLineSegment) { + SGMatrixd toWorld = transform.getToWorldTransform(_time); + _linearVelocity + += transform.getLinearVelocityAt(_lineSegment.getStart()); + _angularVelocity += transform.getAngularVelocity(); + _linearVelocity = toWorld.xformVec(_linearVelocity); + _angularVelocity = toWorld.xformVec(_angularVelocity); + _lineSegment = _lineSegment.transform(toWorld); + } + } + virtual void apply(BVHLineGeometry& node) + { + if (_haveLineSegment) + return; + if (_wire != &node) + return; + if (node.getType() != BVHLineGeometry::CarrierWire) + return; + _lineSegment = SGLineSegmentd(node.getLineSegment()); + _linearVelocity = SGVec3d::zeros(); + _angularVelocity = SGVec3d::zeros(); + _haveLineSegment = true; + } + virtual void apply(BVHStaticGeometry&) { } + + virtual void apply(const BVHStaticBinary&, const BVHStaticData&) { } + virtual void apply(const BVHStaticLeaf&, const BVHStaticData&) { } + virtual void apply(const BVHStaticTriangle&, const BVHStaticData&) { } + + const SGLineSegmentd& getLineSegment() const + { return _lineSegment; } + + bool getHaveLineSegment() const + { return _haveLineSegment; } + + const SGVec3d& getLinearVelocity() const + { return _linearVelocity; } + const SGVec3d& getAngularVelocity() const + { return _angularVelocity; } + +private: + const BVHLineGeometry* _wire; + double _time; + + SGLineSegmentd _lineSegment; + SGVec3d _linearVelocity; + SGVec3d _angularVelocity; + + bool _haveLineSegment; +}; + 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; - - // Time difference to the reference time. - t -= cache_ref_time; - - // 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].gp.wire_id == wire_id) { - for (size_t k = 0; k < 2; ++k) { - SGVec3d pivotoff = wires[i].ends[k] - wires[i].gp.pivot; - vel[k] = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff); - end[k] = wires[i].ends[k] + t*vel[k]; - } - return true; - } - } + // Fast return if we do not have an active wire. + if (!_wire) + return false; + + // Get the wire in question + WireFinder wireFinder(_wire, t); + if (_localBvhTree) + _localBvhTree->accept(wireFinder); + + if (!wireFinder.getHaveLineSegment()) + return false; - return false; + // prepare the returns + end[0] = wireFinder.getLineSegment().getStart(); + end[1] = wireFinder.getLineSegment().getEnd(); + + // The linear velocity is the one at the start of the line segment ... + vel[0] = wireFinder.getLinearVelocity(); + // ... so the end point has the additional cross product. + vel[1] = wireFinder.getLinearVelocity(); + vel[1] += cross(wireFinder.getAngularVelocity(), + wireFinder.getLineSegment().getDirection()); + + return true; } void FGGroundCache::release_wire(void) { - wire_id = -1; + _wire = 0; } diff --git a/src/FDM/groundcache.hxx b/src/FDM/groundcache.hxx index 58996a811..ea8cc16d7 100644 --- a/src/FDM/groundcache.hxx +++ b/src/FDM/groundcache.hxx @@ -23,20 +23,20 @@ #ifndef _GROUNDCACHE_HXX #define _GROUNDCACHE_HXX -#include - -namespace osgUtil -{ -class PolytopeIntersector; -} +#include +#include #include #include #include #include +#include +#include class SGMaterial; -class WireIntersector; +namespace simgear { +class BVHLineGeometry; +} class FGGroundCache { public: @@ -89,42 +89,14 @@ public: void release_wire(void); private: - friend class GroundCacheFillVisitor; - - - // Helper class to hold some properties of the ground triangle. - struct GroundProperty { - GroundProperty() : type(0), wire_id(0), material(0) {} - int type; - int wire_id; - // The linear and angular velocity. - SGVec3d vel; - SGVec3d rot; - SGVec3d pivot; - // the simgear material reference, contains friction coeficients ... - const SGMaterial* material; - }; - - struct Triangle { - GroundProperty gp; - // The triangle we represent - SGTriangled triangle; - SGSphered sphere; - }; - struct Catapult { - GroundProperty gp; - SGVec3d start; - SGVec3d end; - }; - struct Wire { - GroundProperty gp; - SGVec3d ends[2]; - }; - + class CacheFill; + class CatapultFinder; + class WireIntersector; + class WireFinder; // Approximate ground radius. // In case the aircraft is too high above ground. - double ground_radius; + double _altitude; // Ground type int _type; // the simgear material reference, contains friction coeficients ... @@ -132,13 +104,8 @@ private: // 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; - - // Containers which hold all the essential information about this cache. - std::vector triangles; - std::vector catapults; - std::vector wires; + // The wire to track. + const simgear::BVHLineGeometry* _wire; // The point and radius where the cache is built around. // That are the arguments that were given to prepare_ground_cache. @@ -147,14 +114,7 @@ private: SGVec3d down; bool found_ground; - void getGroundProperty(osg::Drawable* drawable, - const osg::NodePath& nodePath, - GroundProperty& gp, bool& backfaceCulling); - static void velocityTransformTriangle(double dt, SGTriangled& dst, - SGSphered& sdst, const Triangle& src); - void getTriIntersectorResults(osgUtil::PolytopeIntersector* triInt); - void getWireIntersectorResults(WireIntersector* wireInt, - double wireCacheRadius); + SGSharedPtr _localBvhTree; }; #endif