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 <OGRE/OgreCamera.h>
00031 #include <OGRE/OgreQuaternion.h>
00032 #include <OGRE/OgreSceneManager.h>
00033 #include <OGRE/OgreSceneNode.h>
00034 #include <OGRE/OgreVector3.h>
00035 #include <OGRE/OgreViewport.h>
00036
00037 #include "rviz/display_context.h"
00038 #include "rviz/ogre_helpers/orthographic.h"
00039 #include "rviz/ogre_helpers/shape.h"
00040 #include "rviz/properties/float_property.h"
00041 #include "rviz/viewport_mouse_event.h"
00042
00043 #include "rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h"
00044
00045 namespace rviz
00046 {
00047
00048 FixedOrientationOrthoViewController::FixedOrientationOrthoViewController()
00049 : dragging_( false )
00050 {
00051 scale_property_ = new FloatProperty( "Scale", 10, "How much to scale up the size of things in the scene.", this );
00052 angle_property_ = new FloatProperty( "Angle", 0, "Angle around the Z axis to rotate.", this );
00053 x_property_ = new FloatProperty( "X", 0, "X component of camera position.", this );
00054 y_property_ = new FloatProperty( "Y", 0, "Y component of camera position.", this );
00055 }
00056
00057 FixedOrientationOrthoViewController::~FixedOrientationOrthoViewController()
00058 {
00059 }
00060
00061 void FixedOrientationOrthoViewController::onInitialize()
00062 {
00063 FramePositionTrackingViewController::onInitialize();
00064
00065 camera_->setProjectionType( Ogre::PT_ORTHOGRAPHIC );
00066 camera_->setFixedYawAxis( false );
00067 }
00068
00069 void FixedOrientationOrthoViewController::reset()
00070 {
00071 scale_property_->setFloat( 10 );
00072 angle_property_->setFloat( 0 );
00073 x_property_->setFloat( 0 );
00074 y_property_->setFloat( 0 );
00075 }
00076
00077 void FixedOrientationOrthoViewController::handleMouseEvent(ViewportMouseEvent& event)
00078 {
00079 if ( event.shift() )
00080 {
00081 setStatus( "<b>Left-Click:</b> Move X/Y." );
00082 }
00083 else
00084 {
00085 setStatus( "<b>Left-Click:</b> Rotate. <b>Middle-Click:</b> Move X/Y. <b>Right-Click:</b>: Zoom. <b>Shift</b>: More options." );
00086 }
00087
00088 bool moved = false;
00089
00090 int32_t diff_x = 0;
00091 int32_t diff_y = 0;
00092
00093 if( event.type == QEvent::MouseButtonPress )
00094 {
00095 dragging_ = true;
00096 }
00097 else if( event.type == QEvent::MouseButtonRelease )
00098 {
00099 dragging_ = false;
00100 }
00101 else if( dragging_ && event.type == QEvent::MouseMove )
00102 {
00103 diff_x = event.x - event.last_x;
00104 diff_y = event.y - event.last_y;
00105 moved = true;
00106 }
00107
00108 if( event.left() && !event.shift() )
00109 {
00110 setCursor( Rotate2D );
00111 angle_property_->add( diff_x * 0.005 );
00112 orientCamera();
00113 }
00114 else if( event.middle() || ( event.shift() && event.left() ))
00115 {
00116 setCursor( MoveXY );
00117 float scale = scale_property_->getFloat();
00118 move( -diff_x / scale, diff_y / scale );
00119 }
00120 else if( event.right() )
00121 {
00122 setCursor( Zoom );
00123 scale_property_->multiply( 1.0 - diff_y * 0.01 );
00124 }
00125 else
00126 {
00127 setCursor( event.shift() ? MoveXY : Rotate2D );
00128 }
00129
00130 if ( event.wheel_delta != 0 )
00131 {
00132 int diff = event.wheel_delta;
00133 scale_property_->multiply( 1.0 - (-diff) * 0.001 );
00134
00135 moved = true;
00136 }
00137
00138 if (moved)
00139 {
00140 context_->queueRender();
00141 emitConfigChanged();
00142 }
00143 }
00144
00145 void FixedOrientationOrthoViewController::orientCamera()
00146 {
00147 camera_->setOrientation( Ogre::Quaternion( Ogre::Radian( angle_property_->getFloat() ), Ogre::Vector3::UNIT_Z ));
00148 }
00149
00150 void FixedOrientationOrthoViewController::mimic( ViewController* source_view )
00151 {
00152 FramePositionTrackingViewController::mimic( source_view );
00153
00154 if( FixedOrientationOrthoViewController* source_ortho = qobject_cast<FixedOrientationOrthoViewController*>( source_view ))
00155 {
00156 scale_property_->setFloat( source_ortho->scale_property_->getFloat() );
00157 angle_property_->setFloat( source_ortho->angle_property_->getFloat() );
00158 x_property_->setFloat( source_ortho->x_property_->getFloat() );
00159 y_property_->setFloat( source_ortho->y_property_->getFloat() );
00160 }
00161 else
00162 {
00163 Ogre::Camera* source_camera = source_view->getCamera();
00164 setPosition( source_camera->getPosition() );
00165 }
00166 }
00167
00168 void FixedOrientationOrthoViewController::update(float dt, float ros_dt)
00169 {
00170 FramePositionTrackingViewController::update( dt, ros_dt );
00171 updateCamera();
00172 }
00173
00174 void FixedOrientationOrthoViewController::lookAt( const Ogre::Vector3& point )
00175 {
00176 setPosition( point - target_scene_node_->getPosition() );
00177 }
00178
00179 void FixedOrientationOrthoViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00180 {
00181 move( old_reference_position.x - reference_position_.x,
00182 old_reference_position.y - reference_position_.y );
00183 }
00184
00185 void FixedOrientationOrthoViewController::updateCamera()
00186 {
00187 orientCamera();
00188
00189 float width = camera_->getViewport()->getActualWidth();
00190 float height = camera_->getViewport()->getActualHeight();
00191
00192 float scale = scale_property_->getFloat();
00193 Ogre::Matrix4 proj;
00194 buildScaledOrthoMatrix( proj, -width / scale / 2, width / scale / 2, -height / scale / 2, height / scale / 2,
00195 camera_->getNearClipDistance(), camera_->getFarClipDistance() );
00196 camera_->setCustomProjectionMatrix(true, proj);
00197
00198
00199
00200
00201 camera_->setPosition( x_property_->getFloat(), y_property_->getFloat(), 500 );
00202 }
00203
00204 void FixedOrientationOrthoViewController::setPosition( const Ogre::Vector3& pos_rel_target )
00205 {
00206 x_property_->setFloat( pos_rel_target.x );
00207 y_property_->setFloat( pos_rel_target.y );
00208 }
00209
00210 void FixedOrientationOrthoViewController::move( float dx, float dy )
00211 {
00212 float angle = angle_property_->getFloat();
00213 x_property_->add( dx*cos(angle)-dy*sin(angle) );
00214 y_property_->add( dx*sin(angle)+dy*cos(angle) );
00215 }
00216
00217 }
00218
00219 #include <pluginlib/class_list_macros.h>
00220 PLUGINLIB_EXPORT_CLASS( rviz::FixedOrientationOrthoViewController, rviz::ViewController )