#include <float.h>
-#include <plib/sg.h>
+#include <utility>
+
+#include <osg/CullFace>
+#include <osg/Drawable>
+#include <osg/Geode>
+#include <osg/Geometry>
+#include <osg/PrimitiveSet>
+#include <osg/TriangleFunctor>
+
+#include <osgUtil/PolytopeIntersector>
#include <simgear/sg_inlines.h>
#include <simgear/constants.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/scene/material/mat.hxx>
#include <simgear/scene/material/matlib.hxx>
+#include <simgear/scene/util/PrimitiveUtils.hxx>
+#include <simgear/scene/util/SGNodeMasks.hxx>
#include <Main/globals.hxx>
#include <Scenery/scenery.hxx>
#include "flight.hxx"
#include "groundcache.hxx"
-// Specialized version of sgMultMat4 needed because of mixed matrix
-// types
-static inline void fgMultMat4(sgdMat4 dst, sgdMat4 m1, sgMat4 m2) {
- for ( int j = 0 ; j < 4 ; j++ ) {
- dst[0][j] = m2[0][0] * m1[0][j] +
- m2[0][1] * m1[1][j] +
- m2[0][2] * m1[2][j] +
- m2[0][3] * m1[3][j] ;
-
- dst[1][j] = m2[1][0] * m1[0][j] +
- m2[1][1] * m1[1][j] +
- m2[1][2] * m1[2][j] +
- m2[1][3] * m1[3][j] ;
-
- dst[2][j] = m2[2][0] * m1[0][j] +
- m2[2][1] * m1[1][j] +
- m2[2][2] * m1[2][j] +
- m2[2][3] * m1[3][j] ;
-
- dst[3][j] = m2[3][0] * m1[0][j] +
- m2[3][1] * m1[1][j] +
- m2[3][2] * m1[2][j] +
- m2[3][3] * m1[3][j] ;
+using namespace osg;
+using namespace osgUtil;
+using namespace simgear;
+
+void makePolytopeShaft(Polytope& polyt, const Vec3d& refPoint,
+ const Vec3d& direction, double radius)
+{
+ polyt.clear();
+ // Choose best principal axis to start making orthogonal axis.
+ Vec3d majorAxis;
+ if (fabs(direction.x()) <= fabs(direction.y())) {
+ if (fabs(direction.z()) <= fabs(direction.x()))
+ majorAxis = Vec3d(0.0, 0.0, 1.0);
+ else
+ majorAxis = Vec3d(1.0, 0.0, 0.0);
+ } else {
+ if (fabs(direction.z()) <= fabs(direction.y()))
+ majorAxis = Vec3d(0.0, 0.0, 1.0);
+ else
+ majorAxis = Vec3d(0.0, 1.0, 0.0);
}
+ Vec3d axis1 = majorAxis ^ direction;
+ axis1.normalize();
+ Vec3d axis2 = direction ^ axis1;
+
+ polyt.add(Plane(-axis1, refPoint + axis1 * radius));
+ polyt.add(Plane(axis1, refPoint - axis1 * radius));
+ polyt.add(Plane(-axis2, refPoint + axis2 * radius));
+ polyt.add(Plane(axis2 , refPoint - axis2 * radius));
}
-static inline bool fgdPointInTriangle( sgdVec3 point, sgdVec3 tri[3] )
+void makePolytopeBox(Polytope& polyt, const Vec3d& center,
+ const Vec3d& direction, double radius)
{
- sgdVec3 dif;
-
- // Some tolerance in meters we accept a point to be outside of the triangle
- // and still return that it is inside.
- SGDfloat eps = 1e-2;
- SGDfloat min, max;
- // punt if outside bouding cube
- SG_MIN_MAX3 ( min, max, tri[0][0], tri[1][0], tri[2][0] );
- if( (point[0] < min - eps) || (point[0] > max + eps) )
- return false;
- dif[0] = max - min;
-
- SG_MIN_MAX3 ( min, max, tri[0][1], tri[1][1], tri[2][1] );
- if( (point[1] < min - eps) || (point[1] > max + eps) )
- return false;
- dif[1] = max - min;
-
- SG_MIN_MAX3 ( min, max, tri[0][2], tri[1][2], tri[2][2] );
- if( (point[2] < min - eps) || (point[2] > max + eps) )
- return false;
- dif[2] = max - min;
-
- // drop the smallest dimension so we only have to work in 2d.
- SGDfloat min_dim = SG_MIN3 (dif[0], dif[1], dif[2]);
- SGDfloat x1, y1, x2, y2, x3, y3, rx, ry;
- if ( fabs(min_dim-dif[0]) <= DBL_EPSILON ) {
- // x is the smallest dimension
- x1 = point[1];
- y1 = point[2];
- x2 = tri[0][1];
- y2 = tri[0][2];
- x3 = tri[1][1];
- y3 = tri[1][2];
- rx = tri[2][1];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[1]) <= DBL_EPSILON ) {
- // y is the smallest dimension
- x1 = point[0];
- y1 = point[2];
- x2 = tri[0][0];
- y2 = tri[0][2];
- x3 = tri[1][0];
- y3 = tri[1][2];
- rx = tri[2][0];
- ry = tri[2][2];
- } else if ( fabs(min_dim-dif[2]) <= DBL_EPSILON ) {
- // z is the smallest dimension
- x1 = point[0];
- y1 = point[1];
- x2 = tri[0][0];
- y2 = tri[0][1];
- x3 = tri[1][0];
- y3 = tri[1][1];
- rx = tri[2][0];
- ry = tri[2][1];
- } else {
- // all dimensions are really small so lets call it close
- // enough and return a successful match
- return true;
+ makePolytopeShaft(polyt, center, direction, radius);
+ polyt.add(Plane(-direction, center + direction * radius));
+ polyt.add(Plane(direction, center - direction * radius));
+}
+
+// Intersector for finding catapults and wires
+
+class WireIntersector : public PolytopeIntersector
+{
+public:
+ WireIntersector(const Polytope& polytope)
+ : PolytopeIntersector(polytope), depth(0)
+ {
+ setDimensionMask(DimOne);
}
- // check if intersection point is on the same side of p1 <-> p2 as p3
- SGDfloat tmp = (y2 - y3);
- SGDfloat tmpn = (x2 - x3);
- int side1 = SG_SIGN (tmp * (rx - x3) + (y3 - ry) * tmpn);
- int side2 = SG_SIGN (tmp * (x1 - x3) + (y3 - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 1 check\n");
- return false;
+ bool enter(const osg::Node& node)
+ {
+ if (!PolytopeIntersector::enter(node))
+ return false;
+ const Referenced* base = node.getUserData();
+ if (base) {
+ const FGAICarrierHardware *ud
+ = dynamic_cast<const FGAICarrierHardware*>(base);
+ if (ud)
+ carriers.push_back(depth);
+ }
+ depth++;
+ return true;
}
- // check if intersection point is on correct side of p2 <-> p3 as p1
- tmp = (y3 - ry);
- tmpn = (x3 - rx);
- side1 = SG_SIGN (tmp * (x2 - rx) + (ry - y2) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 2 check\n");
- return false;
+ void leave()
+ {
+ depth--;
+ if (!carriers.empty() && depth == carriers.back())
+ carriers.pop_back();
}
- // check if intersection point is on correct side of p1 <-> p3 as p2
- tmp = (y2 - ry);
- tmpn = (x2 - rx);
- side1 = SG_SIGN (tmp * (x3 - rx) + (ry - y3) * tmpn);
- side2 = SG_SIGN (tmp * (x1 - rx) + (ry - y1) * tmpn
- + side1 * eps * fabs(tmpn));
- if ( side1 != side2 ) {
- // printf("failed side 3 check\n");
- return false;
+ void intersect(IntersectionVisitor& iv, Drawable* drawable)
+ {
+ if (carriers.empty())
+ return;
+ PolytopeIntersector::intersect(iv, drawable);
}
- return true;
-}
+ void reset()
+ {
+ carriers.clear();
+ }
+ std::vector<int> carriers;
+ int depth;
+};
-// Test if the line given by the point on the line pt_on_line and the
-// line direction dir intersects the sphere sp.
-// Adapted from plib.
+/// Ok, variant that uses a infinite line istead of the ray.
+/// also not that this only works if the ray direction is normalized.
static inline bool
-fgdIsectSphereInfLine(const sgdSphere& sp,
- const sgdVec3 pt_on_line, const sgdVec3 dir)
+intersectsInf(const SGRayd& ray, const SGSphered& sphere)
{
- sgdVec3 r;
- sgdSubVec3( r, sp.getCenter(), pt_on_line ) ;
-
- SGDfloat projectedDistance = sgdScalarProductVec3(r, dir);
-
- SGDfloat dist = sgdScalarProductVec3 ( r, r ) -
- projectedDistance * projectedDistance;
-
- SGDfloat radius = sp.getRadius();
- return dist < radius*radius;
+ SGVec3d r = sphere.getCenter() - ray.getOrigin();
+ double projectedDistance = dot(r, ray.getDirection());
+ double dist = dot(r, r) - projectedDistance * projectedDistance;
+ return dist < sphere.getRadius2();
}
-FGGroundCache::FGGroundCache()
+FGGroundCache::FGGroundCache() :
+ ground_radius(0.0),
+ _type(0),
+ _material(0),
+ cache_ref_time(0.0),
+ wire_id(0),
+ reference_wgs84_point(SGVec3d(0, 0, 0)),
+ reference_vehicle_radius(0.0),
+ down(0.0, 0.0, 0.0),
+ found_ground(false)
{
- sgdSetVec3(cache_center, 0.0, 0.0, 0.0);
- ground_radius = 0.0;
- cache_ref_time = 0.0;
- wire_id = 0;
- sgdSetVec3(reference_wgs84_point, 0.0, 0.0, 0.0);
- reference_vehicle_radius = 0.0;
- found_ground = false;
}
FGGroundCache::~FGGroundCache()
{
}
-FGGroundCache::GroundProperty
-FGGroundCache::extractGroundProperty( ssgLeaf* l )
+inline void
+FGGroundCache::velocityTransformTriangle(double dt,
+ SGTriangled& dst, SGSphered& sdst,
+ const FGGroundCache::Triangle& src)
{
- // FIXME: Do more ...
- // Idea: have a get_globals() function which knows about that stuff.
- // Or most probably read that from a configuration file,
- // from property tree or whatever ...
-
- // Get ground dependent data.
- GroundProperty gp;
- gp.wire_id = -1;
-
- FGAICarrierHardware *ud =
- dynamic_cast<FGAICarrierHardware*>(l->getUserData());
- if (ud) {
- switch (ud->type) {
- case FGAICarrierHardware::Wire:
- gp.type = FGInterface::Wire;
- gp.wire_id = ud->id;
- break;
- case FGAICarrierHardware::Catapult:
- gp.type = FGInterface::Catapult;
- break;
- default:
- gp.type = FGInterface::Solid;
- break;
- }
-
- // Copy the velocity from the carrier class.
- ud->carrier->getVelocityWrtEarth( gp.vel, gp.rot, gp.pivot );
- }
-
- else {
- // Initialize velocity field.
- sgdSetVec3( gp.vel, 0.0, 0.0, 0.0 );
- sgdSetVec3( gp.rot, 0.0, 0.0, 0.0 );
- sgdSetVec3( gp.pivot, 0.0, 0.0, 0.0 );
-
- // get some material information for use in the gear model
- gp.material = globals->get_matlib()->findMaterial(l);
- if (gp.material)
- gp.type = gp.material->get_solid() ? FGInterface::Solid : FGInterface::Water;
- }
-
- return gp;
+ dst = src.triangle;
+ sdst = src.sphere;
+
+ if (dt*dt*dot(src.gp.vel, src.gp.vel) < SGLimitsd::epsilon())
+ return;
+
+ SGVec3d baseVert = dst.getBaseVertex();
+ SGVec3d pivotoff = baseVert - src.gp.pivot;
+ baseVert += dt*(src.gp.vel + cross(src.gp.rot, pivotoff));
+ dst.setBaseVertex(baseVert);
+ dst.setEdge(0, dst.getEdge(0) + dt*cross(src.gp.rot, dst.getEdge(0)));
+ dst.setEdge(1, dst.getEdge(1) + dt*cross(src.gp.rot, dst.getEdge(1)));
}
-void
-FGGroundCache::putLineLeafIntoCache(const sgdSphere *wsp, const sgdMat4 xform,
- ssgLeaf *l)
+void FGGroundCache::getGroundProperty(Drawable* drawable,
+ const NodePath& nodePath,
+ FGGroundCache::GroundProperty& gp,
+ bool& backfaceCulling)
{
- GroundProperty gp = extractGroundProperty(l);
-
- // Lines must have special meanings.
- // Wires and catapults are done with lines.
- int nl = l->getNumLines();
- for (int i = 0; i < nl; ++i) {
- sgdSphere sphere;
- sphere.empty();
- sgdVec3 ends[2];
- short v[2];
- l->getLine(i, v, v+1 );
- for (int k=0; k<2; ++k) {
- sgdSetVec3(ends[k], l->getVertex(v[k]));
- sgdXformPnt3(ends[k], xform);
- sphere.extend(ends[k]);
- }
-
- if (wsp->intersects( &sphere )) {
- if (gp.type == FGInterface::Wire) {
- Wire wire;
- sgdCopyVec3(wire.ends[0], ends[0]);
- sgdCopyVec3(wire.ends[1], ends[1]);
- sgdCopyVec3(wire.velocity, gp.vel);
- sgdCopyVec3(wire.rotation, gp.rot);
- sgdSubVec3(wire.rotation_pivot, gp.pivot, cache_center);
- wire.wire_id = gp.wire_id;
-
- wires.push_back(wire);
- }
- if (gp.type == FGInterface::Catapult) {
- Catapult cat;
- sgdCopyVec3(cat.start, ends[0]);
- sgdCopyVec3(cat.end, ends[1]);
- sgdCopyVec3(cat.velocity, gp.vel);
- sgdCopyVec3(cat.rotation, gp.rot);
- sgdSubVec3(cat.rotation_pivot, gp.pivot, cache_center);
-
- catapults.push_back(cat);
- }
+ gp.type = FGInterface::Unknown;
+ gp.wire_id = 0;
+ gp.vel = SGVec3d(0.0, 0.0, 0.0);
+ gp.rot = SGVec3d(0.0, 0.0, 0.0);
+ gp.pivot = SGVec3d(0.0, 0.0, 0.0);
+ gp.material = 0;
+ backfaceCulling = false;
+ // XXX state set might be higher up in scene graph
+ gp.material = globals->get_matlib()->findMaterial(drawable->getStateSet());
+ if (gp.material)
+ gp.type = (gp.material->get_solid() ? FGInterface::Solid
+ : FGInterface::Water);
+ for (NodePath::const_iterator iter = nodePath.begin(), e = nodePath.end();
+ iter != e;
+ ++iter) {
+ Node* node = *iter;
+ StateSet* stateSet = node->getStateSet();
+ StateAttribute* stateAttribute = 0;
+ if (stateSet && (stateAttribute
+ = stateSet->getAttribute(StateAttribute::CULLFACE))) {
+ backfaceCulling
+ = (static_cast<osg::CullFace*>(stateAttribute)->getMode()
+ == CullFace::BACK);
+ }
+
+ // get some material information for use in the gear model
+ Referenced* base = node->getUserData();
+ if (!base)
+ continue;
+ FGAICarrierHardware *ud = dynamic_cast<FGAICarrierHardware*>(base);
+ if (!ud)
+ continue;
+ switch (ud->type) {
+ case FGAICarrierHardware::Wire:
+ gp.type = FGInterface::Wire;
+ gp.wire_id = ud->id;
+ break;
+ case FGAICarrierHardware::Catapult:
+ gp.type = FGInterface::Catapult;
+ break;
+ default:
+ gp.type = FGInterface::Solid;
+ break;
+ }
+ // Copy the velocity from the carrier class.
+ ud->carrier->getVelocityWrtEarth(gp.vel, gp.rot, gp.pivot);
+ break;
}
- }
}
-void
-FGGroundCache::putSurfaceLeafIntoCache(const sgdSphere *sp,
- const sgdMat4 xform, bool sphIsec,
- sgdVec3 down, ssgLeaf *l)
+void FGGroundCache::getTriIntersectorResults(PolytopeIntersector* triInt)
{
- GroundProperty gp = extractGroundProperty(l);
-
- int nt = l->getNumTriangles();
- for (int i = 0; i < nt; ++i) {
- Triangle t;
- t.sphere.empty();
- t.material = gp.material;
- short v[3];
- l->getTriangle(i, &v[0], &v[1], &v[2]);
- for (int k = 0; k < 3; ++k) {
- sgdSetVec3(t.vertices[k], l->getVertex(v[k]));
- sgdXformPnt3(t.vertices[k], xform);
- t.sphere.extend(t.vertices[k]);
- }
-
- sgdMakePlane(t.plane, t.vertices[0], t.vertices[1], t.vertices[2]);
- SGDfloat dot = sgdScalarProductVec3(down, t.plane);
- if (dot > 0) {
- if (!l->getCullFace()) {
- // Surface points downwards, ignore for altitude computations.
- continue;
- } else
- sgdScaleVec4( t.plane, -1 );
- }
-
- // Check if the sphere around the vehicle intersects the sphere
- // around that triangle. If so, put that triangle into the cache.
- if (sphIsec && sp->intersects(&t.sphere)) {
- sgdCopyVec3(t.velocity, gp.vel);
- sgdCopyVec3(t.rotation, gp.rot);
- sgdSubVec3(t.rotation_pivot, gp.pivot, cache_center);
- t.type = gp.type;
- triangles.push_back(t);
- }
+ const PolytopeIntersector::Intersections& intersections
+ = triInt->getIntersections();
+ Drawable* lastDrawable = 0;
+ RefMatrix* lastMatrix = 0;
+ Matrix worldToLocal;
+ GroundProperty gp;
+ bool backfaceCulling = false;
+ for (PolytopeIntersector::Intersections::const_iterator
+ itr = intersections.begin(), e = intersections.end();
+ itr != e;
+ ++itr) {
+ const PolytopeIntersector::Intersection& intr = *itr;
+ if (intr.drawable.get() != lastDrawable) {
+ getGroundProperty(intr.drawable.get(), intr.nodePath, gp,
+ backfaceCulling);
+ lastDrawable = intr.drawable.get();
+ }
+ Primitive triPrim = getPrimitive(intr.drawable, intr.primitiveIndex);
+ if (triPrim.numVerts != 3)
+ continue;
+ SGVec3d v[3] = { SGVec3d(triPrim.vertices[0]),
+ SGVec3d(triPrim.vertices[1]),
+ SGVec3d(triPrim.vertices[2])
+ };
+ RefMatrix* mat = intr.matrix.get();
+ // If the drawable is the same then the intersection model
+ // matrix should be the same, because it is only set by nodes
+ // in the scene graph. However, do an extra test in case
+ // something funny is going on with the drawable.
+ if (mat != lastMatrix) {
+ lastMatrix = mat;
+ worldToLocal = Matrix::inverse(*mat);
+ }
+ SGVec3d localCacheReference;
+ localCacheReference.osg() = reference_wgs84_point.osg() * worldToLocal;
+ SGVec3d localDown;
+ localDown.osg() = Matrixd::transform3x3(down.osg(), worldToLocal);
+ // a bounding sphere in the node local system
+ SGVec3d boundCenter = (1.0/3)*(v[0] + v[1] + v[2]);
+ double boundRadius = std::max(distSqr(v[0], boundCenter),
+ distSqr(v[1], boundCenter));
+ boundRadius = std::max(boundRadius, distSqr(v[2], boundCenter));
+ boundRadius = sqrt(boundRadius);
+ SGRayd ray(localCacheReference, localDown);
+ SGTriangled triangle(v);
+ // The normal and plane in the node local coordinate system
+ SGVec3d n = cross(triangle.getEdge(0), triangle.getEdge(1));
+ if (0 < dot(localDown, n)) {
+ if (backfaceCulling) {
+ // Surface points downwards, ignore for altitude computations.
+ continue;
+ } else {
+ triangle.flip();
+ }
+ }
- // In case the cache is empty, we still provide agl computations.
- // But then we use the old way of having a fixed elevation value for
- // the whole lifetime of this cache.
- if ( fgdIsectSphereInfLine(t.sphere, sp->getCenter(), down) ) {
- sgdVec3 tmp;
- sgdSetVec3(tmp, sp->center[0], sp->center[1], sp->center[2]);
- sgdVec3 isectpoint;
- if ( sgdIsectInfLinePlane( isectpoint, tmp, down, t.plane ) &&
- fgdPointInTriangle( isectpoint, t.vertices ) ) {
- // Compute the offset to the ground cache midpoint
- sgdVec3 off;
- sgdSubVec3(off, isectpoint, tmp);
- // Only accept the altitude if the intersection point is below the
- // ground cache midpoint
- if (0 < sgdScalarProductVec3( off, down )) {
- found_ground = true;
- sgdAddVec3(isectpoint, cache_center);
- double this_radius = sgdLengthVec3(isectpoint);
- if (ground_radius < this_radius)
- ground_radius = this_radius;
+ // Only check if the triangle is in the cache sphere if the plane
+ // containing the triangle is near enough
+ double d = dot(n, v[0] - localCacheReference);
+ if (d*d < reference_vehicle_radius*dot(n, n)) {
+ // Check if the sphere around the vehicle intersects the sphere
+ // around that triangle. If so, put that triangle into the cache.
+ double r2 = boundRadius + reference_vehicle_radius;
+ if (distSqr(boundCenter, localCacheReference) < r2*r2) {
+ FGGroundCache::Triangle t;
+ t.triangle.setBaseVertex(SGVec3d(v[0].osg() * *mat));
+ t.triangle.setEdge(0, SGVec3d(Matrixd::
+ transform3x3(triangle
+ .getEdge(0).osg(),
+ *mat)));
+ t.triangle.setEdge(1, SGVec3d(Matrixd::
+ transform3x3(triangle
+ .getEdge(1).osg(),
+ *mat)));
+ t.sphere.setCenter(SGVec3d(boundCenter.osg()* *mat));
+ t.sphere.setRadius(boundRadius);
+ t.gp = gp;
+ triangles.push_back(t);
+ }
}
- }
+ // In case the cache is empty, we still provide agl computations.
+ // But then we use the old way of having a fixed elevation value for
+ // the whole lifetime of this cache.
+ SGVec3d isectpoint;
+ if (intersects(isectpoint, triangle, ray, 1e-4)) {
+ found_ground = true;
+ isectpoint.osg() = isectpoint.osg() * *mat;
+ double this_radius = length(isectpoint);
+ if (ground_radius < this_radius) {
+ ground_radius = this_radius;
+ _type = gp.type;
+ _material = gp.material;
+ }
+ }
}
- }
}
-inline void
-FGGroundCache::velocityTransformTriangle(double dt,
- FGGroundCache::Triangle& dst,
- const FGGroundCache::Triangle& src)
+void FGGroundCache::getWireIntersectorResults(WireIntersector* wireInt,
+ double wireCacheRadius)
{
- sgdCopyVec3(dst.vertices[0], src.vertices[0]);
- sgdCopyVec3(dst.vertices[1], src.vertices[1]);
- sgdCopyVec3(dst.vertices[2], src.vertices[2]);
-
- sgdCopyVec4(dst.plane, src.plane);
-
- sgdCopyVec3(dst.sphere.center, src.sphere.center);
- dst.sphere.radius = src.sphere.radius;
-
- sgdCopyVec3(dst.velocity, src.velocity);
- sgdCopyVec3(dst.rotation, src.rotation);
- sgdCopyVec3(dst.rotation_pivot, src.rotation_pivot);
-
- dst.type = src.type;
- dst.material = src.material;
-
- if (dt*sgdLengthSquaredVec3(src.velocity) != 0) {
- sgdVec3 pivotoff, vel;
- for (int i = 0; i < 3; ++i) {
- sgdSubVec3(pivotoff, src.vertices[i], src.rotation_pivot);
- sgdVectorProductVec3(vel, src.rotation, pivotoff);
- sgdAddVec3(vel, src.velocity);
- sgdAddScaledVec3(dst.vertices[i], vel, dt);
- }
-
- // Transform the plane equation
- sgdSubVec3(pivotoff, dst.plane, src.rotation_pivot);
- sgdVectorProductVec3(vel, src.rotation, pivotoff);
- sgdAddVec3(vel, src.velocity);
- dst.plane[3] += dt*sgdScalarProductVec3(dst.plane, vel);
-
- sgdAddScaledVec3(dst.sphere.center, src.velocity, dt);
- }
-}
-
-void
-FGGroundCache::cache_fill(ssgBranch *branch, sgdMat4 xform,
- sgdSphere* sp, sgdVec3 down, sgdSphere* wsp)
-{
- // Travel through all kids.
- ssgEntity *e;
- for ( e = branch->getKid(0); e != NULL ; e = branch->getNextKid() ) {
- if ( !(e->getTraversalMask() & SSGTRAV_HOT) )
- continue;
- if ( e->getBSphere()->isEmpty() )
- continue;
-
- // We need to check further if either the sphere around the branch
- // intersects the sphere around the aircraft or the line downwards from
- // the aircraft intersects the branchs sphere.
- sgdSphere esphere;
- sgdSetVec3(esphere.center, e->getBSphere()->center);
- esphere.radius = e->getBSphere()->radius;
- esphere.orthoXform(xform);
- bool wspIsec = wsp->intersects(&esphere);
- bool downIsec = fgdIsectSphereInfLine(esphere, sp->getCenter(), down);
- if (!wspIsec && !downIsec)
- continue;
-
- // For branches collect up the transforms to reach that branch and
- // call cache_fill recursively.
- if ( e->isAKindOf( ssgTypeBranch() ) ) {
- ssgBranch *b = (ssgBranch *)e;
- if ( b->isAKindOf( ssgTypeTransform() ) ) {
- // Collect up the transforms required to reach that part of
- // the branch.
- sgMat4 xform2;
- sgMakeIdentMat4( xform2 );
- ssgTransform *t = (ssgTransform*)b;
- t->getTransform( xform2 );
- sgdMat4 xform3;
- fgMultMat4(xform3, xform, xform2);
- cache_fill( b, xform3, sp, down, wsp );
- } else
- cache_fill( b, xform, sp, down, wsp );
- }
-
- // For leafs, check each triangle for intersection.
- // This will minimize the number of vertices/triangles in the cache.
- else if (e->isAKindOf(ssgTypeLeaf())) {
- // Since we reach that leaf if we have an intersection with the
- // most probably bigger wire/catapult cache sphere, we need to check
- // that here, if the smaller cache for the surface has a chance for hits.
- // Also, if the spheres do not intersect compute a coarse agl value
- // by following the line downwards originating at the aircraft.
- bool spIsec = sp->intersects(&esphere);
- putSurfaceLeafIntoCache(sp, xform, spIsec, down, (ssgLeaf *)e);
-
- // If we are here, we need to put all special hardware here into
- // the cache.
- if (wspIsec)
- putLineLeafIntoCache(wsp, xform, (ssgLeaf *)e);
+ const WireIntersector::Intersections& intersections
+ = wireInt->getIntersections();
+ Drawable* lastDrawable = 0;
+ GroundProperty gp;
+ bool backfaceCulling = false;
+ for (PolytopeIntersector::Intersections::const_iterator
+ itr = intersections.begin(), e = intersections.end();
+ itr != e;
+ ++itr) {
+ if (itr->drawable.get() != lastDrawable) {
+ getGroundProperty(itr->drawable.get(), itr->nodePath, gp,
+ backfaceCulling);
+ lastDrawable = itr->drawable.get();
+ }
+ Primitive linePrim = getPrimitive(itr->drawable, itr->primitiveIndex);
+ if (linePrim.numVerts != 2)
+ continue;
+ RefMatrix* mat = itr->matrix.get();
+ SGVec3d gv1(osg::Vec3d(linePrim.vertices[0]) * *mat);
+ SGVec3d gv2(osg::Vec3d(linePrim.vertices[1]) * *mat);
+
+ SGVec3d boundCenter = 0.5*(gv1 + gv2);
+ double boundRadius = length(gv1 - boundCenter);
+
+ if (distSqr(boundCenter, reference_wgs84_point)
+ < (boundRadius + wireCacheRadius)*(boundRadius + wireCacheRadius)) {
+ if (gp.type == FGInterface::Wire) {
+ FGGroundCache::Wire wire;
+ wire.ends[0] = gv1;
+ wire.ends[1] = gv2;
+ wire.gp = gp;
+ wires.push_back(wire);
+ } else if (gp.type == FGInterface::Catapult) {
+ FGGroundCache::Catapult cat;
+ // Trick to get the ends in the right order.
+ // Use the x axis in the original coordinate system. Choose the
+ // most negative x-axis as the one pointing forward
+ if (linePrim.vertices[1][0] > linePrim.vertices[2][0]) {
+ cat.start = gv1;
+ cat.end = gv2;
+ } else {
+ cat.start = gv2;
+ cat.end = gv1;
+ }
+ cat.gp = gp;
+ catapults.push_back(cat);
+ }
+ }
+
}
- }
}
bool
-FGGroundCache::prepare_ground_cache(double ref_time, const double pt[3],
+FGGroundCache::prepare_ground_cache(double ref_time, const SGVec3d& pt,
double rad)
{
// Empty cache.
- ground_radius = 0.0;
found_ground = false;
+ SGGeod geodPt = SGGeod::fromCart(pt);
+ // Don't blow away the cache ground_radius and stuff if there's no
+ // scenery
+ if (!globals->get_tile_mgr()->scenery_available(geodPt.getLatitudeDeg(),
+ geodPt.getLongitudeDeg(),
+ rad))
+ return false;
+ ground_radius = 0.0;
triangles.resize(0);
catapults.resize(0);
wires.resize(0);
// Store the parameters we used to build up that cache.
- sgdCopyVec3(reference_wgs84_point, pt);
+ reference_wgs84_point = pt;
reference_vehicle_radius = rad;
// Store the time reference used to compute movements of moving triangles.
cache_ref_time = ref_time;
- // Decide where we put the scenery center.
- SGVec3d old_cntr = globals->get_scenery()->get_center();
- SGVec3d cntr(pt[0], pt[1], pt[2]);
- // Only move the cache center if it is unacceptable far away.
- if (40*40 < distSqr(old_cntr, cntr))
- globals->get_scenery()->set_center(cntr);
- else
- cntr = old_cntr;
-
- // The center of the cache.
- sgdSetVec3(cache_center, cntr[0], cntr[1], cntr[2]);
-
- sgdVec3 ptoff;
- sgdSubVec3(ptoff, pt, cache_center);
+ // Get a normalized down vector valid for the whole cache
+ SGQuatd hlToEc = SGQuatd::fromLonLat(geodPt);
+ down = hlToEc.rotate(SGVec3d(0, 0, 1));
+
// Prepare sphere around the aircraft.
- sgdSphere acSphere;
- acSphere.setRadius(rad);
- acSphere.setCenter(ptoff);
+ double cacheRadius = rad;
// Prepare bigger sphere around the aircraft.
// This one is required for reliably finding wires we have caught but
// have already left the hopefully smaller sphere for the ground reactions.
const double max_wire_dist = 300.0;
- sgdSphere wireSphere;
- wireSphere.setRadius(max_wire_dist < rad ? rad : max_wire_dist);
- wireSphere.setCenter(ptoff);
-
- // Down vector. Is used for croase agl computations when we are far enough
- // from ground that we have an empty cache.
- sgdVec3 down;
- sgdSetVec3(down, -pt[0], -pt[1], -pt[2]);
- sgdNormalizeVec3(down);
-
- // We collapse all transforms we need to reach a particular leaf.
- // The leafs itself will be then transformed later.
- // So our cache is just flat.
- // For leafs which are moving (carriers surface, etc ...)
- // we will later store a speed in the GroundType class. We can then apply
- // some translations to that nodes according to the time which has passed
- // compared to that snapshot.
- sgdMat4 xform;
- sgdMakeIdentMat4( xform );
-
-
- // Walk the scene graph and extract solid ground triangles and carrier data.
- ssgBranch *terrain = globals->get_scenery()->get_scene_graph();
- cache_fill(terrain, xform, &acSphere, down, &wireSphere);
-
+ double wireCacheRadius = max_wire_dist < rad ? rad : max_wire_dist;
+
+ Polytope triPolytope;
+ makePolytopeShaft(triPolytope, pt.osg(), down.osg(), cacheRadius);
+ ref_ptr<PolytopeIntersector> triIntersector
+ = new PolytopeIntersector(triPolytope);
+ triIntersector->setDimensionMask(PolytopeIntersector::DimTwo);
+ Polytope wirePolytope;
+ makePolytopeBox(wirePolytope, pt.osg(), down.osg(), wireCacheRadius);
+ ref_ptr<WireIntersector> wireIntersector = new WireIntersector(wirePolytope);
+ wireIntersector->setDimensionMask(PolytopeIntersector::DimOne);
+ ref_ptr<IntersectorGroup> intersectors = new IntersectorGroup;
+ intersectors->addIntersector(triIntersector.get());
+ intersectors->addIntersector(wireIntersector.get());
+
+ // Walk the scene graph and extract solid ground triangles and
+ // carrier data.
+ IntersectionVisitor iv(intersectors);
+ iv.setTraversalMask(SG_NODEMASK_TERRAIN_BIT);
+ globals->get_scenery()->get_scene_graph()->accept(iv);
+ getTriIntersectorResults(triIntersector.get());
+ getWireIntersectorResults(wireIntersector.get(), wireCacheRadius);
+
// some stats
SG_LOG(SG_FLIGHT,SG_DEBUG, "prepare_ground_cache(): ac radius = " << rad
<< ", # triangles = " << triangles.size()
SG_LOG(SG_FLIGHT, SG_WARN, "prepare_ground_cache(): trying to build cache "
"without any scenery below the aircraft" );
- if (cntr != old_cntr)
- globals->get_scenery()->set_center(old_cntr);
-
return found_ground;
}
bool
-FGGroundCache::is_valid(double *ref_time, double pt[3], double *rad)
+FGGroundCache::is_valid(double& ref_time, SGVec3d& pt, double& rad)
{
- sgdCopyVec3(pt, reference_wgs84_point);
- *rad = reference_vehicle_radius;
- *ref_time = cache_ref_time;
+ pt = reference_wgs84_point;
+ rad = reference_vehicle_radius;
+ ref_time = cache_ref_time;
return found_ground;
}
double
-FGGroundCache::get_cat(double t, const double dpt[3],
- double end[2][3], double vel[2][3])
+FGGroundCache::get_cat(double t, const SGVec3d& dpt,
+ SGVec3d end[2], SGVec3d vel[2])
{
// start with a distance of 1e10 meters...
double dist = 1e10;
size_t sz = catapults.size();
for (size_t i = 0; i < sz; ++i) {
- sgdVec3 pivotoff, rvel[2];
- sgdLineSegment3 ls;
- sgdCopyVec3(ls.a, catapults[i].start);
- sgdCopyVec3(ls.b, catapults[i].end);
-
- sgdSubVec3(pivotoff, ls.a, catapults[i].rotation_pivot);
- sgdVectorProductVec3(rvel[0], catapults[i].rotation, pivotoff);
- sgdAddVec3(rvel[0], catapults[i].velocity);
- sgdSubVec3(pivotoff, ls.b, catapults[i].rotation_pivot);
- sgdVectorProductVec3(rvel[1], catapults[i].rotation, pivotoff);
- sgdAddVec3(rvel[1], catapults[i].velocity);
-
- sgdAddVec3(ls.a, cache_center);
- sgdAddVec3(ls.b, cache_center);
-
- sgdAddScaledVec3(ls.a, rvel[0], t);
- sgdAddScaledVec3(ls.b, rvel[1], t);
-
- double this_dist = sgdDistSquaredToLineSegmentVec3( ls, dpt );
+ SGVec3d pivotoff, rvel[2];
+ pivotoff = catapults[i].start - catapults[i].gp.pivot;
+ rvel[0] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
+ pivotoff = catapults[i].end - catapults[i].gp.pivot;
+ rvel[1] = catapults[i].gp.vel + cross(catapults[i].gp.rot, pivotoff);
+
+ SGVec3d thisEnd[2];
+ thisEnd[0] = catapults[i].start + t*rvel[0];
+ thisEnd[1] = catapults[i].end + t*rvel[1];
+
+ double this_dist = distSqr(SGLineSegmentd(thisEnd[0], thisEnd[1]), dpt);
if (this_dist < dist) {
SG_LOG(SG_FLIGHT,SG_INFO, "Found catapult "
<< this_dist << " meters away");
dist = this_dist;
- // The carrier code takes care of that ordering.
- sgdCopyVec3( end[0], ls.a );
- sgdCopyVec3( end[1], ls.b );
- sgdCopyVec3( vel[0], rvel[0] );
- sgdCopyVec3( vel[1], rvel[1] );
+ end[0] = thisEnd[0];
+ end[1] = thisEnd[1];
+ vel[0] = rvel[0];
+ vel[1] = rvel[1];
}
}
}
bool
-FGGroundCache::get_agl(double t, const double dpt[3], double max_altoff,
- double contact[3], double normal[3], double vel[3],
+FGGroundCache::get_agl(double t, const SGVec3d& dpt, double max_altoff,
+ SGVec3d& contact, SGVec3d& normal, SGVec3d& vel,
int *type, const SGMaterial** material, double *agl)
{
bool ret = false;
// *agl = 0.0;
if (material)
*material = 0;
- sgdSetVec3( vel, 0.0, 0.0, 0.0 );
- sgdSetVec3( contact, 0.0, 0.0, 0.0 );
- sgdSetVec3( normal, 0.0, 0.0, 0.0 );
+ vel = SGVec3d(0, 0, 0);
+ contact = SGVec3d(0, 0, 0);
+ normal = SGVec3d(0, 0, 0);
// Time difference to th reference time.
t -= cache_ref_time;
// The double valued point we start to search for intersection.
- sgdVec3 pt;
- sgdSubVec3( pt, dpt, cache_center );
-
- // The search direction
- sgdVec3 dir;
- sgdSetVec3( dir, -dpt[0], -dpt[1], -dpt[2] );
- sgdNormaliseVec3( dir );
+ SGVec3d pt = dpt;
+ // shift the start of our ray by maxaltoff upwards
+ SGRayd ray(pt - max_altoff*down, down);
// Initialize to something sensible
double current_radius = 0.0;
size_t sz = triangles.size();
for (size_t i = 0; i < sz; ++i) {
- Triangle triangle;
- velocityTransformTriangle(t, triangle, triangles[i]);
- if (!fgdIsectSphereInfLine(triangle.sphere, pt, dir))
+ SGSphered sphere;
+ SGTriangled triangle;
+ velocityTransformTriangle(t, triangle, sphere, triangles[i]);
+ if (!intersectsInf(ray, sphere))
continue;
// Check for intersection.
- sgdVec3 isecpoint;
- if ( sgdIsectInfLinePlane( isecpoint, pt, dir, triangle.plane ) &&
- sgdPointInTriangle( isecpoint, triangle.vertices ) ) {
+ SGVec3d isecpoint;
+ if (intersects(isecpoint, triangle, ray, 1e-4)) {
// Compute the vector from pt to the intersection point ...
- sgdVec3 off;
- sgdSubVec3(off, isecpoint, pt);
+ SGVec3d off = isecpoint - pt;
// ... and check if it is too high or not
- if (-max_altoff < sgdScalarProductVec3( off, dir )) {
- // Transform to the wgs system
- sgdAddVec3( isecpoint, cache_center );
- // compute the radius, good enough approximation to take the geocentric radius
- SGDfloat radius = sgdLengthSquaredVec3(isecpoint);
- if (current_radius < radius) {
- current_radius = radius;
- ret = true;
- // Save the new potential intersection point.
- sgdCopyVec3( contact, isecpoint );
- // The first three values in the vector are the plane normal.
- sgdCopyVec3( normal, triangle.plane );
- // The velocity wrt earth.
- sgdVec3 pivotoff;
- sgdSubVec3(pivotoff, pt, triangle.rotation_pivot);
- sgdVectorProductVec3(vel, triangle.rotation, pivotoff);
- sgdAddVec3(vel, triangle.velocity);
- // Save the ground type.
- *type = triangle.type;
- sgdVec3 dstToContact;
- sgdSubVec3(dstToContact, contact, dpt);
- *agl = sgdScalarProductVec3(dir, dstToContact);
- if (material)
- *material = triangle.material;
- }
+
+ // compute the radius, good enough approximation to take the geocentric radius
+ double radius = dot(isecpoint, isecpoint);
+ if (current_radius < radius) {
+ current_radius = radius;
+ ret = true;
+ // Save the new potential intersection point.
+ contact = isecpoint;
+ // The first three values in the vector are the plane normal.
+ normal = triangle.getNormal();
+ // The velocity wrt earth.
+ SGVec3d pivotoff = pt - triangles[i].gp.pivot;
+ vel = triangles[i].gp.vel + cross(triangles[i].gp.rot, pivotoff);
+ // Save the ground type.
+ *type = triangles[i].gp.type;
+ *agl = dot(down, contact - dpt);
+ if (material)
+ *material = triangles[i].gp.material;
}
}
}
// Whenever we did not have a ground triangle for the requested point,
// take the ground level we found during the current cache build.
// This is as good as what we had before for agl.
- double r = sgdLengthVec3( dpt );
- sgdCopyVec3( contact, dpt );
- sgdScaleVec3( contact, ground_radius/r );
- sgdCopyVec3( normal, dpt );
- sgdNormaliseVec3( normal );
- sgdSetVec3( vel, 0.0, 0.0, 0.0 );
+ double r = length(dpt);
+ contact = dpt;
+ contact *= ground_radius/r;
+ normal = -down;
+ vel = SGVec3d(0, 0, 0);
// The altitude is the distance of the requested point from the
// contact point.
- sgdVec3 dstToContact;
- sgdSubVec3(dstToContact, contact, dpt);
- *agl = sgdScalarProductVec3(dir, dstToContact);
- *type = FGInterface::Unknown;
+ *agl = dot(down, contact - dpt);
+ *type = _type;
+ if (material)
+ *material = _material;
return ret;
}
-bool FGGroundCache::caught_wire(double t, const double pt[4][3])
+bool FGGroundCache::caught_wire(double t, const SGVec3d pt[4])
{
size_t sz = wires.size();
if (sz == 0)
// Build the two triangles spanning the area where the hook has moved
// during the past step.
- sgdVec4 plane[2];
- sgdVec3 tri[2][3];
- sgdMakePlane( plane[0], pt[0], pt[1], pt[2] );
- sgdCopyVec3( tri[0][0], pt[0] );
- sgdCopyVec3( tri[0][1], pt[1] );
- sgdCopyVec3( tri[0][2], pt[2] );
- sgdMakePlane( plane[1], pt[0], pt[2], pt[3] );
- sgdCopyVec3( tri[1][0], pt[0] );
- sgdCopyVec3( tri[1][1], pt[2] );
- sgdCopyVec3( tri[1][2], pt[3] );
+ SGTriangled triangle[2];
+ triangle[0].set(pt[0], pt[1], pt[2]);
+ triangle[1].set(pt[0], pt[2], pt[3]);
// Intersect the wire lines with each of these triangles.
// You have caught a wire if they intersect.
for (size_t i = 0; i < sz; ++i) {
- sgdVec3 le[2];
+ SGVec3d le[2];
for (int k = 0; k < 2; ++k) {
- sgdVec3 pivotoff, vel;
- sgdCopyVec3(le[k], wires[i].ends[k]);
- sgdSubVec3(pivotoff, le[k], wires[i].rotation_pivot);
- sgdVectorProductVec3(vel, wires[i].rotation, pivotoff);
- sgdAddVec3(vel, wires[i].velocity);
- sgdAddScaledVec3(le[k], vel, t);
- sgdAddVec3(le[k], cache_center);
+ le[k] = wires[i].ends[k];
+ SGVec3d pivotoff = le[k] - wires[i].gp.pivot;
+ SGVec3d vel = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
+ le[k] += t*vel;
}
+ SGLineSegmentd lineSegment(le[0], le[1]);
for (int k=0; k<2; ++k) {
- sgdVec3 isecpoint;
- double isecval = sgdIsectLinesegPlane(isecpoint, le[0], le[1], plane[k]);
- if ( 0.0 <= isecval && isecval <= 1.0 &&
- sgdPointInTriangle( isecpoint, tri[k] ) ) {
+ if (intersects(triangle[k], lineSegment)) {
SG_LOG(SG_FLIGHT,SG_INFO, "Caught wire");
// Store the wire id.
- wire_id = wires[i].wire_id;
+ wire_id = wires[i].gp.wire_id;
return true;
}
}
return false;
}
-bool FGGroundCache::get_wire_ends(double t, double end[2][3], double vel[2][3])
+bool FGGroundCache::get_wire_ends(double t, SGVec3d end[2], SGVec3d vel[2])
{
// Fast return if we do not have an active wire.
if (wire_id < 0)
// Search for the wire with the matching wire id.
size_t sz = wires.size();
for (size_t i = 0; i < sz; ++i) {
- if (wires[i].wire_id == wire_id) {
+ if (wires[i].gp.wire_id == wire_id) {
for (size_t k = 0; k < 2; ++k) {
- sgdVec3 pivotoff;
- sgdCopyVec3(end[k], wires[i].ends[k]);
- sgdSubVec3(pivotoff, end[k], wires[i].rotation_pivot);
- sgdVectorProductVec3(vel[k], wires[i].rotation, pivotoff);
- sgdAddVec3(vel[k], wires[i].velocity);
- sgdAddScaledVec3(end[k], vel[k], t);
- sgdAddVec3(end[k], cache_center);
+ SGVec3d pivotoff = wires[i].ends[k] - wires[i].gp.pivot;
+ vel[k] = wires[i].gp.vel + cross(wires[i].gp.rot, pivotoff);
+ end[k] = wires[i].ends[k] + t*vel[k];
}
return true;
}
projects / flightgear.git / blobdiff