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 <OgreCamera.h>
00033 #include <OgreQuaternion.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreSceneNode.h>
00036 #include <OgreVector3.h>
00037 #include <OgreViewport.h>
00038
00039 #include "rviz/display_context.h"
00040 #include "rviz/geometry.h"
00041 #include "rviz/ogre_helpers/shape.h"
00042 #include "rviz/properties/float_property.h"
00043 #include "rviz/properties/vector_property.h"
00044 #include "rviz/uniform_string_stream.h"
00045 #include "rviz/viewport_mouse_event.h"
00046 #include "rviz/load_resource.h"
00047 #include "rviz/render_panel.h"
00048
00049 #include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
00050
00051 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0;
00052 static const float YAW_START = Ogre::Math::HALF_PI * 0.5;
00053 static const float DISTANCE_START = 10;
00054
00055 namespace rviz
00056 {
00057
00058 OrbitViewController::OrbitViewController()
00059 : dragging_( false )
00060 {
00061 distance_property_ = new FloatProperty( "Distance", DISTANCE_START, "Distance from the focal point.", this );
00062 distance_property_->setMin( 0.01 );
00063
00064 yaw_property_ = new FloatProperty( "Yaw", YAW_START, "Rotation of the camera around the Z (up) axis.", this );
00065
00066 pitch_property_ = new FloatProperty( "Pitch", PITCH_START, "How much the camera is tipped downward.", this );
00067 pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
00068 pitch_property_->setMin( -pitch_property_->getMax() );
00069
00070 focal_point_property_ = new VectorProperty( "Focal Point", Ogre::Vector3::ZERO, "The center point which the camera orbits.", this );
00071 }
00072
00073 void OrbitViewController::onInitialize()
00074 {
00075 FramePositionTrackingViewController::onInitialize();
00076
00077 camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00078
00079 focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), target_scene_node_);
00080 focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
00081 focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
00082 focal_shape_->getRootNode()->setVisible(false);
00083 }
00084
00085 OrbitViewController::~OrbitViewController()
00086 {
00087 delete focal_shape_;
00088 }
00089
00090 void OrbitViewController::reset()
00091 {
00092 dragging_ = false;
00093 yaw_property_->setFloat( YAW_START );
00094 pitch_property_->setFloat( PITCH_START );
00095 distance_property_->setFloat( DISTANCE_START );
00096 focal_point_property_->setVector( Ogre::Vector3::ZERO );
00097 }
00098
00099 void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
00100 {
00101 if ( event.shift() )
00102 {
00103 setStatus( "<b>Left-Click:</b> Move X/Y. <b>Right-Click:</b>: Move Z. <b>Mouse Wheel:</b>: Zoom. " );
00104 }
00105 else
00106 {
00107 setStatus( "<b>Left-Click:</b> Rotate. <b>Middle-Click:</b> Move X/Y. <b>Right-Click/Mouse Wheel:</b>: Zoom. <b>Shift</b>: More options." );
00108 }
00109
00110 float distance = distance_property_->getFloat();
00111
00112 int32_t diff_x = 0;
00113 int32_t diff_y = 0;
00114
00115 bool moved = false;
00116
00117 if( event.type == QEvent::MouseButtonPress )
00118 {
00119 focal_shape_->getRootNode()->setVisible(true);
00120 moved = true;
00121 dragging_ = true;
00122 }
00123 else if( event.type == QEvent::MouseButtonRelease )
00124 {
00125 focal_shape_->getRootNode()->setVisible(false);
00126 moved = true;
00127 dragging_ = false;
00128 }
00129 else if( dragging_ && event.type == QEvent::MouseMove )
00130 {
00131 diff_x = event.x - event.last_x;
00132 diff_y = event.y - event.last_y;
00133 moved = true;
00134 }
00135
00136
00137 if( event.left() && !event.shift() )
00138 {
00139 setCursor( Rotate3D );
00140 yaw( diff_x*0.005 );
00141 pitch( -diff_y*0.005 );
00142 }
00143
00144 else if( event.middle() || (event.shift() && event.left()) )
00145 {
00146 setCursor( MoveXY );
00147 float fovY = camera_->getFOVy().valueRadians();
00148 float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
00149
00150 int width = camera_->getViewport()->getActualWidth();
00151 int height = camera_->getViewport()->getActualHeight();
00152
00153 move( -((float)diff_x / (float)width) * distance * tan( fovX / 2.0f ) * 2.0f,
00154 ((float)diff_y / (float)height) * distance * tan( fovY / 2.0f ) * 2.0f,
00155 0.0f );
00156 }
00157 else if( event.right() )
00158 {
00159 if( event.shift() )
00160 {
00161
00162 setCursor( MoveZ );
00163 move(0.0f, 0.0f, diff_y * 0.1 * (distance / 10.0f));
00164 }
00165 else
00166 {
00167
00168 setCursor( Zoom );
00169 zoom( -diff_y * 0.1 * (distance / 10.0f) );
00170 }
00171 }
00172 else
00173 {
00174 setCursor( event.shift() ? MoveXY : Rotate3D );
00175 }
00176
00177 moved = true;
00178
00179 if( event.wheel_delta != 0 )
00180 {
00181 int diff = event.wheel_delta;
00182 if( event.shift() )
00183 {
00184 move( 0, 0, -diff * 0.001 * distance );
00185 }
00186 else
00187 {
00188 zoom( diff * 0.001 * distance );
00189 }
00190
00191 moved = true;
00192 }
00193
00194 if( moved )
00195 {
00196 context_->queueRender();
00197 }
00198 }
00199
00200 void OrbitViewController::mimic( ViewController* source_view )
00201 {
00202 FramePositionTrackingViewController::mimic( source_view );
00203
00204 Ogre::Camera* source_camera = source_view->getCamera();
00205 Ogre::Vector3 position = source_camera->getPosition();
00206 Ogre::Quaternion orientation = source_camera->getOrientation();
00207
00208 if( source_view->getClassId() == "rviz/Orbit" )
00209 {
00210
00211 distance_property_->setFloat( source_view->subProp( "Distance" )->getValue().toFloat() );
00212 }
00213 else
00214 {
00215
00216
00217 distance_property_->setFloat( position.length() );
00218 }
00219
00220 Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat() );
00221 focal_point_property_->setVector( position + direction );
00222
00223 calculatePitchYawFromPosition( position );
00224 }
00225
00226 void OrbitViewController::update(float dt, float ros_dt)
00227 {
00228 FramePositionTrackingViewController::update( dt, ros_dt );
00229 updateCamera();
00230 }
00231
00232 void OrbitViewController::lookAt( const Ogre::Vector3& point )
00233 {
00234 Ogre::Vector3 camera_position = camera_->getPosition();
00235 focal_point_property_->setVector( target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()) );
00236 distance_property_->setFloat( focal_point_property_->getVector().distance( camera_position ));
00237
00238 calculatePitchYawFromPosition(camera_position);
00239 }
00240
00241 void OrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00242 {
00243 focal_point_property_->add( old_reference_position - reference_position_ );
00244 }
00245
00246 void OrbitViewController::updateCamera()
00247 {
00248 float distance = distance_property_->getFloat();
00249 float yaw = yaw_property_->getFloat();
00250 float pitch = pitch_property_->getFloat();
00251
00252 Ogre::Vector3 focal_point = focal_point_property_->getVector();
00253
00254 float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
00255 float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
00256 float z = distance * sin( pitch ) + focal_point.z;
00257
00258 Ogre::Vector3 pos( x, y, z );
00259
00260 camera_->setPosition(pos);
00261 camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
00262 camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));
00263
00264 focal_shape_->setPosition( focal_point );
00265 }
00266
00267 void OrbitViewController::yaw( float angle )
00268 {
00269 yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() - angle ));
00270 }
00271
00272 void OrbitViewController::pitch( float angle )
00273 {
00274 pitch_property_->add( -angle );
00275 }
00276
00277 void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position )
00278 {
00279 Ogre::Vector3 diff = position - focal_point_property_->getVector();
00280 pitch_property_->setFloat( asin( diff.z / distance_property_->getFloat() ));
00281 yaw_property_->setFloat( atan2( diff.y, diff.x ));
00282 }
00283
00284 void OrbitViewController::zoom( float amount )
00285 {
00286 distance_property_->add( -amount );
00287 }
00288
00289 void OrbitViewController::move( float x, float y, float z )
00290 {
00291 focal_point_property_->add( camera_->getOrientation() * Ogre::Vector3( x, y, z ));
00292 }
00293
00294 }
00295
00296 #include <pluginlib/class_list_macros.h>
00297 PLUGINLIB_EXPORT_CLASS( rviz::OrbitViewController, rviz::ViewController )