- // 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<PolytopeIntersector> triIntersector
- = new PolytopeIntersector(triPolytope);
- triIntersector->setDimensionMask(PolytopeIntersector::DimTwo);
- Polytope wirePolytope;
- makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius);
- ref_ptr<WireIntersector> wireIntersector = new WireIntersector(wirePolytope);
- wireIntersector->setDimensionMask(PolytopeIntersector::DimOne);
- ref_ptr<IntersectorGroup> 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;
+#ifdef GROUNDCACHE_DEBUG
+ SGTimeStamp t0 = SGTimeStamp::now();
+#endif
+
+ // Just set up a ground intersection query for the given point
+ SGLineSegmentd line(pt, pt + 10*reference_vehicle_radius*down);
+ t += cache_time_offset;
+ simgear::BVHLineSegmentVisitor lineSegmentVisitor(line, t);
+ if (_localBvhTree)
+ _localBvhTree->accept(lineSegmentVisitor);
+
+#ifdef GROUNDCACHE_DEBUG
+ t0 = SGTimeStamp::now() - t0;
+ _lookupTime += t0;
+ _lookupCount++;
+#endif
+
+ if (!lineSegmentVisitor.empty()) {
+ // Have an intersection
+ contact = lineSegmentVisitor.getPoint();
+ normal = lineSegmentVisitor.getNormal();
+ if (0 < dot(normal, down))
+ normal = -normal;
+ linearVel = lineSegmentVisitor.getLinearVelocity();
+ angularVel = lineSegmentVisitor.getAngularVelocity();
+ material = lineSegmentVisitor.getMaterial();
+ id = lineSegmentVisitor.getId();
+
+ 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);
+ geodPt.setElevationM(_altitude);
+ contact = SGVec3d::fromGeod(geodPt);
+ normal = -down;
+ linearVel = SGVec3d(0, 0, 0);
+ angularVel = SGVec3d(0, 0, 0);
+ material = _material;
+ id = 0;
+
+ return found_ground;
+ }