Implemented HeightAboveTerrain and added usage into osgintersection

src/osgSim/HeightAboveTerrain.cpp

@@ -11,6 +11,111 @@
  * OpenSceneGraph Public License for more details.
 */
 
+#include <osg/CoordinateSystemNode>
 #include <osgSim/HeightAboveTerrain>
 
+#include <osg/Notify>
+
 using namespace osgSim;
+
+HeightAboveTerrain::HeightAboveTerrain()
+{
+    _lowestHeight = -1000.0;
+    
+    setDatabaseCacheReadCallback(new DatabaseCacheReadCallback);
+}
+
+void HeightAboveTerrain::clear()
+{
+    _HATList.clear();
+}
+
+unsigned int HeightAboveTerrain::addPoint(const osg::Vec3d& point)
+{
+    unsigned int index = _HATList.size();
+    _HATList.push_back(HAT(point));
+    return index;
+}
+
+void HeightAboveTerrain::computeIntersections(osg::Node* scene)
+{
+    osg::CoordinateSystemNode* csn = dynamic_cast<osg::CoordinateSystemNode*>(scene);
+    osg::EllipsoidModel* em = csn ? csn->getEllipsoidModel() : 0;
+
+    osg::ref_ptr<osgUtil::IntersectorGroup> intersectorGroup = new osgUtil::IntersectorGroup();
+
+    for(HATList::iterator itr = _HATList.begin();
+        itr != _HATList.end();
+        ++itr)
+    {
+        if (em)
+        {
+        
+            osg::Vec3d start = itr->_point;
+            osg::Vec3d upVector = em->computeLocalUpVector(start.x(), start.y(), start.z());
+
+            double latitude, longitude, height;
+            em->convertXYZToLatLongHeight(start.x(), start.y(), start.z(), latitude, longitude, height);
+            osg::Vec3d end = start - upVector * (height - _lowestHeight);            
+            
+            itr->_hat = height;
+
+            osg::notify(osg::NOTICE)<<"lat = "<<latitude<<" longitude = "<<longitude<<" height = "<<height<<std::endl;
+
+            osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector = new osgUtil::LineSegmentIntersector(start, end);
+            intersectorGroup->addIntersector( intersector.get() );
+        }
+        else
+        {
+            osg::Vec3d start = itr->_point;
+            osg::Vec3d upVector (0.0, 0.0, 1.0);
+            
+            double height = start.z();
+            osg::Vec3d end = start - upVector * (height - _lowestHeight);            
+
+            itr->_hat = height;
+
+            osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector = new osgUtil::LineSegmentIntersector( start, end);
+            intersectorGroup->addIntersector( intersector.get() );
+        }
+    }
+    
+    _intersectionVisitor.reset();
+    _intersectionVisitor.setIntersector( intersectorGroup.get() );
+    
+    scene->accept(_intersectionVisitor);
+    
+    unsigned int index = 0;
+    osgUtil::IntersectorGroup::Intersectors& intersectors = intersectorGroup->getIntersectors();
+    for(osgUtil::IntersectorGroup::Intersectors::iterator intersector_itr = intersectors.begin();
+        intersector_itr != intersectors.end();
+        ++intersector_itr, ++index)
+    {
+        osgUtil::LineSegmentIntersector* lsi = dynamic_cast<osgUtil::LineSegmentIntersector*>(intersector_itr->get());
+        if (lsi)
+        {
+            osgUtil::LineSegmentIntersector::Intersections& intersections = lsi->getIntersections();
+            if (!intersections.empty())
+            {
+                const osgUtil::LineSegmentIntersector::Intersection& intersection = *intersections.begin();
+                osg::Vec3d intersectionPoint = intersection.localIntersectionPoint * (*intersection.matrix);
+                _HATList[index]._hat = (_HATList[index]._point - intersectionPoint).length();
+            }
+        }
+    }
+    
+}
+
+double HeightAboveTerrain::computeHeightAboveTerrain(osg::Node* scene, const osg::Vec3d& point)
+{
+    HeightAboveTerrain hat;
+    unsigned int index = hat.addPoint(point);
+    hat.computeIntersections(scene);
+    return hat.getHeightAboveTerrain(index);
+}
+
+void HeightAboveTerrain::setDatabaseCacheReadCallback(DatabaseCacheReadCallback* dcrc)
+{
+    _dcrc = dcrc;
+    _intersectionVisitor.setReadCallback(dcrc);
+}

src/osgSim/LineOfSight.cpp

@@ -92,7 +92,7 @@ LineOfSight::LineOfSight()
     setDatabaseCacheReadCallback(new DatabaseCacheReadCallback);
 }
 
-void LineOfSight::clearLOS()
+void LineOfSight::clear()
 {
     _LOSList.clear();
 }