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