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 }