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 vtxa->setUserData( extractGroundProperty( l ) );
155 vtxa->setCullFace( l->getCullFace() );
156 // Finally append to cache.
157 cache_root.addKid((ssgEntity*)vtxa);
161 FGGroundCache::putLineLeafIntoCache(const sgSphere *wsp, const sgMat4 xform,
164 ssgIndexArray *ia = 0;
166 // Lines must have special meanings.
167 // Wires and catapults are done with lines.
168 int nl = l->getNumLines();
169 for (int i = 0; i < nl; ++i) {
172 l->getLine(i, v, v+1 );
173 for (int k=0; k<2; ++k)
174 tmp.extend( l->getVertex( v[k] ) );
175 tmp.orthoXform(xform);
177 if (wsp->intersects( &tmp )) {
179 ia = new ssgIndexArray();
188 addAndFlattenLeaf(GL_LINES, l, ia, xform);
192 FGGroundCache::putSurfaceLeafIntoCache(const sgSphere *sp, const sgMat4 xform,
193 bool sphIsec, sgVec3 down, ssgLeaf *l)
195 ssgIndexArray *ia = 0;
197 int nt = l->getNumTriangles();
198 for (int i = 0; i < nt; ++i) {
199 // Build up a sphere around that particular triangle-
202 l->getTriangle(i, v, v+1, v+2 );
203 for (int k=0; k<3; ++k)
204 tmp.extend( l->getVertex( v[k] ) );
205 tmp.orthoXform(xform);
207 // Check if the sphere around the vehicle intersects the sphere
208 // around that triangle. If so, put that triangle into the cache.
209 if (sphIsec && sp->intersects( &tmp )) {
211 ia = new ssgIndexArray();
218 // In case the cache is empty, we still provide agl computations.
219 // But then we use the old way of having a fixed elevation value for
220 // the whole lifetime of this cache.
221 if ( sgIsectSphereInfLine(&tmp, sp->getCenter(), down) ) {
223 for (int k=0; k<3; ++k) {
224 sgCopyVec3( tri[k], l->getVertex( v[k] ) );
225 sgXformPnt3( tri[k], xform );
229 sgMakePlane( plane, tri[0], tri[1], tri[2] );
231 sgCopyVec3(ac_cent, sp->getCenter());
233 sgIsectInfLinePlane( dst, ac_cent, down, plane );
234 if ( sgPointInTriangle ( dst, tri ) ) {
237 sgdSetVec3(ddst, dst);
238 sgdAddVec3(ddst, cache_center);
239 double this_radius = sgdLengthVec3(ddst);
240 if (ground_radius < this_radius)
241 ground_radius = this_radius;
248 addAndFlattenLeaf(GL_TRIANGLES, l, ia, xform);
251 // Here is the point where rotation should be handled
253 FGGroundCache::extractCacheRelativeVertex(double t, ssgVtxArray *va,
255 short i, sgVec3 rel_pos,
258 sgCopyVec3( rel_pos, va->getVertex( i ) );
259 sgAddScaledVec3( rel_pos, gp->vel, t );
262 sgdSetVec3( wgs84_vel, gp->vel );
266 FGGroundCache::extractWgs84Vertex(double t, ssgVtxArray *va,
267 GroundProperty *gp, short i,
268 sgdVec3 wgs84_pos, sgdVec3 wgs84_vel)
271 extractCacheRelativeVertex(t, va, gp, i, rel_pos, wgs84_vel);
272 sgdSetVec3( wgs84_pos, rel_pos );
273 sgdAddVec3( wgs84_pos, cache_center );
278 FGGroundCache::cache_fill(ssgBranch *branch, sgMat4 xform,
279 sgSphere* sp, sgVec3 down, sgSphere* wsp)
281 // Travel through all kids.
283 for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
284 if ( !( e->getTraversalMask() & SSGTRAV_HOT) )
286 if ( e->getBSphere()->isEmpty() )
289 // Wee need to check further if either the sphere around the branch
290 // intersects the sphere around the aircraft or the line downwards from
291 // the aircraft intersects the branchs sphere.
292 sgSphere esphere = *(e->getBSphere());
293 esphere.orthoXform(xform);
294 bool wspIsec = wsp->intersects(&esphere);
295 bool downIsec = sgIsectSphereInfLine(&esphere, sp->getCenter(), down);
296 if (!wspIsec && !downIsec)
299 // For branches collect up the transforms to reach that branch and
300 // call cache_fill recursively.
301 if ( e->isAKindOf( ssgTypeBranch() ) ) {
302 ssgBranch *b = (ssgBranch *)e;
303 if ( b->isAKindOf( ssgTypeTransform() ) ) {
304 // Collect up the transfors required to reach that part of
307 sgMakeIdentMat4( xform2 );
308 ssgTransform *t = (ssgTransform*)b;
309 t->getTransform( xform2 );
310 sgPostMultMat4( xform2, xform );
311 cache_fill( b, xform2, sp, down, wsp );
313 cache_fill( b, xform, sp, down, wsp );
316 // For leafs, check each triangle for intersection.
317 // This will minimize the number of vertices/triangles in the cache.
318 else if (e->isAKindOf(ssgTypeLeaf())) {
319 // Since we reach that leaf if we have an intersection with the
320 // most propably bigger wire/catapult cache sphere, we need to check
321 // that here, if the smaller cache for the surface has a chance for hits.
322 // Also, if the spheres do not intersect compute a croase agl value
323 // by following the line downwards originating at the aircraft.
324 bool spIsec = sp->intersects(&esphere);
325 putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
327 // If we are here, we need to put all special hardware here into
330 putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
336 FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
340 cache_root.removeAllKids();
342 found_ground = false;
344 // Store the parameters we used to build up that cache.
345 sgdCopyVec3(reference_wgs84_point, pt);
346 reference_vehicle_radius = rad;
347 // Store the time reference used to compute movements of moving triangles.
348 cache_ref_time = ref_time;
350 // The center of the cache.
351 Point3D psc = globals->get_tile_mgr()->get_current_center();
352 sgdSetVec3(cache_center, psc[0], psc[1], psc[2]);
354 // Prepare sphere around the aircraft.
356 acSphere.setRadius(rad);
358 // Compute the postion of the aircraft relative to the scenery center.
360 sgdSubVec3( doffset, pt, cache_center );
362 sgSetVec3( offset, doffset[0], doffset[1], doffset[2] );
363 acSphere.setCenter( offset );
365 // Prepare bigger sphere around the aircraft.
366 // This one is required for reliably finding wires we have caught but
367 // have already left the hopefully smaller sphere for the ground reactions.
368 const double max_wire_dist = 300.0;
370 wireSphere.setRadius( max_wire_dist < rad ? rad : max_wire_dist );
371 wireSphere.setCenter( offset );
373 // Down vector. Is used for croase agl computations when we are far enough
374 // from ground that we have an empty cache.
376 sgSetVec3(down, -pt[0], -pt[1], -pt[2]);
377 sgNormalizeVec3(down);
379 // We collaps all transforms we need to reach a particular leaf.
380 // The leafs itself will be then transformed later.
381 // So our cache is just flat.
382 // For leafs which are moving (carriers surface, etc ...)
383 // we will later store a speed in the GroundType class. We can then apply
384 // some translations to that nodes according to the time which has passed
385 // compared to that snapshot.
387 sgMakeIdentMat4(xform);
389 // Walk the terrain branch for now.
390 ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
391 cache_fill(terrain, xform, &acSphere, down, &wireSphere);
394 SG_LOG(SG_FLIGHT,SG_INFO, "prepare_ground_cache(): ac radius = " << rad
395 << ", # leafs = " << cache_root.getNumKids()
396 << ", ground_radius = " << ground_radius );
398 // If the ground radius is still below 5e6 meters, then we do not yet have
400 found_ground = found_ground && 5e6 < ground_radius;
402 SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
403 "without any scenery below the aircraft" );
409 FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
411 sgdCopyVec3(pt, reference_wgs84_point);
412 *rad = reference_vehicle_radius;
413 *ref_time = cache_ref_time;
418 FGGroundCache::get_cat(double t, const double dpt[3],
419 double end[2][3], double vel[2][3])
421 // start with a distance of 1e10 meters...
424 // Time difference to the reference time.
427 // We know that we have a flat cache ...
429 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
430 // We just know that, because we build that ourselfs ...
431 ssgVtxArray *va = (ssgVtxArray *)e;
432 // Only lines are interresting ...
433 if (va->getPrimitiveType() != GL_LINES)
435 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
439 // Check if we have a catapult ...
440 if ( gp->type != FGInterface::Catapult )
443 int nl = va->getNumLines();
444 for (int i=0; i < nl; ++i) {
448 va->getLine(i, vi, vi+1 );
449 extractWgs84Vertex(t, va, gp, vi[0], ls.a, lsVel[0]);
450 extractWgs84Vertex(t, va, gp, vi[1], ls.b, lsVel[1]);
452 double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
453 if (this_dist < dist) {
456 // end[0] is the end where the cat starts.
457 // end[1] is the end where the cat ends.
458 // The carrier code takes care of that ordering.
459 sgdCopyVec3( end[0], ls.a );
460 sgdCopyVec3( end[1], ls.b );
461 sgdCopyVec3( vel[0], lsVel[0] );
462 sgdCopyVec3( vel[1], lsVel[1] );
467 // At the end take the root, we only computed squared distances ...
472 FGGroundCache::get_agl(double t, const double dpt[3],
473 double contact[3], double normal[3], double vel[3],
474 int *type, double *loadCapacity,
475 double *frictionFactor, double *agl)
479 *type = FGInterface::Unknown;
481 *loadCapacity = DBL_MAX;
482 *frictionFactor = 1.0;
483 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
484 sgdSetVec3( contact, 0.0, 0.0, 0.0 );
485 sgdSetVec3( normal, 0.0, 0.0, 0.0 );
487 // Time difference to th reference time.
490 // The double valued point we start to search for intersection.
492 sgdSubVec3( tmp, dpt, cache_center );
494 sgSetVec3( pt, tmp );
496 // The search direction
498 sgSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
500 // Initialize to something sensible
501 double sqdist = DBL_MAX;
503 // We know that we have a flat cache ...
504 // We just know that, because we build that ourselfs ...
506 for ( e = cache_root.getKid(0) ; e != NULL ; e = cache_root.getNextKid() ) {
507 // We just know that, because we build that ourselfs ...
508 ssgVtxArray *va = (ssgVtxArray *)e;
509 // AGL computations are done with triangle/surface leafs.
510 if (va->getPrimitiveType() != GL_TRIANGLES)
512 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
517 int nt = va->getNumTriangles();
518 for (int i=0; i < nt; ++i) {
520 va->getTriangle( i, vi, vi+1, vi+2 );
524 for (int k=0; k<3; ++k)
525 extractCacheRelativeVertex(t, va, gp, vi[k], tri[k], dvel[k]);
527 sgMakePlane( plane, tri[0], tri[1], tri[2] );
529 // Check for intersection.
531 if ( sgIsectInfLinePlane( isecpoint, pt, dir, plane ) &&
532 sgPointInTriangle3( isecpoint, tri ) ) {
533 // Only accept surfaces with the normal pointing upwards.
534 // For double sided surfaces flip the normal in this case.
535 float dirDot = sgScalarProductVec3(plane, dir);
536 if ( dirDot >= 0 && va->getCullFace() == 1 ) {
537 sgScaleVec4( plane, -1 );
541 // Check for the closest intersection point.
542 // FIXME: is this the right one?
543 double newSqdist = sgDistanceSquaredVec3( isecpoint, pt );
544 if ( newSqdist < sqdist && dirDot < 0 ) {
547 // Save the new potential intersection point.
548 sgdSetVec3( contact, isecpoint );
549 sgdAddVec3( contact, cache_center );
550 // The first three values in the vector are the plane normal.
551 sgdSetVec3( normal, plane );
552 // The velocity wrt earth.
553 /// FIXME: only true for non rotating objects!!!!
554 sgdCopyVec3( vel, dvel[0] );
555 // Save the ground type.
557 // FIXME: figure out how to get that sign ...
558 // *agl = sqrt(sqdist);
559 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
560 // *loadCapacity = DBL_MAX;
561 // *frictionFactor = 1.0;
570 // Whenever we did not have a ground triangle for the requested point,
571 // take the ground level we found during the current cache build.
572 // This is as good as what we had before for agl.
573 double r = sgdLengthVec3( dpt );
574 sgdCopyVec3( contact, dpt );
575 sgdScaleVec3( contact, ground_radius/r );
576 sgdCopyVec3( normal, dpt );
577 sgdNormaliseVec3( normal );
578 sgdSetVec3( vel, 0.0, 0.0, 0.0 );
580 // The altitude is the distance of the requested point from the
582 *agl = sgdLengthVec3( dpt ) - sgdLengthVec3( contact );
583 *type = FGInterface::Unknown;
584 *loadCapacity = DBL_MAX;
585 *frictionFactor = 1.0;
590 bool FGGroundCache::caught_wire(double t, const double cpt[4][3])
594 // Time difference to the reference time.
597 bool firsttime = true;
601 // We know that we have a flat cache ...
603 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
604 // We just know that, because we build that ourselfs ...
605 ssgVtxArray *va = (ssgVtxArray *)e;
606 // Only lines are interresting ...
607 if (va->getPrimitiveType() != GL_LINES)
609 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
613 // Check if we have a catapult ...
614 if ( gp->type != FGInterface::Wire )
617 // Lazy compute the values required for intersectiion tests.
618 // Since we normally do not have wires in the cache this is a
623 for (int k=0; k<4; ++k) {
625 sgdSubVec3( tmp, cpt[k], cache_center );
626 sgSetVec3( pt[k], tmp );
628 sgMakePlane( plane[0], pt[0], pt[1], pt[2] );
629 sgCopyVec3( tri[0][0], pt[0] );
630 sgCopyVec3( tri[0][1], pt[1] );
631 sgCopyVec3( tri[0][2], pt[2] );
632 sgMakePlane( plane[1], pt[0], pt[2], pt[3] );
633 sgCopyVec3( tri[1][0], pt[0] );
634 sgCopyVec3( tri[1][1], pt[2] );
635 sgCopyVec3( tri[1][2], pt[3] );
638 int nl = va->getNumLines();
639 for (int i=0; i < nl; ++i) {
641 va->getLine(i, vi, vi+1 );
644 extractCacheRelativeVertex(t, va, gp, vi[0], le[0], dummy);
645 extractCacheRelativeVertex(t, va, gp, vi[1], le[1], dummy);
647 for (int k=0; k<2; ++k) {
649 float isecval = sgIsectLinesegPlane( isecpoint, le[0], le[1], plane[k] );
651 if ( 0.0 <= isecval && isecval <= 1.0 &&
652 sgPointInTriangle( isecpoint, tri[k] ) ) {
653 // Store the wire id.
654 wire_id = gp->wire_id;
664 bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
666 // Fast return if we do not have an active wire.
672 // Time difference to th reference time.
675 // We know that we have a flat cache ...
677 for ( e = cache_root.getKid(0); e != NULL ; e = cache_root.getNextKid() ) {
678 // We just know that, because we build that ourselfs ...
679 ssgVtxArray *va = (ssgVtxArray *)e;
680 // Only lines are interresting ...
681 if (va->getPrimitiveType() != GL_LINES)
683 GroundProperty *gp = dynamic_cast<GroundProperty*>(va->getUserData());
687 // Check if we have a catapult ...
688 if ( gp->type != FGInterface::Wire )
690 if ( gp->wire_id != wire_id )
693 // Get the line ends, that are the wire endpoints.
695 va->getLine(0, vi, vi+1 );
696 extractWgs84Vertex(t, va, gp, vi[0], end[0], vel[0]);
697 extractWgs84Vertex(t, va, gp, vi[1], end[1], vel[1]);
705 void FGGroundCache::release_wire(void)