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 <OgreViewport.h>
00031 #include <OgreQuaternion.h>
00032 #include <OgreVector3.h>
00033 #include <OgreSceneNode.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreCamera.h>
00036
00037 #include "rviz/uniform_string_stream.h"
00038 #include "rviz/display_context.h"
00039 #include "rviz/viewport_mouse_event.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
00045 #include "fps_view_controller.h"
00046
00047 namespace rviz
00048 {
00049
00050 static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION =
00051 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) *
00052 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z );
00053
00054 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001;
00055 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00056
00057 FPSViewController::FPSViewController()
00058 {
00059 yaw_property_ = new FloatProperty( "Yaw", 0, "Rotation of the camera around the Z (up) axis.", this );
00060
00061 pitch_property_ = new FloatProperty( "Pitch", 0, "How much the camera is tipped downward.", this );
00062 pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
00063 pitch_property_->setMin( -pitch_property_->getMax() );
00064
00065 position_property_ = new VectorProperty( "Position", Ogre::Vector3( 5, 5, 10 ), "Position of the camera.", this );
00066 }
00067
00068 FPSViewController::~FPSViewController()
00069 {
00070 }
00071
00072 void FPSViewController::onInitialize()
00073 {
00074 FramePositionTrackingViewController::onInitialize();
00075 camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00076 }
00077
00078 void FPSViewController::reset()
00079 {
00080 camera_->setPosition( Ogre::Vector3( 5, 5, 10 ));
00081 camera_->lookAt( 0, 0, 0 );
00082 setPropertiesFromCamera( camera_ );
00083
00084
00085
00086
00087
00088 updateCamera();
00089 camera_->lookAt( 0, 0, 0 );
00090 setPropertiesFromCamera( camera_ );
00091 }
00092
00093 void FPSViewController::handleMouseEvent(ViewportMouseEvent& event)
00094 {
00095 if ( event.shift() )
00096 {
00097 setStatus( "<b>Left-Click:</b> Move X/Y. <b>Right-Click:</b>: Move Z." );
00098 }
00099 else
00100 {
00101 setStatus( "<b>Left-Click:</b> Rotate. <b>Middle-Click:</b> Move X/Y. <b>Right-Click:</b>: Zoom. <b>Shift</b>: More options." );
00102 }
00103
00104 bool moved = false;
00105
00106 int32_t diff_x = 0;
00107 int32_t diff_y = 0;
00108
00109 if( event.type == QEvent::MouseMove )
00110 {
00111 diff_x = event.x - event.last_x;
00112 diff_y = event.y - event.last_y;
00113 moved = true;
00114 }
00115
00116 if( event.left() && !event.shift() )
00117 {
00118 setCursor( Rotate3D );
00119 yaw( -diff_x*0.005 );
00120 pitch( diff_y*0.005 );
00121 }
00122 else if( event.middle() || ( event.shift() && event.left() ))
00123 {
00124 setCursor( MoveXY );
00125 move( diff_x*0.01, -diff_y*0.01, 0.0f );
00126 }
00127 else if( event.right() )
00128 {
00129 setCursor( MoveZ );
00130 move( 0.0f, 0.0f, diff_y*0.1 );
00131 }
00132 else
00133 {
00134 setCursor( event.shift() ? MoveXY : Rotate3D );
00135 }
00136
00137 if ( event.wheel_delta != 0 )
00138 {
00139 int diff = event.wheel_delta;
00140 move( 0.0f, 0.0f, -diff * 0.01 );
00141
00142 moved = true;
00143 }
00144
00145 if (moved)
00146 {
00147 context_->queueRender();
00148 }
00149 }
00150
00151 void FPSViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
00152 {
00153 Ogre::Quaternion quat = source_camera->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
00154 float yaw = quat.getRoll( false ).valueRadians();
00155 float pitch = quat.getYaw( false ).valueRadians();
00156
00157 Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;
00158
00159 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00160 {
00161 if ( pitch > Ogre::Math::HALF_PI )
00162 {
00163 pitch -= Ogre::Math::PI;
00164 }
00165 else if ( pitch < -Ogre::Math::HALF_PI )
00166 {
00167 pitch += Ogre::Math::PI;
00168 }
00169
00170 yaw = -yaw;
00171
00172 if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
00173 {
00174 yaw -= Ogre::Math::PI;
00175 }
00176 else
00177 {
00178 yaw += Ogre::Math::PI;
00179 }
00180 }
00181
00182 pitch_property_->setFloat( pitch );
00183 yaw_property_->setFloat( mapAngleTo0_2Pi( yaw ));
00184 position_property_->setVector( source_camera->getPosition() );
00185 }
00186
00187 void FPSViewController::mimic( ViewController* source_view )
00188 {
00189 FramePositionTrackingViewController::mimic( source_view );
00190 setPropertiesFromCamera( source_view->getCamera() );
00191 }
00192
00193 void FPSViewController::update(float dt, float ros_dt)
00194 {
00195 FramePositionTrackingViewController::update( dt, ros_dt );
00196 updateCamera();
00197 }
00198
00199 void FPSViewController::lookAt( const Ogre::Vector3& point )
00200 {
00201 camera_->lookAt( point );
00202 setPropertiesFromCamera( camera_ );
00203 }
00204
00205 void FPSViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00206 {
00207 position_property_->add( old_reference_position - reference_position_ );
00208 }
00209
00210 void FPSViewController::updateCamera()
00211 {
00212 camera_->setOrientation( getOrientation() );
00213 camera_->setPosition( position_property_->getVector() );
00214 }
00215
00216 void FPSViewController::yaw( float angle )
00217 {
00218 yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() + angle ));
00219 }
00220
00221 void FPSViewController::pitch( float angle )
00222 {
00223 pitch_property_->add( angle );
00224 }
00225
00226 Ogre::Quaternion FPSViewController::getOrientation()
00227 {
00228 Ogre::Quaternion pitch, yaw;
00229
00230 yaw.FromAngleAxis( Ogre::Radian( yaw_property_->getFloat() ), Ogre::Vector3::UNIT_Z );
00231 pitch.FromAngleAxis( Ogre::Radian( pitch_property_->getFloat() ), Ogre::Vector3::UNIT_Y );
00232
00233 return yaw * pitch * ROBOT_TO_CAMERA_ROTATION;
00234 }
00235
00236 void FPSViewController::move( float x, float y, float z )
00237 {
00238 Ogre::Vector3 translate( x, y, z );
00239 position_property_->add( getOrientation() * translate );
00240 }
00241
00242 }
00243
00244 #include <pluginlib/class_list_macros.h>
00245 PLUGINLIB_EXPORT_CLASS( rviz::FPSViewController, rviz::ViewController )