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/constants.h>
29 #include <simgear/debug/logstream.hxx>
30 #include <simgear/math/sg_geodesy.hxx>
32 #include <Main/globals.hxx>
33 #include <Scenery/scenery.hxx>
34 #include <Scenery/tilemgr.hxx>
35 #include <AIModel/AICarrier.hxx>
38 #include "groundcache.hxx"
40 FGGroundCache::FGGroundCache()
42 sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
46 sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
47 reference_vehicle_radius = 0.0;
51 FGGroundCache::~FGGroundCache()
53 cache_root.removeAllKids();
56 FGGroundCache::GroundProperty*
57 FGGroundCache::extractGroundProperty( ssgLeaf* l )
60 // Idea: have a get_globals() function which knows about that stuff.
61 // Or most propably read that from a configuration file,
62 // from property tree or whatever ...
64 // Get ground dependent data.
65 GroundProperty *gp = new GroundProperty;
68 FGAICarrierHardware *ud =
69 dynamic_cast<FGAICarrierHardware*>(l->getUserData());
72 case FGAICarrierHardware::Wire:
73 gp->type = FGInterface::Wire;
76 case FGAICarrierHardware::Catapult:
77 gp->type = FGInterface::Catapult;
80 gp->type = FGInterface::Solid;
84 // Copy the velocity from the carrier class.
85 ud->carrier->getVelocityWrtEarth( gp->vel );
90 // Initialize velocity field.
91 sgSetVec3( gp->vel, 0.0, 0.0, 0.0 );
94 // Get the texture name and decide what ground type we have.
95 ssgState *st = l->getState();
96 if (st != NULL && st->isAKindOf(ssgTypeSimpleState())) {
97 ssgSimpleState *ss = (ssgSimpleState*)st;
98 SGPath fullPath( ss->getTextureFilename() ? ss->getTextureFilename(): "" );
99 string file = fullPath.file();
100 SGPath dirPath(fullPath.dir());
101 string category = dirPath.file();
103 SG_LOG(SG_FLIGHT,SG_INFO,
104 "New triangle in cache: " << category << " " << file );
106 if (category == "Runway")
107 gp->type = FGInterface::Solid;
109 if (file == "asphault.rgb" || file == "airport.rgb")
110 gp->type = FGInterface::Solid;
111 else if (file == "water.rgb" || file == "water-lake.rgb")
112 gp->type = FGInterface::Water;
113 else if (file == "forest.rgb" || file == "cropwood.rgb")
114 gp->type = FGInterface::Forest;
121 // Test if the line given by the point on the line pt_on_line and the
122 // line direction dir intersects the sphere sp.
123 // Adapted from plib.
125 sgIsectSphereInfLine(const sgSphere *sp,
126 const sgVec3 pt_on_line, const sgVec3 dir)
129 sgSubVec3 ( r, sp->getCenter(), pt_on_line ) ;
131 SGfloat projectedDistance = sgScalarProductVec3(r, dir);
133 SGfloat dist = sgScalarProductVec3 ( r, r ) -
134 projectedDistance * projectedDistance;
136 SGfloat radius = sp->getRadius();
137 return dist < radius*radius;
141 FGGroundCache::addAndFlattenLeaf(GLenum ty, ssgLeaf *l, ssgIndexArray *ia,
144 // Extract data from the leaf which is just copied.
145 ssgVertexArray *va = ((ssgVtxTable *)l)->getVertices();
146 // Create a new leaf.
147 ssgVtxArray *vtxa = new ssgVtxArray( ty, va, 0, 0, 0, ia );
149 vtxa->removeUnusedVertices();
150 // Apply transform. We won't store transforms in our cache.
151 vtxa->transform( xform );
152 // Check for magic texture names object names and such ...
153 vtxa->setUserData( extractGroundProperty( l ) );
154 // Finally append to cache.
155 cache_root.addKid((ssgEntity*)vtxa);
159 FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform,
162 ssgIndexArray *ia = 0;
164 // Lines must have special meanings.
165 // Wires and catapults are done with lines.
166 int nl = l->getNumLines();
167 for (int i = 0; i < nl; ++i) {
170 l->getLine(i, v, v+1 );
171 for (int k=0; k<2; ++k)
172 tmp.extend( l->getVertex( v[k] ) );
173 tmp.orthoXform(xform);
175 if (wsp->intersects( &tmp )) {
177 ia = new ssgIndexArray();
186 addAndFlattenLeaf(GL_LINES, l, ia, xform);
190 FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform,
191 bool sphIsec, sgVec3 down, ssgLeaf *l)
193 ssgIndexArray *ia = 0;
195 int nt = l->getNumTriangles();
196 for (int i = 0; i < nt; ++i) {
197 // Build up a sphere around that particular triangle-
200 l->getTriangle(i, v, v+1, v+2 );
201 for (int k=0; k<3; ++k)
202 tmp.extend( l->getVertex( v[k] ) );
203 tmp.orthoXform(xform);
205 // Check if the sphere around the vehicle intersects the sphere
206 // around that triangle. If so, put that triangle into the cache.
207 if (sphIsec && sp->intersects( &tmp )) {
209 ia = new ssgIndexArray();
216 // In case the cache is empty, we still provide agl computations.
217 // But then we use the old way of having a fixed elevation value for
218 // the whole lifetime of this cache.
219 if ( sgIsectSphereInfLine(&tmp, sp->getCenter(), down) ) {
221 for (int k=0; k<3; ++k) {
222 sgCopyVec3( tri[k], l->getVertex( v[k] ) );
223 sgXformPnt3( tri[k], xform );
227 sgMakePlane( plane, tri[0], tri[1], tri[2] );
229 sgCopyVec3(ac_cent, sp->getCenter());
231 sgIsectInfLinePlane( dst, ac_cent, down, plane );
232 if ( sgPointInTriangle ( dst, tri ) ) {
235 sgdSetVec3(ddst, dst);
236 sgdAddVec3(ddst, cache_center);
237 double this_radius = sgdLengthVec3(ddst);
238 if (ground_radius < this_radius)
239 ground_radius = this_radius;
246 addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform);
249 // Here is the point where rotation should be handled
251 FGGroundCache::extractCacheRelativeVertex(double t, ssgVtxArray *va,
253 short i, sgVec3 rel_pos,
256 sgCopyVec3( rel_pos, va->getVertex( i ) );
257 sgAddScaledVec3( rel_pos, gp->vel, t );
260 sgdSetVec3( wgs84_vel, gp->vel );
264 FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va,
265 GroundProperty *gp, short i,
266 sgdVec3 wgs84_pos, sgdVec3 wgs84_vel)
269 extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel);
270 sgdSetVec3( wgs84_pos, rel_pos );
271 sgdAddVec3( wgs84_pos, cache_center );
276 FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
277 sgSphere* sp, sgVec3 down, sgSphere* wsp)
279 // Travel through all kids.
281 for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
282 if ( !( e->getTraversalMask() & SSGTRAV_HOT) )
284 if ( e->getBSphere()->isEmpty() )
287 // Wee need to check further if either the sphere around the branch
288 // intersects the sphere around the aircraft or the line downwards from
289 // the aircraft intersects the branchs sphere.
290 sgSphere esphere = *(e->getBSphere());
291 esphere.orthoXform(xform);
292 bool wspIsec = wsp->intersects(&esphere);
293 bool downIsec = sgIsectSphereInfLine(&esphere, sp->getCenter(), down);
294 if (!wspIsec && !downIsec)
297 // For branches collect up the transforms to reach that branch and
298 // call cache_fill recursively.
299 if ( e->isAKindOf( ssgTypeBranch() ) ) {
300 ssgBranch *b = (ssgBranch *)e;
301 if ( b->isAKindOf( ssgTypeTransform() ) ) {
302 // Collect up the transfors required to reach that part of
305 sgMakeIdentMat4( xform2 );
306 ssgTransform *t = (ssgTransform*)b;
307 t->getTransform( xform2 );
308 sgPostMultMat4( xform2, xform );
309 cache_fill( b, xform2, sp, down, wsp );
311 cache_fill( b, xform, sp, down, wsp );
314 // For leafs, check each triangle for intersection.
315 // This will minimize the number of vertices/triangles in the cache.
316 else if (e->isAKindOf(ssgTypeLeaf())) {
317 // Since we reach that leaf if we have an intersection with the
318 // most propably bigger wire/catapult cache sphere, we need to check
319 // that here, if the smaller cache for the surface has a chance for hits.
320 // Also, if the spheres do not intersect compute a croase agl value
321 // by following the line downwards originating at the aircraft.
322 bool spIsec = sp->intersects(&esphere);
323 putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
325 // If we are here, we need to put all special hardware here into
328 putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
334 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
338 cache_root.removeAllKids();
340 found_ground = false;
342 // Store the parameters we used to build up that cache.
343 sgdCopyVec3(reference_wgs84_point, pt);
344 reference_vehicle_radius = rad;
345 // Store the time reference used to compute movements of moving triangles.
346 cache_ref_time = ref_time;
348 // The center of the cache.
349 Point3D psc = globals->get_tile_mgr()->get_current_center();
350 sgdSetVec3(cache_center, psc[0], psc[1], psc[2]);
352 // Prepare sphere around the aircraft.
354 acSphere.setRadius(rad);
356 // Compute the postion of the aircraft relative to the scenery center.
358 sgdSubVec3( doffset, pt, cache_center );
360 sgSetVec3( offset, doffset[0], doffset[1], doffset[2] );
361 acSphere.setCenter( offset );
363 // Prepare bigger sphere around the aircraft.
364 // This one is required for reliably finding wires we have caught but
365 // have already left the hopefully smaller sphere for the ground reactions.
366 const double max_wire_dist = 300.0;
368 wireSphere.setRadius( max_wire_dist < rad ? rad : max_wire_dist );
369 wireSphere.setCenter( offset );
371 // Down vector. Is used for croase agl computations when we are far enough
372 // from ground that we have an empty cache.
374 sgSetVec3(down, -pt[0], -pt[1], -pt[2]);
375 sgNormalizeVec3(down);
377 // We collaps all transforms we need to reach a particular leaf.
378 // The leafs itself will be then transformed later.
379 // So our cache is just flat.
380 // For leafs which are moving (carriers surface, etc ...)
381 // we will later store a speed in the GroundType class. We can then apply
382 // some translations to that nodes according to the time which has passed
383 // compared to that snapshot.
385 sgMakeIdentMat4(xform);
387 // Walk the terrain branch for now.
388 ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
389 cache_fill(terrain, xform, &acSphere, down, &wireSphere);
392 SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad
393 << ", # leafs = " << cache_root.getNumKids()
394 << ", ground_radius = " << ground_radius );
396 // If the ground radius is still below 5e6 meters, then we do not yet have
398 found_ground = found_ground && 5e6 < ground_radius;
400 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
401 "without any scenery below the aircraft" );
407 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
409 sgdCopyVec3(pt, reference_wgs84_point);
410 *rad = reference_vehicle_radius;
411 *ref_time = cache_ref_time;
416 FGGroundCache::get_cat(double t, const double dpt[3],
417 double end[2][3], double vel[2][3])
419 // start with a distance of 1e10 meters...
422 // Time difference to the reference time.
425 // We know that we have a flat cache ...
427 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
428 // We just know that, because we build that ourselfs ...
429 ssgVtxArray *va = (ssgVtxArray *)e;
430 // Only lines are interresting ...
431 if (va->getPrimitiveType() != GL_LINES)
433 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
437 // Check if we have a catapult ...
438 if ( gp->type != FGInterface::Catapult )
441 int nl = va->getNumLines();
442 for (int i=0; i < nl; ++i) {
446 va->getLine(i, vi, vi+1 );
447 extractWgs84Vertex(t, va, gp, vi[0], ls.a, lsVel[0]);
448 extractWgs84Vertex(t, va, gp, vi[1], ls.b, lsVel[1]);
450 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
451 if (this_dist < dist) {
454 // end[0] is the end where the cat starts.
455 // end[1] is the end where the cat ends.
456 // The carrier code takes care of that ordering.
457 sgdCopyVec3( end[0], ls.a );
458 sgdCopyVec3( end[1], ls.b );
459 sgdCopyVec3( vel[0], lsVel[0] );
460 sgdCopyVec3( vel[1], lsVel[1] );
465 // At the end take the root, we only computed squared distances ...
470 FGGroundCache::get_agl(double t, const double dpt[3],
471 double contact[3], double normal[3], double vel[3],
472 int *type, double *loadCapacity,
473 double *frictionFactor, double *agl)
477 *type = FGInterface::Unknown;
479 *loadCapacity = DBL_MAX;
480 *frictionFactor = 1.0;
481 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
482 sgdSetVec3( contact, 0.0, 0.0, 0.0 );
483 sgdSetVec3( normal, 0.0, 0.0, 0.0 );
485 // Time difference to th reference time.
488 // The double valued point we start to search for intersection.
490 sgdSubVec3( tmp, dpt, cache_center );
492 sgSetVec3( pt, tmp );
494 // The search direction
496 sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
498 // Initialize to something sensible
499 double sqdist = DBL_MAX;
501 // We know that we have a flat cache ...
502 // We just know that, because we build that ourselfs ...
504 for ( e = cache_root.getKid(0) ; e != NULL ; e = cache_root.getNextKid() ) {
505 // We just know that, because we build that ourselfs ...
506 ssgVtxArray *va = (ssgVtxArray *)e;
507 // AGL computations are done with triangle/surface leafs.
508 if (va->getPrimitiveType() != GL_TRIANGLES)
510 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
515 int nt = va->getNumTriangles();
516 for (int i=0; i < nt; ++i) {
518 va->getTriangle( i, vi, vi+1, vi+2 );
522 for (int k=0; k<3; ++k)
523 extractCacheRelativeVertex(t, va, gp, vi[k], tri[k], dvel[k]);
525 sgMakePlane( plane, tri[0], tri[1], tri[2] );
527 // Check for intersection.
529 if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
530 sgPointInTriangle3( isecpoint, tri ) ) {
531 // Check for the closest intersection point.
532 // FIXME: is this the right one?
533 double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
534 if ( newSqdist < sqdist ) {
537 // Save the new potential intersection point.
538 sgdSetVec3( contact, isecpoint );
539 sgdAddVec3( contact, cache_center );
540 // The first three values in the vector are the plane normal.
541 sgdSetVec3( normal, plane );
542 // The velocity wrt earth.
543 /// FIXME: only true for non rotating objects!!!!
544 sgdCopyVec3( vel, dvel[0] );
545 // Save the ground type.
547 // FIXME: figure out how to get that sign ...
548 // *agl = sqrt(sqdist);
549 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
550 // *loadCapacity = DBL_MAX;
551 // *frictionFactor = 1.0;
560 // Whenever we did not have a ground triangle for the requested point,
561 // take the ground level we found during the current cache build.
562 // This is as good as what we had before for agl.
563 double r = sgdLengthVec3( dpt );
564 sgdCopyVec3( contact, dpt );
565 sgdScaleVec3( contact, ground_radius/r );
566 sgdCopyVec3( normal, dpt );
567 sgdNormaliseVec3( normal );
568 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
570 // The altitude is the distance of the requested point from the
572 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
573 *type = FGInterface::Unknown;
574 *loadCapacity = DBL_MAX;
575 *frictionFactor = 1.0;
580 bool FGGroundCache::caught_wire(double t, const double cpt[4][3])
584 // Time difference to the reference time.
587 bool firsttime = true;
591 // We know that we have a flat cache ...
593 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
594 // We just know that, because we build that ourselfs ...
595 ssgVtxArray *va = (ssgVtxArray *)e;
596 // Only lines are interresting ...
597 if (va->getPrimitiveType() != GL_LINES)
599 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
603 // Check if we have a catapult ...
604 if ( gp->type != FGInterface::Wire )
607 // Lazy compute the values required for intersectiion tests.
608 // Since we normally do not have wires in the cache this is a
613 for (int k=0; k<4; ++k) {
615 sgdSubVec3( tmp, cpt[k], cache_center );
616 sgSetVec3( pt[k], tmp );
618 sgMakePlane( plane[0], pt[0], pt[1], pt[2] );
619 sgCopyVec3( tri[0][0], pt[0] );
620 sgCopyVec3( tri[0][1], pt[1] );
621 sgCopyVec3( tri[0][2], pt[2] );
622 sgMakePlane( plane[1], pt[0], pt[2], pt[3] );
623 sgCopyVec3( tri[1][0], pt[0] );
624 sgCopyVec3( tri[1][1], pt[2] );
625 sgCopyVec3( tri[1][2], pt[3] );
628 int nl = va->getNumLines();
629 for (int i=0; i < nl; ++i) {
631 va->getLine(i, vi, vi+1 );
634 extractCacheRelativeVertex(t, va, gp, vi[0], le[0], dummy);
635 extractCacheRelativeVertex(t, va, gp, vi[1], le[1], dummy);
637 for (int k=0; k<2; ++k) {
639 float isecval = sgIsectLinesegPlane( isecpoint, le[0], le[1], plane[k] );
641 if ( 0.0 <= isecval && isecval <= 1.0 &&
642 sgPointInTriangle( isecpoint, tri[k] ) ) {
643 // Store the wire id.
644 wire_id = gp->wire_id;
654 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
656 // Fast return if we do not have an active wire.
662 // Time difference to th reference time.
665 // We know that we have a flat cache ...
667 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
668 // We just know that, because we build that ourselfs ...
669 ssgVtxArray *va = (ssgVtxArray *)e;
670 // Only lines are interresting ...
671 if (va->getPrimitiveType() != GL_LINES)
673 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
677 // Check if we have a catapult ...
678 if ( gp->type != FGInterface::Wire )
680 if ( gp->wire_id != wire_id )
683 // Get the line ends, that are the wire endpoints.
685 va->getLine(0, vi, vi+1 );
686 extractWgs84Vertex(t, va, gp, vi[0], end[0], vel[0]);
687 extractWgs84Vertex(t, va, gp, vi[1], end[1], vel[1]);
695 void FGGroundCache::release_wire(void)