+ 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::getGroundProperty(Drawable* drawable,
+ const NodePath& nodePath,
+ FGGroundCache::GroundProperty& gp,
+ bool& backfaceCulling)
+{
+ 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::getTriIntersectorResults(PolytopeIntersector* triInt)
+{
+ 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();
+ }
+ }
+
+ // 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;
+ }
+ }
+ }