orbit_view_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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       // regular left-button drag
00103       if( event.left() && !event.shift() )
00104       {
00105         yaw( diff_x*0.005 );
00106         pitch( -diff_y*0.005 );
00107       }
00108       // middle or shift-left drag
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     // Determine the distance from here to the reference frame, and use that as the distance our focal point should be at
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 //  ROS_INFO_STREAM( camera_->getDerivedDirection() );
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 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32