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 }