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.
29 #include <osg/CullFace>
30 #include <osg/Drawable>
32 #include <osg/Geometry>
33 #include <osg/TriangleFunctor>
35 #include <simgear/sg_inlines.h>
36 #include <simgear/constants.h>
37 #include <simgear/debug/logstream.hxx>
38 #include <simgear/math/sg_geodesy.hxx>
39 #include <simgear/scene/material/mat.hxx>
40 #include <simgear/scene/material/matlib.hxx>
41 #include <simgear/scene/util/SGNodeMasks.hxx>
43 #include <Main/globals.hxx>
44 #include <Scenery/scenery.hxx>
45 #include <Scenery/tilemgr.hxx>
46 #include <AIModel/AICarrier.hxx>
49 #include "groundcache.hxx"
51 /// Ok, variant that uses a infinite line istead of the ray.
52 /// also not that this only works if the ray direction is normalized.
54 intersectsInf(const SGRayd& ray, const SGSphered& sphere)
56 SGVec3d r = sphere.getCenter() - ray.getOrigin();
57 double projectedDistance = dot(r, ray.getDirection());
58 double dist = dot(r, r) - projectedDistance * projectedDistance;
59 return dist < sphere.getRadius2();
63 class SGExtendedTriangleFunctor : public osg::TriangleFunctor<T> {
65 // Ok, to be complete we should also implement the indexed variants
66 // For now this one appears to be enough ...
67 void drawArrays(GLenum mode, GLint first, GLsizei count)
69 if (_vertexArrayPtr==0 || count==0) return;
71 const osg::Vec3* vlast;
72 const osg::Vec3* vptr;
75 vlast = &_vertexArrayPtr[first+count];
76 for(vptr=&_vertexArrayPtr[first];vptr<vlast;vptr+=2)
77 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
80 vlast = &_vertexArrayPtr[first+count-1];
81 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
82 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
85 vlast = &_vertexArrayPtr[first+count-1];
86 for(vptr=&_vertexArrayPtr[first];vptr<vlast;++vptr)
87 this->operator()(*(vptr),*(vptr+1),_treatVertexDataAsTemporary);
88 this->operator()(_vertexArrayPtr[first+count-1],
89 _vertexArrayPtr[first],_treatVertexDataAsTemporary);
92 osg::TriangleFunctor<T>::drawArrays(mode, first, count);
97 using osg::TriangleFunctor<T>::_vertexArrayPtr;
98 using osg::TriangleFunctor<T>::_treatVertexDataAsTemporary;
101 class GroundCacheFillVisitor : public osg::NodeVisitor {
104 /// class to just redirect triangles to the GroundCacheFillVisitor
105 class GroundCacheFill {
107 void setGroundCacheFillVisitor(GroundCacheFillVisitor* gcfv)
108 { mGroundCacheFillVisitor = gcfv; }
110 void operator () (const osg::Vec3& v1, const osg::Vec3& v2,
111 const osg::Vec3& v3, bool)
112 { mGroundCacheFillVisitor->addTriangle(v1, v2, v3); }
114 void operator () (const osg::Vec3& v1, const osg::Vec3& v2, bool)
115 { mGroundCacheFillVisitor->addLine(v1, v2); }
118 GroundCacheFillVisitor* mGroundCacheFillVisitor;
122 GroundCacheFillVisitor(FGGroundCache* groundCache,
124 const SGVec3d& cacheReference,
126 double wireCacheRadius) :
127 osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN),
128 mGroundCache(groundCache)
130 setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
134 mBackfaceCulling = false;
135 mCacheReference = cacheReference;
136 mLocalCacheReference = cacheReference;
137 mCacheRadius = cacheRadius;
138 mWireCacheRadius = wireCacheRadius;
140 mTriangleFunctor.setGroundCacheFillVisitor(this);
142 mGroundProperty.wire_id = -1;
143 mGroundProperty.vel = SGVec3d(0, 0, 0);
144 mGroundProperty.rot = SGVec3d(0, 0, 0);
145 mGroundProperty.pivot = SGVec3d(0, 0, 0);
148 void setSceneryCenter(const SGVec3d& cntr)
150 mLocalToGlobal.makeTranslate(cntr.osg());
151 mGlobalToLocal.makeTranslate(-cntr.osg());
154 void updateCullMode(osg::StateSet* stateSet)
159 osg::StateAttribute* stateAttribute;
160 stateAttribute = stateSet->getAttribute(osg::StateAttribute::CULLFACE);
163 osg::CullFace* cullFace = static_cast<osg::CullFace*>(stateAttribute);
164 mBackfaceCulling = cullFace->getMode() == osg::CullFace::BACK;
167 bool enterBoundingSphere(const osg::BoundingSphere& bs)
172 SGVec3d cntr(osg::Vec3d(bs.center())*mLocalToGlobal);
173 double rc = bs.radius() + mCacheRadius;
174 // Ok, this node might intersect the cache. Visit it in depth.
175 double centerDist2 = distSqr(mCacheReference, cntr);
176 if (centerDist2 < rc*rc) {
179 // Check if the down direction touches the bounding sphere of the node
180 // if so, do at least croase agl computations.
181 // Ther other thing is that we must check if we are in range of
183 double rw = bs.radius() + mWireCacheRadius;
184 if (rw*rw < centerDist2 &&
185 !intersectsInf(SGRayd(mCacheReference, mDown),
186 SGSphered(cntr, bs.radius())))
194 bool enterNode(osg::Node& node)
196 if (!enterBoundingSphere(node.getBound()))
199 updateCullMode(node.getStateSet());
201 FGGroundCache::GroundProperty& gp = mGroundProperty;
202 // get some material information for use in the gear model
203 gp.material = globals->get_matlib()->findMaterial(&node);
205 gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
208 gp.type = FGInterface::Unknown;
209 osg::Referenced* base = node.getUserData();
212 FGAICarrierHardware *ud =
213 dynamic_cast<FGAICarrierHardware*>(base);
218 case FGAICarrierHardware::Wire:
219 gp.type = FGInterface::Wire;
222 case FGAICarrierHardware::Catapult:
223 gp.type = FGInterface::Catapult;
226 gp.type = FGInterface::Solid;
229 // Copy the velocity from the carrier class.
230 ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
235 void fillWith(osg::Drawable* drawable)
237 bool oldSphIsec = sphIsec;
238 if (!enterBoundingSphere(drawable->getBound()))
241 bool oldBackfaceCulling = mBackfaceCulling;
242 updateCullMode(drawable->getStateSet());
244 drawable->accept(mTriangleFunctor);
246 mBackfaceCulling = oldBackfaceCulling;
247 sphIsec = oldSphIsec;
250 virtual void apply(osg::Geode& geode)
252 bool oldBackfaceCulling = mBackfaceCulling;
253 bool oldSphIsec = sphIsec;
254 FGGroundCache::GroundProperty oldGp = mGroundProperty;
255 if (!enterNode(geode))
258 for(unsigned i = 0; i < geode.getNumDrawables(); ++i)
259 fillWith(geode.getDrawable(i));
260 sphIsec = oldSphIsec;
261 mGroundProperty = oldGp;
262 mBackfaceCulling = oldBackfaceCulling;
265 virtual void apply(osg::Group& group)
267 bool oldBackfaceCulling = mBackfaceCulling;
268 bool oldSphIsec = sphIsec;
269 FGGroundCache::GroundProperty oldGp = mGroundProperty;
270 if (!enterNode(group))
273 sphIsec = oldSphIsec;
274 mBackfaceCulling = oldBackfaceCulling;
275 mGroundProperty = oldGp;
278 virtual void apply(osg::Transform& transform)
280 if (!enterNode(transform))
282 bool oldBackfaceCulling = mBackfaceCulling;
283 bool oldSphIsec = sphIsec;
284 FGGroundCache::GroundProperty oldGp = mGroundProperty;
285 /// transform the caches center to local coords
286 osg::Matrix oldLocalToGlobal = mLocalToGlobal;
287 osg::Matrix oldGlobalToLocal = mGlobalToLocal;
288 transform.computeLocalToWorldMatrix(mLocalToGlobal, this);
289 transform.computeWorldToLocalMatrix(mGlobalToLocal, this);
291 SGVec3d oldLocalCacheReference = mLocalCacheReference;
292 mLocalCacheReference.osg() = mCacheReference.osg()*mGlobalToLocal;
293 SGVec3d oldLocalDown = mLocalDown;
294 mLocalDown.osg() = osg::Matrixd::transform3x3(mDown.osg(), mGlobalToLocal);
300 mLocalDown = oldLocalDown;
301 mLocalCacheReference = oldLocalCacheReference;
302 mLocalToGlobal = oldLocalToGlobal;
303 mGlobalToLocal = oldGlobalToLocal;
304 sphIsec = oldSphIsec;
305 mBackfaceCulling = oldBackfaceCulling;
306 mGroundProperty = oldGp;
309 void addTriangle(const osg::Vec3& v1, const osg::Vec3& v2,
318 // a bounding sphere in the node local system
319 SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
320 double boundRadius = std::max(distSqr(v[0], boundCenter),
321 distSqr(v[1], boundCenter));
322 boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
323 boundRadius = sqrt(boundRadius);
325 SGRayd ray(mLocalCacheReference, mLocalDown);
327 // if we are not in the downward cylinder bail out
328 if (!intersectsInf(ray, SGSphered(boundCenter, boundRadius + mCacheRadius)))
331 SGTriangled triangle(v);
333 // The normal and plane in the node local coordinate system
334 SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
335 if (0 < dot(mLocalDown, n)) {
336 if (mBackfaceCulling) {
337 // Surface points downwards, ignore for altitude computations.
344 // Only check if the triangle is in the cache sphere if the plane
345 // containing the triangle is near enough
347 double d = dot(n, v[0] - mLocalCacheReference);
348 if (d*d < mCacheRadius*dot(n, n)) {
349 // Check if the sphere around the vehicle intersects the sphere
350 // around that triangle. If so, put that triangle into the cache.
351 double r2 = boundRadius + mCacheRadius;
352 if (distSqr(boundCenter, mLocalCacheReference) < r2*r2) {
353 FGGroundCache::Triangle t;
354 t.triangle.setBaseVertex(SGVec3d(v[0].osg()*mLocalToGlobal));
355 t.triangle.setEdge(0, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(0).osg(), mLocalToGlobal)));
356 t.triangle.setEdge(1, SGVec3d(osg::Matrixd::transform3x3(triangle.getEdge(1).osg(), mLocalToGlobal)));
358 t.sphere.setCenter(SGVec3d(boundCenter.osg()*mLocalToGlobal));
359 t.sphere.setRadius(boundRadius);
361 t.velocity = mGroundProperty.vel;
362 t.rotation = mGroundProperty.rot;
363 t.rotation_pivot = mGroundProperty.pivot;
364 t.type = mGroundProperty.type;
365 t.material = mGroundProperty.material;
366 mGroundCache->triangles.push_back(t);
371 // In case the cache is empty, we still provide agl computations.
372 // But then we use the old way of having a fixed elevation value for
373 // the whole lifetime of this cache.
375 if (intersects(isectpoint, triangle, ray, 1e-4)) {
376 mGroundCache->found_ground = true;
377 isectpoint.osg() = isectpoint.osg()*mLocalToGlobal;
378 double this_radius = length(isectpoint);
379 if (mGroundCache->ground_radius < this_radius) {
380 mGroundCache->ground_radius = this_radius;
381 mGroundCache->_type = mGroundProperty.type;
382 mGroundCache->_material = mGroundProperty.material;
387 void addLine(const osg::Vec3& v1, const osg::Vec3& v2)
389 SGVec3d gv1(osg::Vec3d(v1)*mLocalToGlobal);
390 SGVec3d gv2(osg::Vec3d(v2)*mLocalToGlobal);
392 SGVec3d boundCenter = 0.5*(gv1 + gv2);
393 double boundRadius = length(gv1 - boundCenter);
395 if (distSqr(boundCenter, mCacheReference)
396 < (boundRadius + mWireCacheRadius)*(boundRadius + mWireCacheRadius) ) {
397 if (mGroundProperty.type == FGInterface::Wire) {
398 FGGroundCache::Wire wire;
401 wire.velocity = mGroundProperty.vel;
402 wire.rotation = mGroundProperty.rot;
403 wire.rotation_pivot = mGroundProperty.pivot;
404 wire.wire_id = mGroundProperty.wire_id;
406 mGroundCache->wires.push_back(wire);
408 if (mGroundProperty.type == FGInterface::Catapult) {
409 FGGroundCache::Catapult cat;
410 // Trick to get the ends in the right order.
411 // Use the x axis in the original coordinate system. Choose the
412 // most negative x-axis as the one pointing forward
420 cat.velocity = mGroundProperty.vel;
421 cat.rotation = mGroundProperty.rot;
422 cat.rotation_pivot = mGroundProperty.pivot;
424 mGroundCache->catapults.push_back(cat);
429 SGExtendedTriangleFunctor<GroundCacheFill> mTriangleFunctor;
430 FGGroundCache* mGroundCache;
431 SGVec3d mCacheReference;
433 double mWireCacheRadius;
434 osg::Matrix mLocalToGlobal;
435 osg::Matrix mGlobalToLocal;
438 SGVec3d mLocalCacheReference;
440 bool mBackfaceCulling;
441 FGGroundCache::GroundProperty mGroundProperty;
444 FGGroundCache::FGGroundCache()
447 cache_ref_time = 0.0;
449 reference_wgs84_point = SGVec3d(0, 0, 0);
450 reference_vehicle_radius = 0.0;
451 found_ground = false;
454 FGGroundCache::~FGGroundCache()
459 FGGroundCache::velocityTransformTriangle(double dt,
460 SGTriangled& dst, SGSphered& sdst,
461 const FGGroundCache::Triangle& src)
466 if (dt*dt*dot(src.velocity, src.velocity) < SGLimitsd::epsilon())
469 SGVec3d baseVert = dst.getBaseVertex();
470 SGVec3d pivotoff = baseVert - src.rotation_pivot;
471 baseVert += dt*(src.velocity + cross(src.rotation, pivotoff));
472 dst.setBaseVertex(baseVert);
473 dst.setEdge(0, dst.getEdge(0) + dt*cross(src.rotation, dst.getEdge(0)));
474 dst.setEdge(1, dst.getEdge(1) + dt*cross(src.rotation, dst.getEdge(1)));
479 FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
484 found_ground = false;
489 // Store the parameters we used to build up that cache.
490 reference_wgs84_point = pt;
491 reference_vehicle_radius = rad;
492 // Store the time reference used to compute movements of moving triangles.
493 cache_ref_time = ref_time;
495 // Get a normalized down vector valid for the whole cache
496 SGQuatd hlToEc = SGQuatd::fromLonLat(SGGeod::fromCart(pt));
497 down = hlToEc.rotate(SGVec3d(0, 0, 1));
499 // Prepare sphere around the aircraft.
500 double cacheRadius = rad;
502 // Prepare bigger sphere around the aircraft.
503 // This one is required for reliably finding wires we have caught but
504 // have already left the hopefully smaller sphere for the ground reactions.
505 const double max_wire_dist = 300.0;
506 double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
508 // Walk the scene graph and extract solid ground triangles and carrier data.
509 GroundCacheFillVisitor gcfv(this, down, pt, cacheRadius, wireCacheRadius);
510 gcfv.setSceneryCenter(globals->get_scenery()->get_center());
511 globals->get_scenery()->get_scene_graph()->accept(gcfv);
514 SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
515 << ", # triangles = " << triangles.size()
516 << ", # wires = " << wires.size()
517 << ", # catapults = " << catapults.size()
518 << ", ground_radius = " << ground_radius );
520 // If the ground radius is still below 5e6 meters, then we do not yet have
522 found_ground = found_ground && 5e6 < ground_radius;
524 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
525 "without any scenery below the aircraft" );
531 FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
533 pt = reference_wgs84_point;
534 rad = reference_vehicle_radius;
535 ref_time = cache_ref_time;
540 FGGroundCache::get_cat(double t, const SGVec3d& dpt,
541 SGVec3d end[2], SGVec3d vel[2])
543 // start with a distance of 1e10 meters...
546 // Time difference to the reference time.
549 size_t sz = catapults.size();
550 for (size_t i = 0; i < sz; ++i) {
551 SGVec3d pivotoff, rvel[2];
552 pivotoff = catapults[i].start - catapults[i].rotation_pivot;
553 rvel[0] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
554 pivotoff = catapults[i].end - catapults[i].rotation_pivot;
555 rvel[1] = catapults[i].velocity + cross(catapults[i].rotation, pivotoff);
558 thisEnd[0] = catapults[i].start + t*rvel[0];
559 thisEnd[1] = catapults[i].end + t*rvel[1];
561 double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
562 if (this_dist < dist) {
563 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
564 << this_dist << " meters away");
574 // At the end take the root, we only computed squared distances ...
579 FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
580 SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
581 int *type, const SGMaterial** material, double *agl)
585 *type = FGInterface::Unknown;
589 vel = SGVec3d(0, 0, 0);
590 contact = SGVec3d(0, 0, 0);
591 normal = SGVec3d(0, 0, 0);
593 // Time difference to th reference time.
596 // The double valued point we start to search for intersection.
598 // shift the start of our ray by maxaltoff upwards
599 SGRayd ray(pt - max_altoff*down, down);
601 // Initialize to something sensible
602 double current_radius = 0.0;
604 size_t sz = triangles.size();
605 for (size_t i = 0; i < sz; ++i) {
607 SGTriangled triangle;
608 velocityTransformTriangle(t, triangle, sphere, triangles[i]);
609 if (!intersectsInf(ray, sphere))
612 // Check for intersection.
614 if (intersects(isecpoint, triangle, ray, 1e-4)) {
615 // Compute the vector from pt to the intersection point ...
616 SGVec3d off = isecpoint - pt;
617 // ... and check if it is too high or not
619 // compute the radius, good enough approximation to take the geocentric radius
620 double radius = dot(isecpoint, isecpoint);
621 if (current_radius < radius) {
622 current_radius = radius;
624 // Save the new potential intersection point.
626 // The first three values in the vector are the plane normal.
627 normal = triangle.getNormal();
628 // The velocity wrt earth.
629 SGVec3d pivotoff = pt - triangles[i].rotation_pivot;
630 vel = triangles[i].velocity + cross(triangles[i].rotation, pivotoff);
631 // Save the ground type.
632 *type = triangles[i].type;
633 *agl = dot(down, contact - dpt);
635 *material = triangles[i].material;
643 // Whenever we did not have a ground triangle for the requested point,
644 // take the ground level we found during the current cache build.
645 // This is as good as what we had before for agl.
646 double r = length(dpt);
648 contact *= ground_radius/r;
650 vel = SGVec3d(0, 0, 0);
652 // The altitude is the distance of the requested point from the
654 *agl = dot(down, contact - dpt);
657 *material = _material;
662 bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
664 size_t sz = wires.size();
668 // Time difference to the reference time.
671 // Build the two triangles spanning the area where the hook has moved
672 // during the past step.
673 SGTriangled triangle[2];
674 triangle[0].set(pt[0], pt[1], pt[2]);
675 triangle[1].set(pt[0], pt[2], pt[3]);
677 // Intersect the wire lines with each of these triangles.
678 // You have caught a wire if they intersect.
679 for (size_t i = 0; i < sz; ++i) {
681 for (int k = 0; k < 2; ++k) {
682 le[k] = wires[i].ends[k];
683 SGVec3d pivotoff = le[k] - wires[i].rotation_pivot;
684 SGVec3d vel = wires[i].velocity + cross(wires[i].rotation, pivotoff);
687 SGLineSegmentd lineSegment(le[0], le[1]);
689 for (int k=0; k<2; ++k) {
690 if (intersects(triangle[k], lineSegment)) {
691 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
692 // Store the wire id.
693 wire_id = wires[i].wire_id;
702 bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
704 // Fast return if we do not have an active wire.
708 // Time difference to the reference time.
711 // Search for the wire with the matching wire id.
712 size_t sz = wires.size();
713 for (size_t i = 0; i < sz; ++i) {
714 if (wires[i].wire_id == wire_id) {
715 for (size_t k = 0; k < 2; ++k) {
716 SGVec3d pivotoff = wires[i].ends[k] - wires[i].rotation_pivot;
717 vel[k] = wires[i].velocity + cross(wires[i].rotation, pivotoff);
718 end[k] = wires[i].ends[k] + t*vel[k];
727 void FGGroundCache::release_wire(void)