xy_orbit_view_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <stdint.h>
00031 
00032 #include <ros/ros.h>
00033 
00034 #include <OgreCamera.h>
00035 #include <OgrePlane.h>
00036 #include <OgreQuaternion.h>
00037 #include <OgreRay.h>
00038 #include <OgreSceneNode.h>
00039 #include <OgreVector3.h>
00040 #include <OgreViewport.h>
00041 
00042 #include "rviz/display_context.h"
00043 #include "rviz/ogre_helpers/shape.h"
00044 #include "rviz/properties/float_property.h"
00045 #include "rviz/properties/vector_property.h"
00046 #include "rviz/viewport_mouse_event.h"
00047 
00048 #include "rviz/default_plugin/view_controllers/xy_orbit_view_controller.h"
00049 
00050 namespace rviz
00051 {
00052 
00053 // move camera up so the focal point appears in the lower image half
00054 static const float CAMERA_OFFSET = 0.2;
00055 
00056 void XYOrbitViewController::onInitialize()
00057 {
00058   OrbitViewController::onInitialize();
00059   focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f);
00060 }
00061 
00062 bool XYOrbitViewController::intersectGroundPlane( Ogre::Ray mouse_ray, Ogre::Vector3 &intersection_3d )
00063 {
00064   //convert rays into reference frame
00065   mouse_ray.setOrigin( target_scene_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00066   mouse_ray.setDirection( target_scene_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00067 
00068   Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0 );
00069 
00070   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(ground_plane);
00071   if (!intersection.first)
00072   {
00073     return false;
00074   }
00075 
00076   intersection_3d = mouse_ray.getPoint(intersection.second);
00077   return true;
00078 }
00079 
00080 void XYOrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
00081 {
00082   if ( event.shift() )
00083   {
00084     setStatus( "<b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Zoom." );
00085   }
00086   else
00087   {
00088     setStatus( "<b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z.  <b>Shift</b>: More options." );
00089   }
00090 
00091   int32_t diff_x = 0;
00092   int32_t diff_y = 0;
00093 
00094   bool moved = false;
00095   if( event.type == QEvent::MouseButtonPress )
00096   {
00097     focal_shape_->getRootNode()->setVisible(true);
00098     moved = true;
00099   }
00100   else if( event.type == QEvent::MouseButtonRelease )
00101   {
00102     focal_shape_->getRootNode()->setVisible(false);
00103     moved = true;
00104   }
00105   else if( event.type == QEvent::MouseMove )
00106   {
00107     diff_x = event.x - event.last_x;
00108     diff_y = event.y - event.last_y;
00109     moved = true;
00110   }
00111 
00112   if( event.left() && !event.shift() )
00113   {
00114     setCursor( Rotate3D );
00115     yaw( diff_x*0.005 );
00116     pitch( -diff_y*0.005 );
00117   }
00118   else if( event.middle() || (event.left() && event.shift()) )
00119   {
00120     setCursor( MoveXY );
00121     // handle mouse movement
00122     int width = event.viewport->getActualWidth();
00123     int height = event.viewport->getActualHeight();
00124 
00125     Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( event.x / (float) width,
00126                                                                                event.y / (float) height );
00127 
00128     Ogre::Ray last_mouse_ray =
00129       event.viewport->getCamera()->getCameraToViewportRay( event.last_x / (float) width,
00130                                                            event.last_y / (float) height );
00131 
00132     Ogre::Vector3 last_intersect, intersect;
00133 
00134     if( intersectGroundPlane( last_mouse_ray, last_intersect ) &&
00135         intersectGroundPlane( mouse_ray, intersect ))
00136     {
00137       Ogre::Vector3 motion = last_intersect - intersect;
00138 
00139       // When dragging near the horizon, the motion can get out of
00140       // control.  This throttles it to an arbitrary limit per mouse
00141       // event.
00142       float motion_distance_limit = 1; /*meter*/
00143       if( motion.length() > motion_distance_limit )
00144       {
00145         motion.normalise();
00146         motion *= motion_distance_limit;
00147       }
00148 
00149       focal_point_property_->add( motion );
00150       emitConfigChanged();
00151     }
00152   }
00153   else if( event.right() )
00154   {
00155     setCursor( Zoom );
00156     zoom( -diff_y * 0.1 * (distance_property_->getFloat() / 10.0f) );
00157   }
00158   else
00159   {
00160     setCursor( event.shift() ? MoveXY : Rotate3D );
00161   }
00162 
00163   if( event.wheel_delta != 0 )
00164   {
00165     int diff = event.wheel_delta;
00166     zoom( diff * 0.001 * distance_property_->getFloat() );
00167     moved = true;
00168   }
00169 
00170   if( moved )
00171   {
00172     context_->queueRender();
00173   }
00174 }
00175 
00176 void XYOrbitViewController::mimic( ViewController* source_view )
00177 {
00178   FramePositionTrackingViewController::mimic( source_view );
00179 
00180   Ogre::Camera* source_camera = source_view->getCamera();
00181   // do some trigonometry
00182   Ogre::Ray camera_dir_ray( source_camera->getRealPosition(), source_camera->getRealDirection() );
00183   Ogre::Ray camera_down_ray( source_camera->getRealPosition(), -1.0 * source_camera->getRealUp() );
00184 
00185   Ogre::Vector3 a,b;
00186 
00187   if( intersectGroundPlane( camera_dir_ray, b ) &&
00188       intersectGroundPlane( camera_down_ray, a ) )
00189   {
00190     float l_a = source_camera->getPosition().distance( b );
00191     float l_b = source_camera->getPosition().distance( a );
00192 
00193     distance_property_->setFloat(( l_a * l_b ) / ( CAMERA_OFFSET * l_a + l_b ));
00194     float distance = distance_property_->getFloat();
00195 
00196     camera_dir_ray.setOrigin( source_camera->getRealPosition() - source_camera->getRealUp() * distance * CAMERA_OFFSET );
00197     Ogre::Vector3 new_focal_point;
00198     intersectGroundPlane( camera_dir_ray, new_focal_point );
00199     focal_point_property_->setVector( new_focal_point );
00200 
00201     calculatePitchYawFromPosition( source_camera->getPosition() - source_camera->getUp() * distance * CAMERA_OFFSET );
00202   }
00203 }
00204 
00205 void XYOrbitViewController::updateCamera()
00206 {
00207   OrbitViewController::updateCamera();
00208   camera_->setPosition( camera_->getPosition() + camera_->getUp() * distance_property_->getFloat() * CAMERA_OFFSET );
00209 }
00210 
00211 void XYOrbitViewController::lookAt( const Ogre::Vector3& point )
00212 {
00213   Ogre::Vector3 camera_position = camera_->getPosition();
00214   Ogre::Vector3 new_focal_point = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition());
00215   new_focal_point.z = 0;
00216   distance_property_->setFloat( new_focal_point.distance( camera_position ));
00217   focal_point_property_->setVector( new_focal_point );
00218 
00219   calculatePitchYawFromPosition(camera_position);
00220 }
00221 
00222 } // end namespace rviz
00223 
00224 #include <pluginlib/class_list_macros.h>
00225 PLUGINLIB_EXPORT_CLASS( rviz::XYOrbitViewController, rviz::ViewController )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:28