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., 675 Mass Ave, Cambridge, MA 02139, USA.
28 #include <simgear/sg_inlines.h>
29 #include <simgear/constants.h>
30 #include <simgear/debug/logstream.hxx>
31 #include <simgear/math/sg_geodesy.hxx>
33 #include <Main/globals.hxx>
34 #include <Scenery/scenery.hxx>
35 #include <Scenery/tilemgr.hxx>
36 #include <AIModel/AICarrier.hxx>
39 #include "groundcache.hxx"
41 // Specialized version of sgMultMat4 needed because of mixed matrix
43 static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
44 for ( int j = 0 ; j < 4 ; j++ ) {
45 dst[0][j] = m2[0][0] * m1[0][j] +
50 dst[1][j] = m2[1][0] * m1[0][j] +
55 dst[2][j] = m2[2][0] * m1[0][j] +
60 dst[3][j] = m2[3][0] * m1[0][j] +
67 static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
71 // Some tolerance in meters we accept a point to be outside of the triangle
72 // and still return that it is inside.
75 // punt if outside bouding cube
76 SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
77 if( (point[0] < min - eps) || (point[0] > max + eps) )
81 SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
82 if( (point[1] < min - eps) || (point[1] > max + eps) )
86 SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
87 if( (point[2] < min - eps) || (point[2] > max + eps) )
91 // drop the smallest dimension so we only have to work in 2d.
92 SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
93 SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
94 if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
95 // x is the smallest dimension
104 } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
105 // y is the smallest dimension
114 } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
115 // z is the smallest dimension
125 // all dimensions are really small so lets call it close
126 // enough and return a successful match
130 // check if intersection point is on the same side of p1 <-> p2 as p3
131 SGDfloat tmp = (y2 - y3);
132 SGDfloat tmpn = (x2 - x3);
133 int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
134 int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - side1*eps - y1) * tmpn);
135 if ( side1 != side2 ) {
136 // printf("failed side 1 check\n");
140 // check if intersection point is on correct side of p2 <-> p3 as p1
143 side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
144 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - side1*eps - y1) * tmpn);
145 if ( side1 != side2 ) {
146 // printf("failed side 2 check\n");
150 // check if intersection point is on correct side of p1 <-> p3 as p2
153 side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
154 side2 = SG_SIGN (tmp * (x1 - rx) + (ry - side1*eps - y1) * tmpn);
155 if ( side1 != side2 ) {
156 // printf("failed side 3 check\n");
163 // Test if the line given by the point on the line pt_on_line and the
164 // line direction dir intersects the sphere sp.
165 // Adapted from plib.
167 fgdIsectSphereInfLine(const sgdSphere& sp,
168 const sgdVec3 pt_on_line, const sgdVec3 dir)
171 sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
173 SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
175 SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
176 projectedDistance * projectedDistance;
178 SGDfloat radius = sp.getRadius();
179 return dist < radius*radius;
182 FGGroundCache::FGGroundCache()
184 sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
186 cache_ref_time = 0.0;
188 sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
189 reference_vehicle_radius = 0.0;
190 found_ground = false;
193 FGGroundCache::~FGGroundCache()
197 FGGroundCache::GroundProperty
198 FGGroundCache::extractGroundProperty( ssgLeaf* l )
200 // FIXME: Do more ...
201 // Idea: have a get_globals() function which knows about that stuff.
202 // Or most propably read that from a configuration file,
203 // from property tree or whatever ...
205 // Get ground dependent data.
209 FGAICarrierHardware *ud =
210 dynamic_cast<FGAICarrierHardware*>(l->getUserData());
213 case FGAICarrierHardware::Wire:
214 gp.type = FGInterface::Wire;
217 case FGAICarrierHardware::Catapult:
218 gp.type = FGInterface::Catapult;
221 gp.type = FGInterface::Solid;
225 // Copy the velocity from the carrier class.
226 ud->carrier->getVelocityWrtEarth( gp.vel );
231 // Initialize velocity field.
232 sgSetVec3( gp.vel, 0.0, 0.0, 0.0 );
235 // Get the texture name and decide what ground type we have.
236 ssgState *st = l->getState();
237 if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) {
238 ssgSimpleState *ss = (ssgSimpleState*)st;
239 SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" );
240 string file = fullPath.file();
241 SGPath dirPath(fullPath.dir());
242 string category = dirPath.file();
244 if (category == "Runway")
245 gp.type = FGInterface::Solid;
247 if (file == "asphault.rgb" || file == "airport.rgb")
248 gp.type = FGInterface::Solid;
249 else if (file == "water.rgb" || file == "water-lake.rgb")
250 gp.type = FGInterface::Water;
251 else if (file == "forest.rgb" || file == "cropwood.rgb")
252 gp.type = FGInterface::Forest;
260 FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
263 GroundProperty gp = extractGroundProperty(l);
265 // Lines must have special meanings.
266 // Wires and catapults are done with lines.
267 int nl = l->getNumLines();
268 for (int i = 0; i < nl; ++i) {
273 l->getLine(i, v, v+1 );
274 for (int k=0; k<2; ++k) {
275 sgdSetVec3(ends[k], l->getVertex(v[k]));
276 sgdXformPnt3(ends[k], xform);
277 sphere.extend(ends[k]);
280 if (wsp->intersects( &sphere )) {
281 if (gp.type == FGInterface::Wire) {
283 sgdCopyVec3(wire.ends[0], ends[0]);
284 sgdCopyVec3(wire.ends[1], ends[1]);
285 sgdSetVec3(wire.velocity, gp.vel);
286 wire.wire_id = gp.wire_id;
288 wires.push_back(wire);
290 if (gp.type == FGInterface::Catapult) {
292 sgdCopyVec3(cat.start, ends[0]);
293 sgdCopyVec3(cat.end, ends[1]);
294 sgdSetVec3(cat.velocity, gp.vel);
296 catapults.push_back(cat);
303 FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
304 const sgdMat4 xform, bool sphIsec,
305 sgdVec3 down, ssgLeaf *l)
307 GroundProperty gp = extractGroundProperty(l);
309 int nt = l->getNumTriangles();
310 for (int i = 0; i < nt; ++i) {
314 l->getTriangle(i, &v[0], &v[1], &v[2]);
315 for (int k = 0; k < 3; ++k) {
316 sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
317 sgdXformPnt3(t.vertices[k], xform);
318 t.sphere.extend(t.vertices[k]);
321 sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
322 SGDfloat dot = sgdScalarProductVec3(down, t.plane);
324 if (!l->getCullFace()) {
325 // Surface points downwards, ignore for altitude computations.
328 sgdScaleVec4( t.plane, -1 );
331 // Check if the sphere around the vehicle intersects the sphere
332 // around that triangle. If so, put that triangle into the cache.
333 if (sphIsec && sp->intersects(&t.sphere)) {
334 sgdSetVec3(t.velocity, gp.vel);
336 triangles.push_back(t);
339 // In case the cache is empty, we still provide agl computations.
340 // But then we use the old way of having a fixed elevation value for
341 // the whole lifetime of this cache.
342 if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
344 sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
346 if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
347 fgdPointInTriangle( isectpoint, t.vertices ) ) {
349 sgdAddVec3(isectpoint, cache_center);
350 double this_radius = sgdLengthVec3(isectpoint);
351 if (ground_radius < this_radius)
352 ground_radius = this_radius;
359 FGGroundCache::velocityTransformTriangle(double dt,
360 FGGroundCache::Triangle& dst,
361 const FGGroundCache::Triangle& src)
363 sgdCopyVec3(dst.vertices[0], src.vertices[0]);
364 sgdCopyVec3(dst.vertices[1], src.vertices[1]);
365 sgdCopyVec3(dst.vertices[2], src.vertices[2]);
367 sgdCopyVec4(dst.plane, src.plane);
369 sgdCopyVec3(dst.sphere.center, src.sphere.center);
370 dst.sphere.radius = src.sphere.radius;
372 sgdCopyVec3(dst.velocity, src.velocity);
376 if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
377 sgdAddScaledVec3(dst.vertices[0], src.velocity, dt);
378 sgdAddScaledVec3(dst.vertices[1], src.velocity, dt);
379 sgdAddScaledVec3(dst.vertices[2], src.velocity, dt);
381 dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, src.velocity);
383 sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
388 FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
389 sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
391 // Travel through all kids.
393 for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
394 if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
396 if ( e->getBSphere()->isEmpty() )
399 // We need to check further if either the sphere around the branch
400 // intersects the sphere around the aircraft or the line downwards from
401 // the aircraft intersects the branchs sphere.
403 sgdSetVec3(esphere.center, e->getBSphere()->center);
404 esphere.radius = e->getBSphere()->radius;
405 esphere.orthoXform(xform);
406 bool wspIsec = wsp->intersects(&esphere);
407 bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
408 if (!wspIsec && !downIsec)
411 // For branches collect up the transforms to reach that branch and
412 // call cache_fill recursively.
413 if ( e->isAKindOf( ssgTypeBranch() ) ) {
414 ssgBranch *b = (ssgBranch *)e;
415 if ( b->isAKindOf( ssgTypeTransform() ) ) {
416 // Collect up the transfors required to reach that part of
419 sgMakeIdentMat4( xform2 );
420 ssgTransform *t = (ssgTransform*)b;
421 t->getTransform( xform2 );
423 fgMultMat4(xform3, xform, xform2);
424 cache_fill( b, xform3, sp, down, wsp );
426 cache_fill( b, xform, sp, down, wsp );
429 // For leafs, check each triangle for intersection.
430 // This will minimize the number of vertices/triangles in the cache.
431 else if (e->isAKindOf(ssgTypeLeaf())) {
432 // Since we reach that leaf if we have an intersection with the
433 // most propably bigger wire/catapult cache sphere, we need to check
434 // that here, if the smaller cache for the surface has a chance for hits.
435 // Also, if the spheres do not intersect compute a croase agl value
436 // by following the line downwards originating at the aircraft.
437 bool spIsec = sp->intersects(&esphere);
438 putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
440 // If we are here, we need to put all special hardware here into
443 putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
449 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
454 found_ground = false;
459 // Store the parameters we used to build up that cache.
460 sgdCopyVec3(reference_wgs84_point, pt);
461 reference_vehicle_radius = rad;
462 // Store the time reference used to compute movements of moving triangles.
463 cache_ref_time = ref_time;
465 // Decide where we put the scenery center.
466 Point3D old_cntr = globals->get_scenery()->get_center();
467 Point3D cntr(pt[0], pt[1], pt[2]);
468 // Only move the cache center if it is unaccaptable far away.
469 if (40*40 < old_cntr.distance3Dsquared(cntr))
470 globals->get_scenery()->set_center(cntr);
474 // The center of the cache.
475 sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
478 sgdSubVec3(ptoff, pt, cache_center);
479 // Prepare sphere around the aircraft.
481 acSphere.setRadius(rad);
482 acSphere.setCenter(ptoff);
484 // Prepare bigger sphere around the aircraft.
485 // This one is required for reliably finding wires we have caught but
486 // have already left the hopefully smaller sphere for the ground reactions.
487 const double max_wire_dist = 300.0;
488 sgdSphere wireSphere;
489 wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
490 wireSphere.setCenter(ptoff);
492 // Down vector. Is used for croase agl computations when we are far enough
493 // from ground that we have an empty cache.
495 sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
496 sgdNormalizeVec3(down);
498 // We collaps all transforms we need to reach a particular leaf.
499 // The leafs itself will be then transformed later.
500 // So our cache is just flat.
501 // For leafs which are moving (carriers surface, etc ...)
502 // we will later store a speed in the GroundType class. We can then apply
503 // some translations to that nodes according to the time which has passed
504 // compared to that snapshot.
506 sgdMakeIdentMat4( xform );
509 // Walk the scene graph and extract solid ground triangles and carrier data.
510 ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
511 cache_fill(terrain, xform, &acSphere, down, &wireSphere);
514 SG_LOG(SG_FLIGHT,SG_INFO, "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" );
527 if (cntr != old_cntr)
528 globals->get_scenery()->set_center(old_cntr);
534 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
536 sgdCopyVec3(pt, reference_wgs84_point);
537 *rad = reference_vehicle_radius;
538 *ref_time = cache_ref_time;
543 FGGroundCache::get_cat(double t, const double dpt[3],
544 double end[2][3], double vel[2][3])
546 // start with a distance of 1e10 meters...
549 // Time difference to the reference time.
552 size_t sz = catapults.size();
553 for (size_t i = 0; i < sz; ++i) {
555 sgdCopyVec3(ls.a, catapults[i].start);
556 sgdCopyVec3(ls.b, catapults[i].end);
558 sgdAddVec3(ls.a, cache_center);
559 sgdAddVec3(ls.b, cache_center);
561 sgdAddScaledVec3(ls.a, catapults[i].velocity, t);
562 sgdAddScaledVec3(ls.b, catapults[i].velocity, t);
564 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
565 if (this_dist < dist) {
566 SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
567 << this_dist << " meters away");
570 // The carrier code takes care of that ordering.
571 sgdCopyVec3( end[0], ls.a );
572 sgdCopyVec3( end[1], ls.b );
573 sgdCopyVec3( vel[0], catapults[i].velocity );
574 sgdCopyVec3( vel[1], catapults[i].velocity );
578 // At the end take the root, we only computed squared distances ...
583 FGGroundCache::get_agl(double t, const double dpt[3],
584 double contact[3], double normal[3], double vel[3],
585 int *type, double *loadCapacity,
586 double *frictionFactor, double *agl)
590 *type = FGInterface::Unknown;
592 *loadCapacity = DBL_MAX;
593 *frictionFactor = 1.0;
594 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
595 sgdSetVec3( contact, 0.0, 0.0, 0.0 );
596 sgdSetVec3( normal, 0.0, 0.0, 0.0 );
598 // Time difference to th reference time.
601 // The double valued point we start to search for intersection.
603 sgdSubVec3( pt, dpt, cache_center );
605 // The search direction
607 sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
609 // Initialize to something sensible
610 double sqdist = DBL_MAX;
612 size_t sz = triangles.size();
613 for (size_t i = 0; i < sz; ++i) {
615 velocityTransformTriangle(t, triangle, triangles[i]);
616 if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
619 // Check for intersection.
621 if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
622 sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
624 // Check for the closest intersection point.
625 // FIXME: is this the right one?
626 SGDfloat newSqdist = sgdDistanceSquaredVec3( isecpoint, pt );
627 if ( newSqdist < sqdist ) {
630 // Save the new potential intersection point.
631 sgdCopyVec3( contact, isecpoint );
632 sgdAddVec3( contact, cache_center );
633 // The first three values in the vector are the plane normal.
634 sgdCopyVec3( normal, triangle.plane );
635 // The velocity wrt earth.
636 /// FIXME: only true for non rotating objects!!!!
637 sgdCopyVec3( vel, triangle.velocity );
638 // Save the ground type.
639 *type = triangle.type;
640 // FIXME: figure out how to get that sign ...
641 // *agl = sqrt(sqdist);
642 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
643 // *loadCapacity = DBL_MAX;
644 // *frictionFactor = 1.0;
652 // Whenever we did not have a ground triangle for the requested point,
653 // take the ground level we found during the current cache build.
654 // This is as good as what we had before for agl.
655 double r = sgdLengthVec3( dpt );
656 sgdCopyVec3( contact, dpt );
657 sgdScaleVec3( contact, ground_radius/r );
658 sgdCopyVec3( normal, dpt );
659 sgdNormaliseVec3( normal );
660 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
662 // The altitude is the distance of the requested point from the
664 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
665 *type = FGInterface::Unknown;
666 *loadCapacity = DBL_MAX;
667 *frictionFactor = 1.0;
672 bool FGGroundCache::caught_wire(double t, const double pt[4][3])
674 size_t sz = wires.size();
678 // Time difference to the reference time.
681 // Build the two triangles spanning the area where the hook has moved
682 // during the past step.
685 sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
686 sgdCopyVec3( tri[0][0], pt[0] );
687 sgdCopyVec3( tri[0][1], pt[1] );
688 sgdCopyVec3( tri[0][2], pt[2] );
689 sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
690 sgdCopyVec3( tri[1][0], pt[0] );
691 sgdCopyVec3( tri[1][1], pt[2] );
692 sgdCopyVec3( tri[1][2], pt[3] );
694 // Intersect the wire lines with each of these triangles.
695 // You have cautght a wire if they intersect.
696 for (size_t i = 0; i < sz; ++i) {
698 sgdCopyVec3(le[0], wires[i].ends[0]);
699 sgdCopyVec3(le[1], wires[i].ends[1]);
701 sgdAddVec3(le[0], cache_center);
702 sgdAddVec3(le[1], cache_center);
704 sgdAddScaledVec3(le[0], wires[i].velocity, t);
705 sgdAddScaledVec3(le[1], wires[i].velocity, t);
707 for (int k=0; k<2; ++k) {
709 double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
710 if ( 0.0 <= isecval && isecval <= 1.0 &&
711 sgdPointInTriangle( isecpoint, tri[k] ) ) {
712 SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
713 // Store the wire id.
714 wire_id = wires[i].wire_id;
723 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
725 // Fast return if we do not have an active wire.
729 // Time difference to the reference time.
732 // Search for the wire with the matching wire id.
733 size_t sz = wires.size();
734 for (size_t i = 0; i < sz; ++i) {
735 if (wires[i].wire_id == wire_id) {
736 sgdCopyVec3(end[0], wires[i].ends[0]);
737 sgdCopyVec3(end[1], wires[i].ends[1]);
739 sgdAddVec3(end[0], cache_center);
740 sgdAddVec3(end[1], cache_center);
742 sgdAddScaledVec3(end[0], wires[i].velocity, t);
743 sgdAddScaledVec3(end[1], wires[i].velocity, t);
745 sgdCopyVec3(vel[0], wires[i].velocity);
746 sgdCopyVec3(vel[1], wires[i].velocity);
754 void FGGroundCache::release_wire(void)