Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
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
00140
00141
00142 float motion_distance_limit = 1;
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
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 }
00223
00224 #include <pluginlib/class_list_macros.h>
00225 PLUGINLIB_EXPORT_CLASS( rviz::XYOrbitViewController, rviz::ViewController )