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 #include <rviz/properties/bool_property.h>
00045
00046 #include <fps_motion_view_controller.h>
00047
00048 namespace rviz
00049 {
00050
00051 static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION =
00052 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) *
00053 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z );
00054
00055 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001;
00056 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00057
00058 FPSMotionViewController::FPSMotionViewController()
00059 {
00060 yaw_property_ = new FloatProperty( "Yaw", 0, "Rotation of the camera around the Z (up) axis.", this );
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
00069 FPSMotionViewController::~FPSMotionViewController()
00070 {
00071 }
00072
00073 void FPSMotionViewController::onInitialize()
00074 {
00075 FramePositionTrackingViewController::onInitialize();
00076 camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00077 }
00078
00079 void FPSMotionViewController::reset()
00080 {
00081 camera_->setPosition( Ogre::Vector3( 0.0, 0.0, 1.75 ));
00082 camera_->lookAt( 0.0, 0.0, 0.0);
00083 setPropertiesFromCamera( camera_ );
00084
00085 updateCamera();
00086 camera_->lookAt( -1.0, 0.0, 1.75);
00087 setPropertiesFromCamera( camera_ );
00088 }
00089
00090 void FPSMotionViewController::handleMouseEvent(ViewportMouseEvent& event)
00091 {
00092 bool moved = false;
00093
00094 int32_t diff_x = 0;
00095 int32_t diff_y = 0;
00096
00097 if( event.type == QEvent::MouseMove )
00098 {
00099 diff_x = event.x - event.last_x;
00100 diff_y = event.y - event.last_y;
00101 moved = true;
00102 }
00103
00104 if( event.left() )
00105 {
00106 setCursor( Rotate3D );
00107 yaw( -diff_x * 0.005 );
00108 pitch( diff_y * 0.005 );
00109 }
00110
00111 if (moved)
00112 {
00113 context_->queueRender();
00114 }
00115 }
00116
00117 void FPSMotionViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
00118 {
00119 Ogre::Quaternion quat = source_camera->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
00120 float yaw = quat.getRoll( false ).valueRadians();
00121 float pitch = quat.getYaw( false ).valueRadians();
00122
00123 Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;
00124
00125 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00126 {
00127 if ( pitch > Ogre::Math::HALF_PI )
00128 {
00129 pitch -= Ogre::Math::PI;
00130 }
00131 else if ( pitch < -Ogre::Math::HALF_PI )
00132 {
00133 pitch += Ogre::Math::PI;
00134 }
00135
00136 yaw = -yaw;
00137
00138 if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
00139 {
00140 yaw -= Ogre::Math::PI;
00141 }
00142 else
00143 {
00144 yaw += Ogre::Math::PI;
00145 }
00146 }
00147
00148 pitch_property_->setFloat( pitch );
00149 yaw_property_->setFloat( mapAngleTo0_2Pi( yaw ));
00150 position_property_->setVector( source_camera->getPosition() );
00151 }
00152
00153 void FPSMotionViewController::mimic( ViewController* source_view )
00154 {
00155 FramePositionTrackingViewController::mimic( source_view );
00156 setPropertiesFromCamera( source_view->getCamera() );
00157 }
00158
00159 void FPSMotionViewController::update(float dt, float ros_dt)
00160 {
00161 FramePositionTrackingViewController::update( dt, ros_dt );
00162 updateCamera();
00163 }
00164
00165 void FPSMotionViewController::lookAt( const Ogre::Vector3& point )
00166 {
00167 camera_->lookAt( point );
00168 setPropertiesFromCamera( camera_ );
00169 }
00170
00171 void FPSMotionViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00172 {
00173 position_property_->add( old_reference_position - reference_position_ );
00174 }
00175
00176 void FPSMotionViewController::updateCamera()
00177 {
00178 camera_->setOrientation( getOrientation() );
00179 camera_->setPosition( position_property_->getVector() );
00180 }
00181
00182 void FPSMotionViewController::updateCamera(Ogre::Vector3& position, Ogre::Quaternion& orientation )
00183 {
00184 camera_->setOrientation(orientation);
00185 camera_->setPosition(position);
00186 context_->queueRender();
00187 }
00188
00189 void FPSMotionViewController::yaw( float angle )
00190 {
00191 yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() + angle ));
00192 }
00193
00194 void FPSMotionViewController::pitch( float angle )
00195 {
00196 pitch_property_->add( angle );
00197 }
00198
00199 Ogre::Quaternion FPSMotionViewController::getOrientation()
00200 {
00201 Ogre::Quaternion pitch, yaw;
00202
00203 yaw.FromAngleAxis( Ogre::Radian( yaw_property_->getFloat() ), Ogre::Vector3::UNIT_Z );
00204 pitch.FromAngleAxis( Ogre::Radian( pitch_property_->getFloat() ), Ogre::Vector3::UNIT_Y );
00205
00206 return yaw * pitch * ROBOT_TO_CAMERA_ROTATION;
00207 }
00208
00209 void FPSMotionViewController::move( float x, float y, float z )
00210 {
00211 Ogre::Vector3 translate( x, y, z );
00212 Ogre::Vector3 update = getOrientation() * translate;
00213 update.z = 0.0;
00214 position_property_->add( update );
00215 }
00216
00217 void FPSMotionViewController::changeZ(float z)
00218 {
00219 Ogre::Vector3 translate( 0.0, 0.0, z );
00220 Ogre::Vector3 update = getOrientation() * translate;
00221 position_property_->add( translate );
00222 }
00223
00224 void FPSMotionViewController::fly( float x, float y, float z )
00225 {
00226 Ogre::Vector3 translate( x, y, z );
00227 Ogre::Vector3 update = getOrientation() * translate;
00228 position_property_->add( update );
00229 }
00230
00231 }
00232
00233 #include <pluginlib/class_list_macros.h>
00234 PLUGINLIB_EXPORT_CLASS(rviz::FPSMotionViewController, rviz::ViewController)