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 <stdint.h>
00031 
00032 #include <OgreCamera.h>
00033 #include <OgreQuaternion.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreSceneNode.h>
00036 #include <OgreVector3.h>
00037 #include <OgreViewport.h>
00038 
00039 #include "rviz/display_context.h"
00040 #include "rviz/geometry.h"
00041 #include "rviz/ogre_helpers/shape.h"
00042 #include "rviz/properties/float_property.h"
00043 #include "rviz/properties/vector_property.h"
00044 #include "rviz/uniform_string_stream.h"
00045 #include "rviz/viewport_mouse_event.h"
00046 #include "rviz/load_resource.h"
00047 #include "rviz/render_panel.h"
00048 
00049 #include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
00050 
00051 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0;
00052 static const float YAW_START = Ogre::Math::HALF_PI * 0.5;
00053 static const float DISTANCE_START = 10;
00054 
00055 namespace rviz
00056 {
00057 
00058 OrbitViewController::OrbitViewController()
00059   : dragging_( false )
00060 {
00061   distance_property_ = new FloatProperty( "Distance", DISTANCE_START, "Distance from the focal point.", this );
00062   distance_property_->setMin( 0.01 );
00063 
00064   yaw_property_ = new FloatProperty( "Yaw", YAW_START, "Rotation of the camera around the Z (up) axis.", this );
00065 
00066   pitch_property_ = new FloatProperty( "Pitch", PITCH_START, "How much the camera is tipped downward.", this );
00067   pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
00068   pitch_property_->setMin( -pitch_property_->getMax() );
00069 
00070   focal_point_property_ = new VectorProperty( "Focal Point", Ogre::Vector3::ZERO, "The center point which the camera orbits.", this );
00071 }
00072 
00073 void OrbitViewController::onInitialize()
00074 {
00075   FramePositionTrackingViewController::onInitialize();
00076 
00077   camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00078 
00079   focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), target_scene_node_);
00080   focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
00081   focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
00082   focal_shape_->getRootNode()->setVisible(false);
00083 }
00084 
00085 OrbitViewController::~OrbitViewController()
00086 {
00087   delete focal_shape_;
00088 }
00089 
00090 void OrbitViewController::reset()
00091 {
00092   dragging_ = false;
00093   yaw_property_->setFloat( YAW_START );
00094   pitch_property_->setFloat( PITCH_START );
00095   distance_property_->setFloat( DISTANCE_START );
00096   focal_point_property_->setVector( Ogre::Vector3::ZERO );
00097 }
00098 
00099 void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
00100 {
00101   if ( event.shift() )
00102   {
00103     setStatus( "<b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z.  <b>Mouse Wheel:</b>: Zoom.  " );
00104   }
00105   else
00106   {
00107     setStatus( "<b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click/Mouse Wheel:</b>: Zoom.  <b>Shift</b>: More options." );
00108   }
00109 
00110   float distance = distance_property_->getFloat();
00111 
00112   int32_t diff_x = 0;
00113   int32_t diff_y = 0;
00114 
00115   bool moved = false;
00116 
00117   if( event.type == QEvent::MouseButtonPress )
00118   {
00119     focal_shape_->getRootNode()->setVisible(true);
00120     moved = true;
00121     dragging_ = true;
00122   }
00123   else if( event.type == QEvent::MouseButtonRelease )
00124   {
00125     focal_shape_->getRootNode()->setVisible(false);
00126     moved = true;
00127     dragging_ = false;
00128   }
00129   else if( dragging_ && event.type == QEvent::MouseMove )
00130   {
00131     diff_x = event.x - event.last_x;
00132     diff_y = event.y - event.last_y;
00133     moved = true;
00134   }
00135 
00136   // regular left-button drag
00137   if( event.left() && !event.shift() )
00138   {
00139     setCursor( Rotate3D );
00140     yaw( diff_x*0.005 );
00141     pitch( -diff_y*0.005 );
00142   }
00143   // middle or shift-left drag
00144   else if( event.middle() || (event.shift() && event.left()) )
00145   {
00146     setCursor( MoveXY );
00147     float fovY = camera_->getFOVy().valueRadians();
00148     float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );
00149 
00150     int width = camera_->getViewport()->getActualWidth();
00151     int height = camera_->getViewport()->getActualHeight();
00152 
00153     move( -((float)diff_x / (float)width) * distance * tan( fovX / 2.0f ) * 2.0f,
00154           ((float)diff_y / (float)height) * distance * tan( fovY / 2.0f ) * 2.0f,
00155           0.0f );
00156   }
00157   else if( event.right() )
00158   {
00159     if( event.shift() )
00160     {
00161       // move in z direction
00162       setCursor( MoveZ );
00163       move(0.0f, 0.0f, diff_y * 0.1 * (distance / 10.0f));
00164     }
00165     else
00166     {
00167       // zoom
00168       setCursor( Zoom );
00169       zoom( -diff_y * 0.1 * (distance / 10.0f) );
00170     }
00171   }
00172   else
00173   {
00174     setCursor( event.shift() ? MoveXY : Rotate3D );
00175   }
00176 
00177   moved = true;
00178 
00179   if( event.wheel_delta != 0 )
00180   {
00181     int diff = event.wheel_delta;
00182     if( event.shift() )
00183     {
00184       move( 0, 0, -diff * 0.001 * distance );
00185     }
00186     else
00187     {
00188       zoom( diff * 0.001 * distance );
00189     }
00190 
00191     moved = true;
00192   }
00193 
00194   if( moved )
00195   {
00196     context_->queueRender();
00197   }
00198 }
00199 
00200 void OrbitViewController::mimic( ViewController* source_view )
00201 {
00202   FramePositionTrackingViewController::mimic( source_view );
00203 
00204   Ogre::Camera* source_camera = source_view->getCamera();
00205   Ogre::Vector3 position = source_camera->getPosition();
00206   Ogre::Quaternion orientation = source_camera->getOrientation();
00207 
00208   if( source_view->getClassId() == "rviz/Orbit" )
00209   {
00210     // If I'm initializing from another instance of this same class, get the distance exactly.
00211     distance_property_->setFloat( source_view->subProp( "Distance" )->getValue().toFloat() );
00212   }
00213   else
00214   {
00215     // Determine the distance from here to the reference frame, and use
00216     // that as the distance our focal point should be at.
00217     distance_property_->setFloat( position.length() );
00218   }
00219 
00220   Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat() );
00221   focal_point_property_->setVector( position + direction );
00222 
00223   calculatePitchYawFromPosition( position );
00224 }
00225 
00226 void OrbitViewController::update(float dt, float ros_dt)
00227 {
00228   FramePositionTrackingViewController::update( dt, ros_dt );
00229   updateCamera();
00230 }
00231 
00232 void OrbitViewController::lookAt( const Ogre::Vector3& point )
00233 {
00234   Ogre::Vector3 camera_position = camera_->getPosition();
00235   focal_point_property_->setVector( target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()) );
00236   distance_property_->setFloat( focal_point_property_->getVector().distance( camera_position ));
00237 
00238   calculatePitchYawFromPosition(camera_position);
00239 }
00240 
00241 void OrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00242 {
00243   focal_point_property_->add( old_reference_position - reference_position_ );
00244 }
00245 
00246 void OrbitViewController::updateCamera()
00247 {
00248   float distance = distance_property_->getFloat();
00249   float yaw = yaw_property_->getFloat();
00250   float pitch = pitch_property_->getFloat();
00251 
00252   Ogre::Vector3 focal_point = focal_point_property_->getVector();
00253 
00254   float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
00255   float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
00256   float z = distance *              sin( pitch ) + focal_point.z;
00257 
00258   Ogre::Vector3 pos( x, y, z );
00259 
00260   camera_->setPosition(pos);
00261   camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
00262   camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));
00263 
00264   focal_shape_->setPosition( focal_point );
00265 }
00266 
00267 void OrbitViewController::yaw( float angle )
00268 {
00269   yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() - angle ));
00270 }
00271 
00272 void OrbitViewController::pitch( float angle )
00273 {
00274   pitch_property_->add( -angle );
00275 }
00276 
00277 void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position )
00278 {
00279   Ogre::Vector3 diff = position - focal_point_property_->getVector();
00280   pitch_property_->setFloat( asin( diff.z / distance_property_->getFloat() ));
00281   yaw_property_->setFloat( atan2( diff.y, diff.x ));
00282 }
00283 
00284 void OrbitViewController::zoom( float amount )
00285 {
00286   distance_property_->add( -amount );
00287 }
00288 
00289 void OrbitViewController::move( float x, float y, float z )
00290 {
00291   focal_point_property_->add( camera_->getOrientation() * Ogre::Vector3( x, y, z ));
00292 }
00293 
00294 } // end namespace rviz
00295 
00296 #include <pluginlib/class_list_macros.h>
00297 PLUGINLIB_EXPORT_CLASS( rviz::OrbitViewController, rviz::ViewController )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:15