Changes needed for envire

src/PickHandler.cpp

@@ -17,8 +17,7 @@ using namespace vizkit3d;
 
 PickHandler::PickHandler():
     _mx(0.0),_my(0.0),
-    _usePolytopeIntersector(false),
-    _useWindowCoordinates(false) {}
+    _usePolytopeIntersector(true) {}
 
 PickHandler::~PickHandler() {}
 
@@ -67,94 +66,84 @@ bool PickHandler::handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapt
     switch(ea.getEventType())
     {
         case(osgGA::GUIEventAdapter::KEYDOWN):
+        {
+            if (ea.getKey()=='w')
             {
-                if (ea.getKey()=='w')
-		{
-                    this->wireFrameModeOn(viewer->getSceneData());
-		}
-                else if (ea.getKey()=='n')
-		{
-                    this->wireFrameModeOff(viewer->getSceneData());
-		}
-                else
-                {
-                    keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey());
-                    if (itr != keyFuncMap.end())
-                    {
-                        if ( (*itr).second.keyState == KEY_UP )
-                        {
-                            (*itr).second.keyState = KEY_DOWN;
-                            newKeyDownEvent = true;
-                        }
-                        if (newKeyDownEvent)
-                        {
-                            (*itr).second.keyFunction();
-                            newKeyDownEvent = false;
-                        }
-                        return true;
-                    }
-                }
-                return false;
+                this->wireFrameModeOn(viewer->getSceneData());
             }
-	case(osgGA::GUIEventAdapter::KEYUP):
-	    {
-		if (ea.getKey()=='p')
-		{
-		    _usePolytopeIntersector = !_usePolytopeIntersector;
-		    if (_usePolytopeIntersector)
-		    {
-			osg::notify(osg::NOTICE)<<"Using PolytopeIntersector"<<std::endl;
-		    } else {
-			osg::notify(osg::NOTICE)<<"Using LineSegmentIntersector"<<std::endl;
-		    }
-		}
-		else if (ea.getKey()=='c')
-		{
-		    _useWindowCoordinates = !_useWindowCoordinates;
-		    if (_useWindowCoordinates)
-		    {
-			osg::notify(osg::NOTICE)<<"Using window coordinates for picking"<<std::endl;
-		    } else {
-			osg::notify(osg::NOTICE)<<"Using projection coordiates for picking"<<std::endl;
-		    }
-		}
-                else
+            else if (ea.getKey()=='n')
+            {
+                this->wireFrameModeOff(viewer->getSceneData());
+            }
+            else
+            {
+                keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey());
+                if (itr != keyFuncMap.end())
                 {
-                    keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey());
-                    if (itr != keyFuncMap.end() )
+                    if ( (*itr).second.keyState == KEY_UP )
                     {
-                        (*itr).second.keyState = KEY_UP;
+                        (*itr).second.keyState = KEY_DOWN;
+                        newKeyDownEvent = true;
                     }
-
-                    itr = keyUPFuncMap.find(ea.getKey());
-                    if (itr != keyUPFuncMap.end())
+                    if (newKeyDownEvent)
                     {
                         (*itr).second.keyFunction();
-                        return true;
+                        newKeyDownEvent = false;
                     }
+                    return true;
+                }
+            }
+            return false;
+        }
+        case(osgGA::GUIEventAdapter::KEYUP):
+        {
+            if (ea.getKey()=='p')
+            {
+                _usePolytopeIntersector = !_usePolytopeIntersector;
+                if (_usePolytopeIntersector)
+                {
+                osg::notify(osg::NOTICE)<<"Using PolytopeIntersector"<<std::endl;
+                } else {
+                osg::notify(osg::NOTICE)<<"Using LineSegmentIntersector"<<std::endl;
+                }
+            }
+            else
+            {
+                keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey());
+                if (itr != keyFuncMap.end() )
+                {
+                    (*itr).second.keyState = KEY_UP;
                 }
 
-		return false;
-	    }
-	case(osgGA::GUIEventAdapter::PUSH):
-	case(osgGA::GUIEventAdapter::MOVE):
-	    {
-		_mx = ea.getX();
-		_my = ea.getY();
-		return false;
-	    }
-	case(osgGA::GUIEventAdapter::RELEASE):
-	    {
-		if (_mx == ea.getX() && _my == ea.getY())
-		{
-		    // only do a pick if the mouse hasn't moved
-		    pick(ea,viewer);
-		}
-		return false;
+                itr = keyUPFuncMap.find(ea.getKey());
+                if (itr != keyUPFuncMap.end())
+                {
+                    (*itr).second.keyFunction();
+                    return true;
+                }
+            }
+
+            return false;
+        }
+        case(osgGA::GUIEventAdapter::PUSH):
+        case(osgGA::GUIEventAdapter::MOVE):
+        {
+            _mx = ea.getX();
+            _my = ea.getY();
+            return false;
+        }
+        case(osgGA::GUIEventAdapter::RELEASE):
+        {
+            if (_mx == ea.getX() && _my == ea.getY())
+            {
+                // only do a pick if the mouse hasn't moved
+                pick(ea,viewer);
             }
+            return false;
+        }
 
-	default:
-	    return false;
+        default:
+            return false;
     }
 }
 
@@ -163,119 +152,85 @@ void PickHandler::pick(const osgGA::GUIEventAdapter& ea, osgViewer::View* viewer
     osg::Node* scene = viewer->getSceneData();
     if (!scene) return;
 
-    osg::notify(osg::NOTICE)<<std::endl;
-
-    osg::Node* node = 0;
-
     if (_usePolytopeIntersector)
     {
-	osgUtil::PolytopeIntersector* picker;
-	if (_useWindowCoordinates)
-	{
-	    // use window coordinates
-	    // remap the mouse x,y into viewport coordinates.
-	    osg::Viewport* viewport = viewer->getCamera()->getViewport();
-	    double mx = viewport->x() + (int)((double )viewport->width()*(ea.getXnormalized()*0.5+0.5));
-	    double my = viewport->y() + (int)((double )viewport->height()*(ea.getYnormalized()*0.5+0.5));
-
-	    // half width, height.
-	    double w = 5.0f;
-	    double h = 5.0f;
-	    picker = new osgUtil::PolytopeIntersector( osgUtil::Intersector::WINDOW, mx-w, my-h, mx+w, my+h );
-	} else {
-	    double mx = ea.getXnormalized();
-	    double my = ea.getYnormalized();
-	    double w = 0.05;
-	    double h = 0.05;
-	    picker = new osgUtil::PolytopeIntersector( osgUtil::Intersector::PROJECTION, mx-w, my-h, mx+w, my+h );
-	}
-	osgUtil::IntersectionVisitor iv(picker);
-
-	viewer->getCamera()->accept(iv);
-
-	if (picker->containsIntersections())
-	{
-	    for(std::multiset<osgUtil::PolytopeIntersector::Intersection>::iterator it = picker->getIntersections().begin();
-		    it != picker->getIntersections().end(); it++)
-	    {
-		const osgUtil::PolytopeIntersector::Intersection &intersection = *it;
-
-		/*
-		osg::notify(osg::NOTICE)<<"Picked "<<intersection.localIntersectionPoint<<std::endl
-		    <<"  Distance to ref. plane "<<intersection.distance
-		    <<", max. dist "<<intersection.maxDistance
-		    <<", primitive index "<<intersection.primitiveIndex
-		    <<", numIntersectionPoints "
-		    <<intersection.numIntersectionPoints
-		    <<std::endl;
-		    */
-		const osg::NodePath& nodePath = intersection.nodePath;
-		node = (nodePath.size()>=1)?nodePath[nodePath.size()-1]:0;
-
-		//osg::Matrixd l2w = osg::computeLocalToWorld( nodePath );
-		//osg::Vec3 global = *intersection.matrix.get() * intersection.localIntersectionPoint;
-		osg::Vec3 global = intersection.localIntersectionPoint * *intersection.matrix.get();
-
-		QVector3D globalPoint( global.x(), global.y(), global.z());
-		emit picked(globalPoint);
-	    }
-	}
+        //projection space is [-1 ... 1], thus we can directly use the normalized
+        //mouse coordinates
+        double mx = ea.getXnormalized();
+        double my = ea.getYnormalized();
+        double w = 0.01;
+        double h = 0.01;
+        osg::ref_ptr<osgUtil::PolytopeIntersector> picker;
+        picker = new osgUtil::PolytopeIntersector( osgUtil::Intersector::PROJECTION, mx-w, my-h, mx+w, my+h );
+        picker->setIntersectionLimit(osgUtil::Intersector::LIMIT_ONE_PER_DRAWABLE);// a single drawable will appear at most once while calculating intersections.
+
+        osgUtil::IntersectionVisitor iv(picker);
+        viewer->getCamera()->accept(iv);
+
+        if (picker->containsIntersections())
+        {
+            const osgUtil::PolytopeIntersector::Intersection intersection = picker->getFirstIntersection();
+            const osg::NodePath& nodePath = intersection.nodePath;
+            osg::Vec3 global = intersection.localIntersectionPoint * *intersection.matrix.get();
+            pickNodePath(nodePath, global, viewer);
+        }
     }
     else
     {
-	osgUtil::LineSegmentIntersector* picker;
-	if (!_useWindowCoordinates)
-	{
-	    // use non dimensional coordinates - in projection/clip space
-	    picker = new osgUtil::LineSegmentIntersector( osgUtil::Intersector::PROJECTION, ea.getXnormalized(),ea.getYnormalized() );
-	} else {
-	    // use window coordinates
-	    // remap the mouse x,y into viewport coordinates.
-	    osg::Viewport* viewport = viewer->getCamera()->getViewport();
-	    float mx = viewport->x() + (int)((float)viewport->width()*(ea.getXnormalized()*0.5f+0.5f));
-	    float my = viewport->y() + (int)((float)viewport->height()*(ea.getYnormalized()*0.5f+0.5f));
-	    picker = new osgUtil::LineSegmentIntersector( osgUtil::Intersector::WINDOW, mx, my );
-	}
-	osgUtil::IntersectionVisitor iv(picker);
-
-	viewer->getCamera()->accept(iv);
-
-	if (picker->containsIntersections())
-	{
-	    osgUtil::LineSegmentIntersector::Intersection intersection = picker->getFirstIntersection();
-
-	    osg::NodePath& nodePath = intersection.nodePath;
-	    node = (nodePath.size()>=1)?nodePath[nodePath.size()-1]:0;
-
-	    // see if the object has a user object which is derived from pickcallback
-	    PickedCallback *pc = dynamic_cast<PickedCallback*>(node->getUserData());
-	    if( pc )
-		pc->picked();
-
-	    for(int i = nodePath.size()-1; i >= 0; i--)
-	    {
-	        osg::Node *node = nodePath[i];
-	        osg::Referenced *user_data = node->getUserData();
-	        if (!user_data)
-	            continue;
-	        PickedUserData *plugin_data = dynamic_cast<PickedUserData*>(user_data);
-	        if(!plugin_data)
-	            continue;
-
-                // Transform OSG viewport coordinates to QWidget coordinates (invert y axis)
-                float wy = (float)viewer->getCamera()->getViewport()->height() - _my;
-                float wx = _mx;
-                plugin_data->getPlugin()->click(wx, wy);
-
-		osg::Vec3 global = intersection.localIntersectionPoint * *intersection.matrix.get();
-                emit plugin_data->getPlugin()->picked(global.x(),global.y(),global.z());
-                break;
-	    }
-            // setTrackedNode(viewer, node);
-	}
+        osg::ref_ptr<osgUtil::LineSegmentIntersector> picker;
+
+        // use non dimensional coordinates - in projection/clip space
+        picker = new osgUtil::LineSegmentIntersector( osgUtil::Intersector::PROJECTION, ea.getXnormalized(),ea.getYnormalized() );
+        // Using this setting, a single drawable will appear at most once while calculating intersections.
+        picker->setIntersectionLimit(osgUtil::Intersector::LIMIT_ONE_PER_DRAWABLE);
+        osgUtil::IntersectionVisitor iv(picker);
+        viewer->getCamera()->accept(iv);
+
+        if (picker->containsIntersections())
+        {
+            osgUtil::LineSegmentIntersector::Intersection intersection = picker->getFirstIntersection();
+            const osg::NodePath& nodePath = intersection.nodePath;
+            const osg::Vec3 global = intersection.localIntersectionPoint * *intersection.matrix.get();
+            pickNodePath(nodePath, global, viewer);
+        }
     }
 }
 
+void PickHandler::pickNodePath(const osg::NodePath& nodePath, osg::Vec3 global,
+                               osgViewer::View* viewer) const
+{
+    if(nodePath.empty())
+      return;
+
+    osg::Node* node = nodePath[nodePath.size()-1];
+    // see if the object has a user object which is derived from pickcallback
+    PickedCallback *pc = dynamic_cast<PickedCallback*>(node->getUserData());
+    if( pc )
+        pc->picked();
+
+    for(int i = nodePath.size()-1; i >= 0; i--)
+    {
+        osg::Node *node = nodePath[i];
+        osg::Referenced *user_data = node->getUserData();
+        if (!user_data)
+            continue;
+        PickedUserData *plugin_data = dynamic_cast<PickedUserData*>(user_data);
+        if(!plugin_data)
+            continue;
+
+        // Transform OSG viewport coordinates to QWidget coordinates (invert y axis)
+        float wy = (float)viewer->getCamera()->getViewport()->height() - _my;
+        float wx = _mx;
+        plugin_data->getPlugin()->click(wx, wy);
+        emit plugin_data->getPlugin()->picked(global.x(),global.y(),global.z());
+        break;
+    }  
+
+    const QVector3D globalPoint( global.x(), global.y(), global.z());
+    emit picked(globalPoint);
+    emit pickedNodePath(nodePath);
+}
+
 void PickHandler::setTrackedNode(osgViewer::View* viewer, osg::ref_ptr< osg::Node > node)
 {
     osgGA::KeySwitchMatrixManipulator *keyswitchManipulator = 

src/PickHandler.hpp

@@ -56,17 +56,24 @@ class PickHandler : public QObject, public osgGA::GUIEventHandler
         void pick(const osgGA::GUIEventAdapter& ea, osgViewer::View* viewer);
 
     signals:
-        void picked(const QVector3D& coord);
+        void picked(const QVector3D& coord) const;
+        /**Is emitted whenenver the user picks a node.
+         * @p path The path from the root node to the picked node*/
+        void pickedNodePath(const osg::NodePath& path) const;
 
     protected:
         void setTrackedNode(osgViewer::View* viewer, osg::ref_ptr< osg::Node > node);
+
+        /** @param nodePath NodePath to the picked node
+         *  @param global  global coordinates of the clicked point*/
+        void pickNodePath(const osg::NodePath& nodePath, osg::Vec3 global, 
+                          osgViewer::View* viewer) const;
 
         void wireFrameModeOn(osg::Node *srcNode);
         void wireFrameModeOff(osg::Node *srcNode);
 
         float _mx,_my;
         bool _usePolytopeIntersector;
-        bool _useWindowCoordinates;
 
         typedef std::map<int, functionStatusType > keyFunctionMap;
         keyFunctionMap keyFuncMap;