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 ssgNormalArray *na = ((ssgVtxTable *)l)->getNormals();
147 // Create a new leaf.
148 ssgVtxArray *vtxa = new ssgVtxArray( ty, va, na, 0, 0, ia );
150 vtxa->removeUnusedVertices();
151 // Apply transform. We won't store transforms in our cache.
152 vtxa->transform( xform );
153 // Check for magic texture names object names and such ...
154 GroundProperty *gp = extractGroundProperty( l );
157 cerr << "Newly created cache leaf where userdata is not a Ground property!" << endl;
159 vtxa->setUserData( gp );
160 vtxa->setCullFace( l->getCullFace() );
162 // Finally append to cache.
163 cache_root.addKid((ssgEntity*)vtxa);
167 FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform,
170 ssgIndexArray *ia = 0;
172 // Lines must have special meanings.
173 // Wires and catapults are done with lines.
174 int nl = l->getNumLines();
175 for (int i = 0; i < nl; ++i) {
178 l->getLine(i, v, v+1 );
179 for (int k=0; k<2; ++k)
180 tmp.extend( l->getVertex( v[k] ) );
181 tmp.orthoXform(xform);
183 if (wsp->intersects( &tmp )) {
185 ia = new ssgIndexArray();
194 addAndFlattenLeaf(GL_LINES, l, ia, xform);
198 FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform,
199 bool sphIsec, sgVec3 down, ssgLeaf *l)
201 ssgIndexArray *ia = 0;
203 int nt = l->getNumTriangles();
204 for (int i = 0; i < nt; ++i) {
205 // Build up a sphere around that particular triangle-
208 l->getTriangle(i, v, v+1, v+2 );
209 for (int k=0; k<3; ++k)
210 tmp.extend( l->getVertex( v[k] ) );
211 tmp.orthoXform(xform);
213 // Check if the sphere around the vehicle intersects the sphere
214 // around that triangle. If so, put that triangle into the cache.
215 if (sphIsec && sp->intersects( &tmp )) {
217 ia = new ssgIndexArray();
224 // In case the cache is empty, we still provide agl computations.
225 // But then we use the old way of having a fixed elevation value for
226 // the whole lifetime of this cache.
227 if ( sgIsectSphereInfLine(&tmp, sp->getCenter(), down) ) {
229 for (int k=0; k<3; ++k) {
230 sgCopyVec3( tri[k], l->getVertex( v[k] ) );
231 sgXformPnt3( tri[k], xform );
235 sgMakePlane( plane, tri[0], tri[1], tri[2] );
237 sgCopyVec3(ac_cent, sp->getCenter());
239 sgIsectInfLinePlane( dst, ac_cent, down, plane );
240 if ( sgPointInTriangle ( dst, tri ) ) {
243 sgdSetVec3(ddst, dst);
244 sgdAddVec3(ddst, cache_center);
245 double this_radius = sgdLengthVec3(ddst);
246 if (ground_radius < this_radius)
247 ground_radius = this_radius;
254 addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform);
257 // Here is the point where rotation should be handled
259 FGGroundCache::extractCacheRelativeVertex(double t, ssgVtxArray *va,
261 short i, sgVec3 rel_pos,
264 sgCopyVec3( rel_pos, va->getVertex( i ) );
265 sgAddScaledVec3( rel_pos, gp->vel, t );
268 sgdSetVec3( wgs84_vel, gp->vel );
272 FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va,
273 GroundProperty *gp, short i,
274 sgdVec3 wgs84_pos, sgdVec3 wgs84_vel)
277 extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel);
278 sgdSetVec3( wgs84_pos, rel_pos );
279 sgdAddVec3( wgs84_pos, cache_center );
284 FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
285 sgSphere* sp, sgVec3 down, sgSphere* wsp)
287 // Travel through all kids.
289 for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
290 if ( !( e->getTraversalMask() & SSGTRAV_HOT) )
292 if ( e->getBSphere()->isEmpty() )
295 // Wee need to check further if either the sphere around the branch
296 // intersects the sphere around the aircraft or the line downwards from
297 // the aircraft intersects the branchs sphere.
298 sgSphere esphere = *(e->getBSphere());
299 esphere.orthoXform(xform);
300 bool wspIsec = wsp->intersects(&esphere);
301 bool downIsec = sgIsectSphereInfLine(&esphere, sp->getCenter(), down);
302 if (!wspIsec && !downIsec)
305 // For branches collect up the transforms to reach that branch and
306 // call cache_fill recursively.
307 if ( e->isAKindOf( ssgTypeBranch() ) ) {
308 ssgBranch *b = (ssgBranch *)e;
309 if ( b->isAKindOf( ssgTypeTransform() ) ) {
310 // Collect up the transfors required to reach that part of
313 sgMakeIdentMat4( xform2 );
314 ssgTransform *t = (ssgTransform*)b;
315 t->getTransform( xform2 );
316 sgPostMultMat4( xform2, xform );
317 cache_fill( b, xform2, sp, down, wsp );
319 cache_fill( b, xform, sp, down, wsp );
322 // For leafs, check each triangle for intersection.
323 // This will minimize the number of vertices/triangles in the cache.
324 else if (e->isAKindOf(ssgTypeLeaf())) {
325 // Since we reach that leaf if we have an intersection with the
326 // most propably bigger wire/catapult cache sphere, we need to check
327 // that here, if the smaller cache for the surface has a chance for hits.
328 // Also, if the spheres do not intersect compute a croase agl value
329 // by following the line downwards originating at the aircraft.
330 bool spIsec = sp->intersects(&esphere);
331 putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
333 // If we are here, we need to put all special hardware here into
336 putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
342 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
346 cache_root.removeAllKids();
348 found_ground = false;
350 // Store the parameters we used to build up that cache.
351 sgdCopyVec3(reference_wgs84_point, pt);
352 reference_vehicle_radius = rad;
353 // Store the time reference used to compute movements of moving triangles.
354 cache_ref_time = ref_time;
356 // The center of the cache.
357 sgdCopyVec3(cache_center, pt);
361 // Prepare sphere around the aircraft.
363 acSphere.setRadius(rad);
364 acSphere.setCenter(zero);
366 // Prepare bigger sphere around the aircraft.
367 // This one is required for reliably finding wires we have caught but
368 // have already left the hopefully smaller sphere for the ground reactions.
369 const double max_wire_dist = 300.0;
371 wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
372 wireSphere.setCenter(zero);
374 // Down vector. Is used for croase agl computations when we are far enough
375 // from ground that we have an empty cache.
377 sgSetVec3(down, -pt[0], -pt[1], -pt[2]);
378 sgNormalizeVec3(down);
380 // We need the offset to the scenery scenery center.
382 Point3D psc = globals->get_tile_mgr()->get_current_center();
383 sgdSetVec3(doffset, psc[0], psc[1], psc[2]);
384 sgdSubVec3(doffset, doffset, pt);
386 // We collaps all transforms we need to reach a particular leaf.
387 // The leafs itself will be then transformed later.
388 // So our cache is just flat.
389 // For leafs which are moving (carriers surface, etc ...)
390 // we will later store a speed in the GroundType class. We can then apply
391 // some translations to that nodes according to the time which has passed
392 // compared to that snapshot.
394 sgSetVec3(offset, doffset[0], doffset[1], doffset[2]);
396 sgMakeTransMat4(xform, offset);
399 // Walk the terrain branch for now.
400 ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
401 cache_fill(terrain, xform, &acSphere, down, &wireSphere);
404 SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad
405 << ", # leafs = " << cache_root.getNumKids()
406 << ", ground_radius = " << ground_radius );
408 // If the ground radius is still below 5e6 meters, then we do not yet have
410 found_ground = found_ground && 5e6 < ground_radius;
412 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
413 "without any scenery below the aircraft" );
419 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
421 sgdCopyVec3(pt, reference_wgs84_point);
422 *rad = reference_vehicle_radius;
423 *ref_time = cache_ref_time;
428 FGGroundCache::get_cat(double t, const double dpt[3],
429 double end[2][3], double vel[2][3])
431 // start with a distance of 1e10 meters...
434 // Time difference to the reference time.
437 // We know that we have a flat cache ...
439 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
440 // We just know that, because we build that ourselfs ...
441 ssgVtxArray *va = (ssgVtxArray *)e;
442 // Only lines are interresting ...
443 if (va->getPrimitiveType() != GL_LINES)
445 GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
446 // Check if we have a catapult ...
447 if ( gp->type != FGInterface::Catapult )
450 int nl = va->getNumLines();
451 for (int i=0; i < nl; ++i) {
455 va->getLine(i, vi, vi+1 );
456 extractWgs84Vertex(t, va, gp, vi[0], ls.a, lsVel[0]);
457 extractWgs84Vertex(t, va, gp, vi[1], ls.b, lsVel[1]);
459 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
460 if (this_dist < dist) {
463 // end[0] is the end where the cat starts.
464 // end[1] is the end where the cat ends.
465 // The carrier code takes care of that ordering.
466 sgdCopyVec3( end[0], ls.a );
467 sgdCopyVec3( end[1], ls.b );
468 sgdCopyVec3( vel[0], lsVel[0] );
469 sgdCopyVec3( vel[1], lsVel[1] );
474 // At the end take the root, we only computed squared distances ...
479 FGGroundCache::get_agl(double t, const double dpt[3],
480 double contact[3], double normal[3], double vel[3],
481 int *type, double *loadCapacity,
482 double *frictionFactor, double *agl)
486 *type = FGInterface::Unknown;
488 *loadCapacity = DBL_MAX;
489 *frictionFactor = 1.0;
490 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
491 sgdSetVec3( contact, 0.0, 0.0, 0.0 );
492 sgdSetVec3( normal, 0.0, 0.0, 0.0 );
494 // Time difference to th reference time.
497 // The double valued point we start to search for intersection.
499 sgdSubVec3( tmp, dpt, cache_center );
501 sgSetVec3( pt, tmp );
503 // The search direction
505 sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
507 // Initialize to something sensible
508 double sqdist = DBL_MAX;
510 // We know that we have a flat cache ...
511 // We just know that, because we build that ourselfs ...
513 for ( e = cache_root.getKid(0) ; e != NULL ; e = cache_root.getNextKid() ) {
514 // We just know that, because we build that ourselfs ...
515 ssgVtxArray *va = (ssgVtxArray *)e;
516 // AGL computations are done with triangle/surface leafs.
517 if (va->getPrimitiveType() != GL_TRIANGLES)
519 ssgBase *gpb = va->getUserData();
522 cerr << "Found cache leaf without userdata!" << endl;
525 GroundProperty *gp = static_cast<GroundProperty*>(gpb);
527 int nt = va->getNumTriangles();
528 for (int i=0; i < nt; ++i) {
530 va->getTriangle( i, vi, vi+1, vi+2 );
534 for (int k=0; k<3; ++k)
535 extractCacheRelativeVertex(t, va, gp, vi[k], tri[k], dvel[k]);
537 sgMakePlane( plane, tri[0], tri[1], tri[2] );
539 // Check for intersection.
541 if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
542 sgPointInTriangle3( isecpoint, tri ) ) {
543 // Only accept surfaces with the normal pointing upwards.
544 // For double sided surfaces flip the normal in this case.
545 float dirDot = sgScalarProductVec3(plane, dir);
546 if ( dirDot >= 0 && va->getCullFace() == 1 ) {
547 sgScaleVec4( plane, -1 );
551 // Check for the closest intersection point.
552 // FIXME: is this the right one?
553 double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
554 if ( newSqdist < sqdist && dirDot < 0 ) {
557 // Save the new potential intersection point.
558 sgdSetVec3( contact, isecpoint );
559 sgdAddVec3( contact, cache_center );
560 // The first three values in the vector are the plane normal.
561 sgdSetVec3( normal, plane );
562 // Remormalize that as double, else it *can* have surprising effects
563 // when used as plane normal together with a 6000000m offset in a
565 sgdNormalizeVec3( normal );
566 // The velocity wrt earth.
567 /// FIXME: only true for non rotating objects!!!!
568 sgdCopyVec3( vel, dvel[0] );
569 // Save the ground type.
571 // FIXME: figure out how to get that sign ...
572 // *agl = sqrt(sqdist);
573 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
574 // *loadCapacity = DBL_MAX;
575 // *frictionFactor = 1.0;
584 // Whenever we did not have a ground triangle for the requested point,
585 // take the ground level we found during the current cache build.
586 // This is as good as what we had before for agl.
587 double r = sgdLengthVec3( dpt );
588 sgdCopyVec3( contact, dpt );
589 sgdScaleVec3( contact, ground_radius/r );
590 sgdCopyVec3( normal, dpt );
591 sgdNormaliseVec3( normal );
592 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
594 // The altitude is the distance of the requested point from the
596 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
597 *type = FGInterface::Unknown;
598 *loadCapacity = DBL_MAX;
599 *frictionFactor = 1.0;
604 bool FGGroundCache::caught_wire(double t, const double cpt[4][3])
608 // Time difference to the reference time.
611 bool firsttime = true;
615 // We know that we have a flat cache ...
617 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
618 // We just know that, because we build that ourselfs ...
619 ssgVtxArray *va = (ssgVtxArray *)e;
620 // Only lines are interresting ...
621 if (va->getPrimitiveType() != GL_LINES)
623 GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
624 // Check if we have a catapult ...
625 if ( gp->type != FGInterface::Wire )
628 // Lazy compute the values required for intersectiion tests.
629 // Since we normally do not have wires in the cache this is a
634 for (int k=0; k<4; ++k) {
636 sgdSubVec3( tmp, cpt[k], cache_center );
637 sgSetVec3( pt[k], tmp );
639 sgMakePlane( plane[0], pt[0], pt[1], pt[2] );
640 sgCopyVec3( tri[0][0], pt[0] );
641 sgCopyVec3( tri[0][1], pt[1] );
642 sgCopyVec3( tri[0][2], pt[2] );
643 sgMakePlane( plane[1], pt[0], pt[2], pt[3] );
644 sgCopyVec3( tri[1][0], pt[0] );
645 sgCopyVec3( tri[1][1], pt[2] );
646 sgCopyVec3( tri[1][2], pt[3] );
649 int nl = va->getNumLines();
650 for (int i=0; i < nl; ++i) {
652 va->getLine(i, vi, vi+1 );
655 extractCacheRelativeVertex(t, va, gp, vi[0], le[0], dummy);
656 extractCacheRelativeVertex(t, va, gp, vi[1], le[1], dummy);
658 for (int k=0; k<2; ++k) {
660 float isecval = sgIsectLinesegPlane( isecpoint, le[0], le[1], plane[k] );
662 if ( 0.0 <= isecval && isecval <= 1.0 &&
663 sgPointInTriangle( isecpoint, tri[k] ) ) {
664 // Store the wire id.
665 wire_id = gp->wire_id;
675 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
677 // Fast return if we do not have an active wire.
683 // Time difference to th reference time.
686 // We know that we have a flat cache ...
688 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
689 // We just know that, because we build that ourselfs ...
690 ssgVtxArray *va = (ssgVtxArray *)e;
691 // Only lines are interresting ...
692 if (va->getPrimitiveType() != GL_LINES)
694 GroundProperty *gp = static_cast<GroundProperty*>(va->getUserData());
695 // Check if we have a catapult ...
696 if ( gp->type != FGInterface::Wire )
698 if ( gp->wire_id != wire_id )
701 // Get the line ends, that are the wire endpoints.
703 va->getLine(0, vi, vi+1 );
704 extractWgs84Vertex(t, va, gp, vi[0], end[0], vel[0]);
705 extractWgs84Vertex(t, va, gp, vi[1], end[1], vel[1]);
713 void FGGroundCache::release_wire(void)