fixed_orientation_ortho_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 <OGRE/OgreCamera.h>
00031 #include <OGRE/OgreQuaternion.h>
00032 #include <OGRE/OgreSceneManager.h>
00033 #include <OGRE/OgreSceneNode.h>
00034 #include <OGRE/OgreVector3.h>
00035 #include <OGRE/OgreViewport.h>
00036 
00037 #include "rviz/display_context.h"
00038 #include "rviz/ogre_helpers/orthographic.h"
00039 #include "rviz/ogre_helpers/shape.h"
00040 #include "rviz/properties/float_property.h"
00041 #include "rviz/viewport_mouse_event.h"
00042 
00043 #include "rviz/default_plugin/view_controllers/fixed_orientation_ortho_view_controller.h"
00044 
00045 namespace rviz
00046 {
00047 
00048 FixedOrientationOrthoViewController::FixedOrientationOrthoViewController()
00049   : dragging_( false )
00050 {
00051   scale_property_ = new FloatProperty( "Scale", 10, "How much to scale up the size of things in the scene.", this );
00052   angle_property_ = new FloatProperty( "Angle", 0, "Angle around the Z axis to rotate.", this );
00053   x_property_ = new FloatProperty( "X", 0, "X component of camera position.", this );
00054   y_property_ = new FloatProperty( "Y", 0, "Y component of camera position.", this );
00055 }
00056 
00057 FixedOrientationOrthoViewController::~FixedOrientationOrthoViewController()
00058 {
00059 }
00060 
00061 void FixedOrientationOrthoViewController::onInitialize()
00062 {
00063   FramePositionTrackingViewController::onInitialize();
00064 
00065   camera_->setProjectionType( Ogre::PT_ORTHOGRAPHIC );
00066   camera_->setFixedYawAxis( false );
00067 }
00068 
00069 void FixedOrientationOrthoViewController::reset()
00070 {
00071   scale_property_->setFloat( 10 );
00072   angle_property_->setFloat( 0 );
00073   x_property_->setFloat( 0 );
00074   y_property_->setFloat( 0 );
00075 }
00076 
00077 void FixedOrientationOrthoViewController::handleMouseEvent(ViewportMouseEvent& event)
00078 {
00079   if ( event.shift() )
00080   {
00081     setStatus( "<b>Left-Click:</b> Move X/Y." );
00082   }
00083   else
00084   {
00085     setStatus( "<b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click:</b>: Zoom.  <b>Shift</b>: More options." );
00086   }
00087 
00088   bool moved = false;
00089 
00090   int32_t diff_x = 0;
00091   int32_t diff_y = 0;
00092 
00093   if( event.type == QEvent::MouseButtonPress )
00094   {
00095     dragging_ = true;
00096   }
00097   else if( event.type == QEvent::MouseButtonRelease )
00098   {
00099     dragging_ = false;
00100   }
00101   else if( dragging_ && event.type == QEvent::MouseMove )
00102   {
00103     diff_x = event.x - event.last_x;
00104     diff_y = event.y - event.last_y;
00105     moved = true;
00106   }
00107 
00108   if( event.left() && !event.shift() )
00109   {
00110     setCursor( Rotate2D );
00111     angle_property_->add( diff_x * 0.005 );
00112     orientCamera();
00113   }
00114   else if( event.middle() || ( event.shift() && event.left() ))
00115   {
00116     setCursor( MoveXY );
00117     float scale = scale_property_->getFloat();
00118     move( -diff_x / scale, diff_y / scale );
00119   }
00120   else if( event.right() )
00121   {
00122     setCursor( Zoom );
00123     scale_property_->multiply( 1.0 - diff_y * 0.01 );
00124   }
00125   else
00126   {
00127     setCursor( event.shift() ? MoveXY : Rotate2D );
00128   }
00129 
00130   if ( event.wheel_delta != 0 )
00131   {
00132     int diff = event.wheel_delta;
00133     scale_property_->multiply( 1.0 - (-diff) * 0.001 );
00134 
00135     moved = true;
00136   }
00137 
00138   if (moved)
00139   {
00140     context_->queueRender();
00141     emitConfigChanged();
00142   }
00143 }
00144 
00145 void FixedOrientationOrthoViewController::orientCamera()
00146 {
00147   camera_->setOrientation( Ogre::Quaternion( Ogre::Radian( angle_property_->getFloat() ), Ogre::Vector3::UNIT_Z ));
00148 }
00149 
00150 void FixedOrientationOrthoViewController::mimic( ViewController* source_view )
00151 {
00152   FramePositionTrackingViewController::mimic( source_view );
00153 
00154   if( FixedOrientationOrthoViewController* source_ortho = qobject_cast<FixedOrientationOrthoViewController*>( source_view ))
00155   {
00156     scale_property_->setFloat( source_ortho->scale_property_->getFloat() );
00157     angle_property_->setFloat( source_ortho->angle_property_->getFloat() );
00158     x_property_->setFloat( source_ortho->x_property_->getFloat() );
00159     y_property_->setFloat( source_ortho->y_property_->getFloat() );
00160   }
00161   else
00162   {
00163     Ogre::Camera* source_camera = source_view->getCamera();
00164     setPosition( source_camera->getPosition() );
00165   }
00166 }
00167 
00168 void FixedOrientationOrthoViewController::update(float dt, float ros_dt)
00169 {
00170   FramePositionTrackingViewController::update( dt, ros_dt );
00171   updateCamera();
00172 }
00173 
00174 void FixedOrientationOrthoViewController::lookAt( const Ogre::Vector3& point )
00175 {
00176   setPosition( point - target_scene_node_->getPosition() );
00177 }
00178 
00179 void FixedOrientationOrthoViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00180 {
00181   move( old_reference_position.x - reference_position_.x,
00182         old_reference_position.y - reference_position_.y );
00183 }
00184 
00185 void FixedOrientationOrthoViewController::updateCamera()
00186 {
00187   orientCamera();
00188 
00189   float width = camera_->getViewport()->getActualWidth();
00190   float height = camera_->getViewport()->getActualHeight();
00191 
00192   float scale = scale_property_->getFloat();
00193   Ogre::Matrix4 proj;
00194   buildScaledOrthoMatrix( proj, -width / scale / 2, width / scale / 2, -height / scale / 2, height / scale / 2,
00195                           camera_->getNearClipDistance(), camera_->getFarClipDistance() );
00196   camera_->setCustomProjectionMatrix(true, proj);
00197 
00198   // For Z, we use half of the far-clip distance set in
00199   // selection_context.cpp, so that the shader program which computes
00200   // depth can see equal distances above and below the Z=0 plane.
00201   camera_->setPosition( x_property_->getFloat(), y_property_->getFloat(), 500 );
00202 }
00203 
00204 void FixedOrientationOrthoViewController::setPosition( const Ogre::Vector3& pos_rel_target )
00205 {
00206   x_property_->setFloat( pos_rel_target.x );
00207   y_property_->setFloat( pos_rel_target.y );
00208 }
00209 
00210 void FixedOrientationOrthoViewController::move( float dx, float dy )
00211 {
00212   float angle = angle_property_->getFloat();
00213   x_property_->add( dx*cos(angle)-dy*sin(angle) );
00214   y_property_->add( dx*sin(angle)+dy*cos(angle) );
00215 }
00216 
00217 } // end namespace rviz
00218 
00219 #include <pluginlib/class_list_macros.h>
00220 PLUGINLIB_EXPORT_CLASS( rviz::FixedOrientationOrthoViewController, rviz::ViewController )


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:35