1 // groundcache.cxx -- carries a small subset of the scenegraph near the vehicle
3 // Written by Mathias Froehlich, started Nov 2004.
5 // Copyright (C) 2004 Mathias Froehlich - Mathias.Froehlich@web.de
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
31 #include <osg/CullFace>
32 #include <osg/Drawable>
34 #include <osg/Geometry>
35 #include <osg/PrimitiveSet>
36 #include <osg/TriangleFunctor>
38 #include <osgUtil/PolytopeIntersector>
40 #include <simgear/sg_inlines.h>
41 #include <simgear/constants.h>
42 #include <simgear/debug/logstream.hxx>
43 #include <simgear/math/sg_geodesy.hxx>
44 #include <simgear/scene/material/mat.hxx>
45 #include <simgear/scene/material/matlib.hxx>
46 #include <simgear/scene/util/PrimitiveUtils.hxx>
47 #include <simgear/scene/util/SGNodeMasks.hxx>
49 #include <Main/globals.hxx>
50 #include <Scenery/scenery.hxx>
51 #include <Scenery/tilemgr.hxx>
52 #include <AIModel/AICarrier.hxx>
55 #include "groundcache.hxx"
58 using namespace osgUtil;
59 using namespace simgear;
61 void makePolytopeShaft(Polytope& polyt, const Vec3d& refPoint,
62 const Vec3d& direction, double radius)
65 // Choose best principal axis to start making orthogonal axis.
67 if (fabs(direction.x()) <= fabs(direction.y())) {
68 if (fabs(direction.z()) <= fabs(direction.x()))
69 majorAxis = Vec3d(0.0, 0.0, 1.0);
71 majorAxis = Vec3d(1.0, 0.0, 0.0);
73 if (fabs(direction.z()) <= fabs(direction.y()))
74 majorAxis = Vec3d(0.0, 0.0, 1.0);
76 majorAxis = Vec3d(0.0, 1.0, 0.0);
78 Vec3d axis1 = majorAxis ^ direction;
80 Vec3d axis2 = direction ^ axis1;
82 polyt.add(Plane(-axis1, refPoint + axis1 * radius));
83 polyt.add(Plane(axis1, refPoint - axis1 * radius));
84 polyt.add(Plane(-axis2, refPoint + axis2 * radius));
85 polyt.add(Plane(axis2 , refPoint - axis2 * radius));
88 void makePolytopeBox(Polytope& polyt, const Vec3d& center,
89 const Vec3d& direction, double radius)
91 makePolytopeShaft(polyt, center, direction, radius);
92 polyt.add(Plane(-direction, center + direction * radius));
93 polyt.add(Plane(direction, center - direction * radius));
96 // Intersector for finding catapults and wires
98 class WireIntersector : public PolytopeIntersector
101 WireIntersector(const Polytope& polytope)
102 : PolytopeIntersector(polytope), depth(0)
104 setDimensionMask(DimOne);
107 bool enter(const osg::Node& node)
109 if (!PolytopeIntersector::enter(node))
111 const Referenced* base = node.getUserData();
113 const FGAICarrierHardware *ud
114 = dynamic_cast<const FGAICarrierHardware*>(base);
116 carriers.push_back(depth);
125 if (!carriers.empty() && depth == carriers.back())
129 void intersect(IntersectionVisitor& iv, Drawable* drawable)
131 if (carriers.empty())
133 PolytopeIntersector::intersect(iv, drawable);
140 std::vector<int> carriers;
144 /// Ok, variant that uses a infinite line istead of the ray.
145 /// also not that this only works if the ray direction is normalized.
147 intersectsInf(const SGRayd& ray, const SGSphered& sphere)
149 SGVec3d r = sphere.getCenter() - ray.getOrigin();
150 double projectedDistance = dot(r, ray.getDirection());
151 double dist = dot(r, r) - projectedDistance * projectedDistance;
152 return dist < sphere.getRadius2();
155 FGGroundCache::FGGroundCache() :
161 reference_wgs84_point(SGVec3d(0, 0, 0)),
162 reference_vehicle_radius(0.0),
168 FGGroundCache::~FGGroundCache()
173 FGGroundCache::velocityTransformTriangle(double dt,
174 SGTriangled& dst, SGSphered& sdst,
175 const FGGroundCache::Triangle& src)
180 if (dt*dt*dot(src.gp.vel, src.gp.vel) < SGLimitsd::epsilon())
183 SGVec3d baseVert = dst.getBaseVertex();
184 SGVec3d pivotoff = baseVert - src.gp.pivot;
185 baseVert += dt*(src.gp.vel + cross(src.gp.rot, pivotoff));
186 dst.setBaseVertex(baseVert);
187 dst.setEdge(0, dst.getEdge(0) + dt*cross(src.gp.rot, dst.getEdge(0)));
188 dst.setEdge(1, dst.getEdge(1) + dt*cross(src.gp.rot, dst.getEdge(1)));
191 void FGGroundCache::getGroundProperty(Drawable* drawable,
192 const NodePath& nodePath,
193 FGGroundCache::GroundProperty& gp,
194 bool& backfaceCulling)
196 gp.type = FGInterface::Unknown;
198 gp.vel = SGVec3d(0.0, 0.0, 0.0);
199 gp.rot = SGVec3d(0.0, 0.0, 0.0);
200 gp.pivot = SGVec3d(0.0, 0.0, 0.0);
202 backfaceCulling = false;
203 // XXX state set might be higher up in scene graph
204 gp.material = SGMaterialLib::findMaterial(drawable->getStateSet());
206 gp.type = (gp.material->get_solid() ? FGInterface::Solid
207 : FGInterface::Water);
208 for (NodePath::const_iterator iter = nodePath.begin(), e = nodePath.end();
212 StateSet* stateSet = node->getStateSet();
213 StateAttribute* stateAttribute = 0;
214 if (stateSet && (stateAttribute
215 = stateSet->getAttribute(StateAttribute::CULLFACE))) {
217 = (static_cast<osg::CullFace*>(stateAttribute)->getMode()
221 // get some material information for use in the gear model
222 Referenced* base = node->getUserData();
225 FGAICarrierHardware *ud = dynamic_cast<FGAICarrierHardware*>(base);
229 case FGAICarrierHardware::Wire:
230 gp.type = FGInterface::Wire;
233 case FGAICarrierHardware::Catapult:
234 gp.type = FGInterface::Catapult;
237 gp.type = FGInterface::Solid;
240 // Copy the velocity from the carrier class.
241 ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
246 void FGGroundCache::getTriIntersectorResults(PolytopeIntersector* triInt)
248 const PolytopeIntersector::Intersections& intersections
249 = triInt->getIntersections();
250 Drawable* lastDrawable = 0;
251 RefMatrix* lastMatrix = 0;
254 bool backfaceCulling = false;
255 for (PolytopeIntersector::Intersections::const_iterator
256 itr = intersections.begin(), e = intersections.end();
259 const PolytopeIntersector::Intersection& intr = *itr;
260 if (intr.drawable.get() != lastDrawable) {
261 getGroundProperty(intr.drawable.get(), intr.nodePath, gp,
263 lastDrawable = intr.drawable.get();
265 Primitive triPrim = getPrimitive(intr.drawable, intr.primitiveIndex);
266 if (triPrim.numVerts != 3)
268 SGVec3d v[3] = { SGVec3d(triPrim.vertices[0]),
269 SGVec3d(triPrim.vertices[1]),
270 SGVec3d(triPrim.vertices[2])
272 RefMatrix* mat = intr.matrix.get();
273 // If the drawable is the same then the intersection model
274 // matrix should be the same, because it is only set by nodes
275 // in the scene graph. However, do an extra test in case
276 // something funny is going on with the drawable.
277 if (mat != lastMatrix) {
279 worldToLocal = Matrix::inverse(*mat);
281 SGVec3d localCacheReference;
282 localCacheReference.osg() = reference_wgs84_point.osg() * worldToLocal;
284 localDown.osg() = Matrixd::transform3x3(down.osg(), worldToLocal);
285 // a bounding sphere in the node local system
286 SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
287 double boundRadius = std::max(distSqr(v[0], boundCenter),
288 distSqr(v[1], boundCenter));
289 boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
290 boundRadius = sqrt(boundRadius);
291 SGRayd ray(localCacheReference, localDown);
292 SGTriangled triangle(v);
293 // The normal and plane in the node local coordinate system
294 SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
295 if (0 < dot(localDown, n)) {
296 if (backfaceCulling) {
297 // Surface points downwards, ignore for altitude computations.
304 // Only check if the triangle is in the cache sphere if the plane
305 // containing the triangle is near enough
306 double d = dot(n, v[0] - localCacheReference);
307 if (d*d < reference_vehicle_radius*dot(n, n)) {
308 // Check if the sphere around the vehicle intersects the sphere
309 // around that triangle. If so, put that triangle into the cache.
310 double r2 = boundRadius + reference_vehicle_radius;
311 if (distSqr(boundCenter, localCacheReference) < r2*r2) {
312 FGGroundCache::Triangle t;
313 t.triangle.setBaseVertex(SGVec3d(v[0].osg() * *mat));
314 t.triangle.setEdge(0, SGVec3d(Matrixd::
315 transform3x3(triangle
318 t.triangle.setEdge(1, SGVec3d(Matrixd::
319 transform3x3(triangle
322 t.sphere.setCenter(SGVec3d(boundCenter.osg()* *mat));
323 t.sphere.setRadius(boundRadius);
325 triangles.push_back(t);
328 // In case the cache is empty, we still provide agl computations.
329 // But then we use the old way of having a fixed elevation value for
330 // the whole lifetime of this cache.
332 if (intersects(isectpoint, triangle, ray, 1e-4)) {
334 isectpoint.osg() = isectpoint.osg() * *mat;
335 double this_radius = length(isectpoint);
336 if (ground_radius < this_radius) {
337 ground_radius = this_radius;
339 _material = gp.material;
345 void FGGroundCache::getWireIntersectorResults(WireIntersector* wireInt,
346 double wireCacheRadius)
348 const WireIntersector::Intersections& intersections
349 = wireInt->getIntersections();
350 Drawable* lastDrawable = 0;
352 bool backfaceCulling = false;
353 for (PolytopeIntersector::Intersections::const_iterator
354 itr = intersections.begin(), e = intersections.end();
357 if (itr->drawable.get() != lastDrawable) {
358 getGroundProperty(itr->drawable.get(), itr->nodePath, gp,
360 lastDrawable = itr->drawable.get();
362 Primitive linePrim = getPrimitive(itr->drawable, itr->primitiveIndex);
363 if (linePrim.numVerts != 2)
365 RefMatrix* mat = itr->matrix.get();
366 SGVec3d gv1(osg::Vec3d(linePrim.vertices[0]) * *mat);
367 SGVec3d gv2(osg::Vec3d(linePrim.vertices[1]) * *mat);
369 SGVec3d boundCenter = 0.5*(gv1 + gv2);
370 double boundRadius = length(gv1 - boundCenter);
372 if (distSqr(boundCenter, reference_wgs84_point)
373 < (boundRadius + wireCacheRadius)*(boundRadius + wireCacheRadius)) {
374 if (gp.type == FGInterface::Wire) {
375 FGGroundCache::Wire wire;
379 wires.push_back(wire);
380 } else if (gp.type == FGInterface::Catapult) {
381 FGGroundCache::Catapult cat;
382 // Trick to get the ends in the right order.
383 // Use the x axis in the original coordinate system. Choose the
384 // most negative x-axis as the one pointing forward
385 if (linePrim.vertices[0][0] > linePrim.vertices[1][0]) {
393 catapults.push_back(cat);
401 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
405 found_ground = false;
406 SGGeod geodPt = SGGeod::fromCart(pt);
407 // Don't blow away the cache ground_radius and stuff if there's no
409 if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
410 geodPt.getLongitudeDeg(),
418 // Store the parameters we used to build up that cache.
419 reference_wgs84_point = pt;
420 reference_vehicle_radius = rad;
421 // Store the time reference used to compute movements of moving triangles.
422 cache_ref_time = ref_time;
424 // Get a normalized down vector valid for the whole cache
425 SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
426 down = hlToEc.rotate(SGVec3d(0, 0, 1));
428 // Prepare sphere around the aircraft.
429 double cacheRadius = rad;
431 // Prepare bigger sphere around the aircraft.
432 // This one is required for reliably finding wires we have caught but
433 // have already left the hopefully smaller sphere for the ground reactions.
434 const double max_wire_dist = 300.0;
435 double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
437 Polytope triPolytope;
438 makePolytopeShaft(triPolytope, pt.osg(), down.osg(), cacheRadius);
439 ref_ptr<PolytopeIntersector> triIntersector
440 = new PolytopeIntersector(triPolytope);
441 triIntersector->setDimensionMask(PolytopeIntersector::DimTwo);
442 Polytope wirePolytope;
443 makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius);
444 ref_ptr<WireIntersector> wireIntersector = new WireIntersector(wirePolytope);
445 wireIntersector->setDimensionMask(PolytopeIntersector::DimOne);
446 ref_ptr<IntersectorGroup> intersectors = new IntersectorGroup;
447 intersectors->addIntersector(triIntersector.get());
448 intersectors->addIntersector(wireIntersector.get());
450 // Walk the scene graph and extract solid ground triangles and
452 IntersectionVisitor iv(intersectors);
453 iv.setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
454 globals->get_scenery()->get_scene_graph()->accept(iv);
455 getTriIntersectorResults(triIntersector.get());
456 getWireIntersectorResults(wireIntersector.get(), wireCacheRadius);
459 SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
460 << ", # triangles = " << triangles.size()
461 << ", # wires = " << wires.size()
462 << ", # catapults = " << catapults.size()
463 << ", ground_radius = " << ground_radius );
465 // If the ground radius is still below 5e6 meters, then we do not yet have
467 found_ground = found_ground && 5e6 < ground_radius;
469 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
470 "without any scenery below the aircraft" );
476 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
478 pt = reference_wgs84_point;
479 rad = reference_vehicle_radius;
480 ref_time = cache_ref_time;
485 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
486 SGVec3d end[2], SGVec3d vel[2])
488 // start with a distance of 1e10 meters...
491 // Time difference to the reference time.
494 size_t sz = catapults.size();
495 for (size_t i = 0; i < sz; ++i) {
496 SGVec3d pivotoff, rvel[2];
497 pivotoff = catapults[i].start - catapults[i].gp.pivot;
498 rvel[0] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
499 pivotoff = catapults[i].end - catapults[i].gp.pivot;
500 rvel[1] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
503 thisEnd[0] = catapults[i].start + t*rvel[0];
504 thisEnd[1] = catapults[i].end + t*rvel[1];
506 double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
507 if (this_dist < dist) {
508 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
509 << this_dist << " meters away");
519 // At the end take the root, we only computed squared distances ...
524 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
525 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
526 int *type, const SGMaterial** material, double *agl)
530 *type = FGInterface::Unknown;
534 vel = SGVec3d(0, 0, 0);
535 contact = SGVec3d(0, 0, 0);
536 normal = SGVec3d(0, 0, 0);
538 // Time difference to th reference time.
541 // The double valued point we start to search for intersection.
543 // shift the start of our ray by maxaltoff upwards
544 SGRayd ray(pt - max_altoff*down, down);
546 // Initialize to something sensible
547 double current_radius = 0.0;
549 size_t sz = triangles.size();
550 for (size_t i = 0; i < sz; ++i) {
552 SGTriangled triangle;
553 velocityTransformTriangle(t, triangle, sphere, triangles[i]);
554 if (!intersectsInf(ray, sphere))
557 // Check for intersection.
559 if (intersects(isecpoint, triangle, ray, 1e-4)) {
560 // Compute the vector from pt to the intersection point ...
561 SGVec3d off = isecpoint - pt;
562 // ... and check if it is too high or not
564 // compute the radius, good enough approximation to take the geocentric radius
565 double radius = dot(isecpoint, isecpoint);
566 if (current_radius < radius) {
567 current_radius = radius;
569 // Save the new potential intersection point.
571 // The first three values in the vector are the plane normal.
572 normal = triangle.getNormal();
573 // The velocity wrt earth.
574 SGVec3d pivotoff = pt - triangles[i].gp.pivot;
575 vel = triangles[i].gp.vel + cross(triangles[i].gp.rot, pivotoff);
576 // Save the ground type.
577 *type = triangles[i].gp.type;
578 *agl = dot(down, contact - dpt);
580 *material = triangles[i].gp.material;
588 // Whenever we did not have a ground triangle for the requested point,
589 // take the ground level we found during the current cache build.
590 // This is as good as what we had before for agl.
591 double r = length(dpt);
593 contact *= ground_radius/r;
595 vel = SGVec3d(0, 0, 0);
597 // The altitude is the distance of the requested point from the
599 *agl = dot(down, contact - dpt);
602 *material = _material;
607 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
609 size_t sz = wires.size();
613 // Time difference to the reference time.
616 // Build the two triangles spanning the area where the hook has moved
617 // during the past step.
618 SGTriangled triangle[2];
619 triangle[0].set(pt[0], pt[1], pt[2]);
620 triangle[1].set(pt[0], pt[2], pt[3]);
622 // Intersect the wire lines with each of these triangles.
623 // You have caught a wire if they intersect.
624 for (size_t i = 0; i < sz; ++i) {
626 for (int k = 0; k < 2; ++k) {
627 le[k] = wires[i].ends[k];
628 SGVec3d pivotoff = le[k] - wires[i].gp.pivot;
629 SGVec3d vel = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
632 SGLineSegmentd lineSegment(le[0], le[1]);
634 for (int k=0; k<2; ++k) {
635 if (intersects(triangle[k], lineSegment)) {
636 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
637 // Store the wire id.
638 wire_id = wires[i].gp.wire_id;
647 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
649 // Fast return if we do not have an active wire.
653 // Time difference to the reference time.
656 // Search for the wire with the matching wire id.
657 size_t sz = wires.size();
658 for (size_t i = 0; i < sz; ++i) {
659 if (wires[i].gp.wire_id == wire_id) {
660 for (size_t k = 0; k < 2; ++k) {
661 SGVec3d pivotoff = wires[i].ends[k] - wires[i].gp.pivot;
662 vel[k] = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
663 end[k] = wires[i].ends[k] + t*vel[k];
672 void FGGroundCache::release_wire(void)