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 "orbit_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 float MIN_DISTANCE = 0.01;
00050 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00051 static const float PITCH_LIMIT_LOW = -PITCH_LIMIT_HIGH;
00052 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0;
00053 static const float YAW_START = Ogre::Math::HALF_PI * 0.5;
00054
00055 OrbitViewController::OrbitViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node)
00056 : ViewController(manager, name, target_scene_node)
00057 {
00058 reset();
00059 focal_shape_ = new Shape(Shape::Sphere, manager_->getSceneManager(), target_scene_node_);
00060 focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
00061 focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
00062 focal_shape_->getRootNode()->setVisible(false);
00063 }
00064
00065 OrbitViewController::~OrbitViewController()
00066 {
00067 delete focal_shape_;
00068 }
00069
00070 void OrbitViewController::reset()
00071 {
00072 dragging_ = false;
00073 focal_point_ = Ogre::Vector3::ZERO;
00074 yaw_ = YAW_START;
00075 pitch_ = PITCH_START;
00076 distance_ = 10.0f;
00077 emitConfigChanged();
00078 }
00079
00080 void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
00081 {
00082 bool moved = false;
00083 if( event.type == QEvent::MouseButtonPress )
00084 {
00085 focal_shape_->getRootNode()->setVisible(true);
00086 moved = true;
00087 dragging_ = true;
00088 }
00089 else if( event.type == QEvent::MouseButtonRelease )
00090 {
00091 focal_shape_->getRootNode()->setVisible(false);
00092 moved = true;
00093 dragging_ = false;
00094 }
00095 else if( dragging_ && event.type == QEvent::MouseMove )
00096 {
00097 int32_t diff_x = event.x - event.last_x;
00098 int32_t diff_y = event.y - event.last_y;
00099
00100 if( diff_x != 0 || diff_y != 0 )
00101 {
00102
00103 if( event.left() && !event.shift() )
00104 {
00105 yaw( diff_x*0.005 );
00106 pitch( -diff_y*0.005 );
00107 }
00108
00109 else if( event.middle() || (event.shift() && event.left()) )
00110 {
00111 float fovY = camera_->getFOVy().valueRadians();
00112 float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
00113
00114 int width = camera_->getViewport()->getActualWidth();
00115 int height = camera_->getViewport()->getActualHeight();
00116
00117 move( -((float)diff_x / (float)width) * distance_ * tan( fovX / 2.0f ) * 2.0f,
00118 ((float)diff_y / (float)height) * distance_ * tan( fovY / 2.0f ) * 2.0f,
00119 0.0f );
00120 }
00121 else if( event.right() )
00122 {
00123 if( event.shift() )
00124 {
00125 move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f));
00126 }
00127 else
00128 {
00129 zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
00130 }
00131 }
00132
00133 moved = true;
00134 }
00135 }
00136
00137 if( event.wheel_delta != 0 )
00138 {
00139 int diff = event.wheel_delta;
00140 if( event.shift() )
00141 {
00142 move(0.0f, 0.0f, -diff * 0.001 * distance_);
00143 }
00144 else
00145 {
00146 zoom( diff * 0.001 * distance_ );
00147 }
00148
00149 moved = true;
00150 }
00151
00152 if( moved )
00153 {
00154 manager_->queueRender();
00155 }
00156 }
00157
00158 void OrbitViewController::onActivate()
00159 {
00160 if (camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC)
00161 {
00162 camera_->setProjectionType(Ogre::PT_PERSPECTIVE);
00163 }
00164 else
00165 {
00166 Ogre::Vector3 position = camera_->getPosition();
00167 Ogre::Quaternion orientation = camera_->getOrientation();
00168
00169
00170 distance_ = position.length();
00171
00172 Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
00173 focal_point_ = position + direction;
00174
00175 calculatePitchYawFromPosition( position );
00176 }
00177 }
00178
00179 void OrbitViewController::onDeactivate()
00180 {
00181 focal_shape_->getRootNode()->setVisible(false);
00182 camera_->setFixedYawAxis(false);
00183 }
00184
00185 void OrbitViewController::onUpdate(float dt, float ros_dt)
00186 {
00187 updateCamera();
00188 }
00189
00190 void OrbitViewController::lookAt( const Ogre::Vector3& point )
00191 {
00192 Ogre::Vector3 camera_position = camera_->getPosition();
00193 focal_point_ = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition());
00194 distance_ = focal_point_.distance( camera_position );
00195
00196 calculatePitchYawFromPosition(camera_position);
00197 }
00198
00199 void OrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00200 {
00201 focal_point_ += old_reference_position - reference_position_;
00202 }
00203
00204 void OrbitViewController::normalizePitch()
00205 {
00206 if ( pitch_ < PITCH_LIMIT_LOW )
00207 {
00208 pitch_ = PITCH_LIMIT_LOW;
00209 }
00210 else if ( pitch_ > PITCH_LIMIT_HIGH )
00211 {
00212 pitch_ = PITCH_LIMIT_HIGH;
00213 }
00214 }
00215
00216 void OrbitViewController::normalizeYaw()
00217 {
00218 yaw_ = fmod( yaw_, Ogre::Math::TWO_PI );
00219
00220 if ( yaw_ < 0.0f )
00221 {
00222 yaw_ = Ogre::Math::TWO_PI + yaw_;
00223 }
00224 }
00225
00226 void OrbitViewController::updateCamera()
00227 {
00228 float x = distance_ * cos( yaw_ ) * cos( pitch_ ) + focal_point_.x;
00229 float y = distance_ * sin( yaw_ ) * cos( pitch_ ) + focal_point_.y;
00230 float z = distance_ * sin( pitch_ ) + focal_point_.z;
00231
00232 Ogre::Vector3 pos( x, y, z );
00233
00234 camera_->setPosition(pos);
00235 camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
00236 camera_->setDirection(target_scene_node_->getOrientation() * (focal_point_ - pos));
00237
00238
00239
00240 focal_shape_->setPosition(focal_point_);
00241 }
00242
00243 void OrbitViewController::yaw( float angle )
00244 {
00245 yaw_ -= angle;
00246
00247 normalizeYaw();
00248 emitConfigChanged();
00249 }
00250
00251 void OrbitViewController::pitch( float angle )
00252 {
00253 pitch_ -= angle;
00254
00255 normalizePitch();
00256 emitConfigChanged();
00257 }
00258
00259 void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position )
00260 {
00261 float x = position.x - focal_point_.x;
00262 float y = position.y - focal_point_.y;
00263 float z = position.z - focal_point_.z;
00264
00265 pitch_ = asin( z / distance_ );
00266
00267 normalizePitch();
00268
00269 yaw_ = atan2( y, x );
00270 emitConfigChanged();
00271 }
00272
00273 void OrbitViewController::zoom( float amount )
00274 {
00275 distance_ -= amount;
00276
00277 if ( distance_ <= MIN_DISTANCE )
00278 {
00279 distance_ = MIN_DISTANCE;
00280 }
00281 emitConfigChanged();
00282 }
00283
00284 void OrbitViewController::move( float x, float y, float z )
00285 {
00286 focal_point_ += camera_->getOrientation() * Ogre::Vector3( x, y, z );
00287 emitConfigChanged();
00288 }
00289
00290 void OrbitViewController::fromString(const std::string& str)
00291 {
00292 UniformStringStream iss(str);
00293
00294 iss.parseFloat( pitch_ );
00295 iss.parseFloat( yaw_ );
00296 iss.parseFloat( distance_ );
00297 iss.parseFloat( focal_point_.x );
00298 iss.parseFloat( focal_point_.y );
00299 iss.parseFloat( focal_point_.z );
00300 emitConfigChanged();
00301 }
00302
00303 std::string OrbitViewController::toString()
00304 {
00305 UniformStringStream oss;
00306 oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z;
00307
00308 return oss.str();
00309 }
00310
00311
00312 }