fps_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 <OgreViewport.h>
00031 #include <OgreQuaternion.h>
00032 #include <OgreVector3.h>
00033 #include <OgreSceneNode.h>
00034 #include <OgreSceneManager.h>
00035 #include <OgreCamera.h>
00036 
00037 #include "rviz/uniform_string_stream.h"
00038 #include "rviz/display_context.h"
00039 #include "rviz/viewport_mouse_event.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 
00045 #include "fps_view_controller.h"
00046 
00047 namespace rviz
00048 {
00049 
00050 static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION =
00051   Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Y ) *
00052   Ogre::Quaternion( Ogre::Radian( -Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_Z );
00053 
00054 static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001;
00055 static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001;
00056 
00057 FPSViewController::FPSViewController()
00058 {
00059   yaw_property_ = new FloatProperty( "Yaw", 0, "Rotation of the camera around the Z (up) axis.", this );
00060 
00061   pitch_property_ = new FloatProperty( "Pitch", 0, "How much the camera is tipped downward.", this );
00062   pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
00063   pitch_property_->setMin( -pitch_property_->getMax() );
00064 
00065   position_property_ = new VectorProperty( "Position", Ogre::Vector3( 5, 5, 10 ), "Position of the camera.", this );
00066 }
00067 
00068 FPSViewController::~FPSViewController()
00069 {
00070 }
00071 
00072 void FPSViewController::onInitialize()
00073 {
00074   FramePositionTrackingViewController::onInitialize();
00075   camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00076 }
00077 
00078 void FPSViewController::reset()
00079 {
00080   camera_->setPosition( Ogre::Vector3( 5, 5, 10 ));
00081   camera_->lookAt( 0, 0, 0 );
00082   setPropertiesFromCamera( camera_ );
00083 
00084   // Hersh says: why is the following junk necessary?  I don't know.
00085   // However, without this you need to call reset() twice after
00086   // switching from TopDownOrtho to FPS.  After the first call the
00087   // camera is in the right position but pointing the wrong way.
00088   updateCamera();
00089   camera_->lookAt( 0, 0, 0 );
00090   setPropertiesFromCamera( camera_ );
00091 }
00092 
00093 void FPSViewController::handleMouseEvent(ViewportMouseEvent& event)
00094 {
00095   if ( event.shift() )
00096   {
00097     setStatus( "<b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z." );
00098   }
00099   else
00100   {
00101     setStatus( "<b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click:</b>: Zoom.  <b>Shift</b>: More options." );
00102   }
00103 
00104   bool moved = false;
00105 
00106   int32_t diff_x = 0;
00107   int32_t diff_y = 0;
00108 
00109   if( event.type == QEvent::MouseMove )
00110   {
00111     diff_x = event.x - event.last_x;
00112     diff_y = event.y - event.last_y;
00113     moved = true;
00114   }
00115 
00116   if( event.left() && !event.shift() )
00117   {
00118     setCursor( Rotate3D );
00119     yaw( -diff_x*0.005 );
00120     pitch( diff_y*0.005 );
00121   }
00122   else if( event.middle() || ( event.shift() && event.left() ))
00123   {
00124     setCursor( MoveXY );
00125     move( diff_x*0.01, -diff_y*0.01, 0.0f );
00126   }
00127   else if( event.right() )
00128   {
00129     setCursor( MoveZ );
00130     move( 0.0f, 0.0f, diff_y*0.1 );
00131   }
00132   else
00133   {
00134     setCursor( event.shift() ? MoveXY : Rotate3D );
00135   }
00136 
00137   if ( event.wheel_delta != 0 )
00138   {
00139     int diff = event.wheel_delta;
00140     move( 0.0f, 0.0f, -diff * 0.01 );
00141 
00142     moved = true;
00143   }
00144 
00145   if (moved)
00146   {
00147     context_->queueRender();
00148   }
00149 }
00150 
00151 void FPSViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
00152 {
00153   Ogre::Quaternion quat = source_camera->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
00154   float yaw = quat.getRoll( false ).valueRadians(); // OGRE camera frame looks along -Z, so they call rotation around Z "roll".
00155   float pitch = quat.getYaw( false ).valueRadians(); // OGRE camera frame has +Y as "up", so they call rotation around Y "yaw".
00156 
00157   Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;
00158 
00159   if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
00160   {
00161     if ( pitch > Ogre::Math::HALF_PI )
00162     {
00163       pitch -= Ogre::Math::PI;
00164     }
00165     else if ( pitch < -Ogre::Math::HALF_PI )
00166     {
00167       pitch += Ogre::Math::PI;
00168     }
00169 
00170     yaw = -yaw;
00171 
00172     if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
00173     {
00174       yaw -= Ogre::Math::PI;
00175     }
00176     else
00177     {
00178       yaw += Ogre::Math::PI;
00179     }
00180   }
00181 
00182   pitch_property_->setFloat( pitch );
00183   yaw_property_->setFloat( mapAngleTo0_2Pi( yaw ));
00184   position_property_->setVector( source_camera->getPosition() );
00185 }
00186 
00187 void FPSViewController::mimic( ViewController* source_view )
00188 {
00189   FramePositionTrackingViewController::mimic( source_view );
00190   setPropertiesFromCamera( source_view->getCamera() );
00191 }
00192 
00193 void FPSViewController::update(float dt, float ros_dt)
00194 {
00195   FramePositionTrackingViewController::update( dt, ros_dt );
00196   updateCamera();
00197 }
00198 
00199 void FPSViewController::lookAt( const Ogre::Vector3& point )
00200 {
00201   camera_->lookAt( point );
00202   setPropertiesFromCamera( camera_ );
00203 }
00204 
00205 void FPSViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00206 {
00207   position_property_->add( old_reference_position - reference_position_ );
00208 }
00209 
00210 void FPSViewController::updateCamera()
00211 {
00212   camera_->setOrientation( getOrientation() );
00213   camera_->setPosition( position_property_->getVector() );
00214 }
00215 
00216 void FPSViewController::yaw( float angle )
00217 {
00218   yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() + angle ));
00219 }
00220 
00221 void FPSViewController::pitch( float angle )
00222 {
00223   pitch_property_->add( angle );
00224 }
00225 
00226 Ogre::Quaternion FPSViewController::getOrientation()
00227 {
00228   Ogre::Quaternion pitch, yaw;
00229 
00230   yaw.FromAngleAxis( Ogre::Radian( yaw_property_->getFloat() ), Ogre::Vector3::UNIT_Z );
00231   pitch.FromAngleAxis( Ogre::Radian( pitch_property_->getFloat() ), Ogre::Vector3::UNIT_Y );
00232 
00233   return yaw * pitch * ROBOT_TO_CAMERA_ROTATION;
00234 }
00235 
00236 void FPSViewController::move( float x, float y, float z )
00237 {
00238   Ogre::Vector3 translate( x, y, z );
00239   position_property_->add( getOrientation() * translate );
00240 }
00241 
00242 } // end namespace rviz
00243 
00244 #include <pluginlib/class_list_macros.h>
00245 PLUGINLIB_EXPORT_CLASS( rviz::FPSViewController, rviz::ViewController )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Tue Oct 3 2017 03:19:30