#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) { makePolytopeShaft(polyt, center, direction, radius); polyt.add(Plane(-direction, center + direction * radius)); polyt.add(Plane(direction, center - direction * radius)); } // Intersector for finding catapults and wires class WireIntersector : public PolytopeIntersector { public: WireIntersector(const Polytope& polytope) : PolytopeIntersector(polytope), depth(0) { setDimensionMask(DimOne); } bool enter(const osg::Node& node) { 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); } depth++; return true; } void leave() { depth--; if (!carriers.empty() && depth == carriers.back()) carriers.pop_back(); } void intersect(IntersectionVisitor& iv, Drawable* drawable) { if (carriers.empty()) return; PolytopeIntersector::intersect(iv, drawable); } void reset() { carriers.clear(); } std::vector carriers; int depth; }; /// 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(); } 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) { } FGGroundCache::~FGGroundCache() { } inline void FGGroundCache::velocityTransformTriangle(double dt, SGTriangled& dst, SGSphered& sdst, const FGGroundCache::Triangle& src) { 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))); } void FGGroundCache::getGroundProperty(Drawable* drawable, const NodePath& nodePath, FGGroundCache::GroundProperty& gp, bool& backfaceCulling) { 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; } } 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(); } } // 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; } } } } 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); } } } } 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; } 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; } double 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; 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]; } } // At the end take the root, we only computed squared distances ... return sqrt(dist); } bool 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; 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); if (material) *material = triangles[i].gp.material; } } } if (ret) return true; // Whenever we did not have a ground triangle for the requested point, // take the ground level we found during the current cache build. // This is as good as what we had before for agl. double r = 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; } 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; } SGLineSegmentd lineSegment(le[0], le[1]); 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; } } } return false; } 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; } } return false; } void FGGroundCache::release_wire(void) { wire_id = -1; }