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 "fps_view_controller.h"
00031 #include "rviz/viewport_mouse_event.h"
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/uniform_string_stream.h"
00034
00035 #include <OGRE/OgreCamera.h>
00036 #include <OGRE/OgreSceneManager.h>
00037 #include <OGRE/OgreSceneNode.h>
00038 #include <OGRE/OgreVector3.h>
00039 #include <OGRE/OgreQuaternion.h>
00040 #include <OGRE/OgreViewport.h>
00041
00042 #include <ogre_helpers/shape.h>
00043
00044 #include <stdint.h>
00045
00046 namespace rviz
00047 {
00048
00049 static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION =
00050 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) *
00051 Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z );
00052
00053 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001;
00054 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00055
00056 FPSViewController::FPSViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node)
00057 : ViewController(manager, name, target_scene_node)
00058 , yaw_(0.0f)
00059 , pitch_(0.0f)
00060 {
00061 }
00062
00063 FPSViewController::~FPSViewController()
00064 {
00065 }
00066
00067 void FPSViewController::reset()
00068 {
00069 camera_->setPosition( 5, 5, 10 );
00070 camera_->lookAt( 0, 0, 0 );
00071 setYawPitchFromCamera();
00072
00073
00074
00075
00076
00077 updateCamera();
00078 camera_->lookAt( 0, 0, 0 );
00079 setYawPitchFromCamera();
00080 }
00081
00082 void FPSViewController::handleMouseEvent(ViewportMouseEvent& event)
00083 {
00084 bool moved = false;
00085 if( event.type == QEvent::MouseMove )
00086 {
00087 int32_t diff_x = event.x - event.last_x;
00088 int32_t diff_y = event.y - event.last_y;
00089
00090 if( diff_x != 0 || diff_y != 0 )
00091 {
00092 if( event.left() && !event.shift() )
00093 {
00094 yaw( -diff_x*0.005 );
00095 pitch( diff_y*0.005 );
00096 }
00097 else if( event.middle() || ( event.shift() && event.left() ))
00098 {
00099 move( diff_x*0.01, -diff_y*0.01, 0.0f );
00100 }
00101 else if( event.right() )
00102 {
00103 move( 0.0f, 0.0f, diff_y*0.1 );
00104 }
00105
00106 moved = true;
00107 }
00108 }
00109
00110 if ( event.wheel_delta != 0 )
00111 {
00112 int diff = event.wheel_delta;
00113 move( 0.0f, 0.0f, -diff * 0.01 );
00114
00115 moved = true;
00116 }
00117
00118 if (moved)
00119 {
00120 manager_->queueRender();
00121 }
00122 }
00123
00124 void FPSViewController::setYawPitchFromCamera()
00125 {
00126 Ogre::Quaternion quat = camera_->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
00127 yaw_ = quat.getRoll( false ).valueRadians();
00128 pitch_ = quat.getYaw( false ).valueRadians();
00129
00130 Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;
00131
00132 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00133 {
00134 if ( pitch_ > Ogre::Math::HALF_PI )
00135 {
00136 pitch_ -= Ogre::Math::PI;
00137 }
00138 else if ( pitch_ < -Ogre::Math::HALF_PI )
00139 {
00140 pitch_ += Ogre::Math::PI;
00141 }
00142
00143 yaw_ = -yaw_;
00144
00145 if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
00146 {
00147 yaw_ -= Ogre::Math::PI;
00148 }
00149 else
00150 {
00151 yaw_ += Ogre::Math::PI;
00152 }
00153 }
00154
00155 normalizePitch();
00156 normalizeYaw();
00157 emitConfigChanged();
00158 }
00159
00160 void FPSViewController::onActivate()
00161 {
00162 if (camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC)
00163 {
00164 camera_->setProjectionType(Ogre::PT_PERSPECTIVE);
00165 }
00166 else
00167 {
00168 setYawPitchFromCamera();
00169 }
00170 }
00171
00172 void FPSViewController::onDeactivate()
00173 {
00174 }
00175
00176 void FPSViewController::onUpdate(float dt, float ros_dt)
00177 {
00178 updateCamera();
00179 }
00180
00181 void FPSViewController::lookAt( const Ogre::Vector3& point )
00182 {
00183 camera_->lookAt( point );
00184 setYawPitchFromCamera();
00185 }
00186
00187 void FPSViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00188 {
00189 camera_->setPosition( camera_->getPosition() + old_reference_position - reference_position_ );
00190 }
00191
00192 void FPSViewController::normalizePitch()
00193 {
00194 if ( pitch_ < PITCH_LIMIT_LOW )
00195 {
00196 pitch_ = PITCH_LIMIT_LOW;
00197 }
00198 else if ( pitch_ > PITCH_LIMIT_HIGH )
00199 {
00200 pitch_ = PITCH_LIMIT_HIGH;
00201 }
00202 }
00203
00204 void FPSViewController::normalizeYaw()
00205 {
00206 yaw_ = fmod( yaw_, Ogre::Math::TWO_PI );
00207
00208 if ( yaw_ < 0.0f )
00209 {
00210 yaw_ = Ogre::Math::TWO_PI + yaw_;
00211 }
00212 }
00213
00214 void FPSViewController::updateCamera()
00215 {
00216 Ogre::Quaternion pitch, yaw;
00217
00218 yaw.FromAngleAxis( Ogre::Radian( yaw_ ), Ogre::Vector3::UNIT_Z );
00219 pitch.FromAngleAxis( Ogre::Radian( pitch_ ), Ogre::Vector3::UNIT_Y );
00220
00221 camera_->setOrientation( yaw * pitch * ROBOT_TO_CAMERA_ROTATION );
00222 }
00223
00224 void FPSViewController::yaw( float angle )
00225 {
00226 yaw_ += angle;
00227
00228 normalizeYaw();
00229 emitConfigChanged();
00230 }
00231
00232 void FPSViewController::pitch( float angle )
00233 {
00234 pitch_ += angle;
00235
00236 normalizePitch();
00237 emitConfigChanged();
00238 }
00239
00240 void FPSViewController::move( float x, float y, float z )
00241 {
00242 Ogre::Vector3 translate( x, y, z );
00243 camera_->setPosition( camera_->getPosition() + camera_->getOrientation() * translate );
00244 emitConfigChanged();
00245 }
00246
00247 void FPSViewController::fromString(const std::string& str)
00248 {
00249 UniformStringStream iss(str);
00250
00251 iss.parseFloat( pitch_ );
00252 iss.parseFloat( yaw_ );
00253
00254 Ogre::Vector3 vec;
00255 iss.parseFloat( vec.x );
00256 iss.parseFloat( vec.y );
00257 iss.parseFloat( vec.z );
00258 camera_->setPosition(vec);
00259 emitConfigChanged();
00260 }
00261
00262 std::string FPSViewController::toString()
00263 {
00264 UniformStringStream oss;
00265 oss << pitch_ << " " << yaw_ << " " << camera_->getPosition().x << " " << camera_->getPosition().y << " " << camera_->getPosition().z;
00266
00267 return oss.str();
00268 }
00269
00270
00271 }