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 "fixed_orientation_ortho_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 #include <ogre_helpers/orthographic.h>
00044 
00045 #include <stdint.h>
00046 
00047 namespace rviz
00048 {
00049 
00050 FixedOrientationOrthoViewController::FixedOrientationOrthoViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node)
00051 : ViewController(manager, name, target_scene_node)
00052 , scale_(10.0f)
00053 , angle_( 0 )
00054 , dragging_( false )
00055 {
00056 }
00057 
00058 FixedOrientationOrthoViewController::~FixedOrientationOrthoViewController()
00059 {
00060 }
00061 
00062 void FixedOrientationOrthoViewController::reset()
00063 {
00064   scale_ = 10;
00065   angle_ = 0;
00066   setPosition( Ogre::Vector3( 0, 0, 0 ));
00067   emitConfigChanged();
00068 }
00069 
00070 void FixedOrientationOrthoViewController::handleMouseEvent(ViewportMouseEvent& event)
00071 {
00072   bool moved = false;
00073 
00074   if( event.type == QEvent::MouseButtonPress )
00075   {
00076     dragging_ = true;
00077   }
00078   else if( event.type == QEvent::MouseButtonRelease )
00079   {
00080     dragging_ = false;
00081   }
00082   else if( dragging_ && event.type == QEvent::MouseMove )
00083   {
00084     int32_t diff_x = event.x - event.last_x;
00085     int32_t diff_y = event.y - event.last_y;
00086 
00087     if( diff_x != 0 || diff_y != 0 )
00088     {
00089       if( event.left() && !event.shift() )
00090       {
00091         angle_ -= -diff_x * 0.005;
00092         orientCamera();
00093       }
00094       else if( event.middle() || ( event.shift() && event.left() ))
00095       {
00096         move( -diff_x / scale_, diff_y / scale_ );
00097       }
00098       else if( event.right() )
00099       {
00100         scale_ *= 1.0 - diff_y * 0.01;
00101       }
00102 
00103       moved = true;
00104     }
00105   }
00106 
00107   if ( event.wheel_delta != 0 )
00108   {
00109     int diff = event.wheel_delta;
00110     scale_ *= 1.0 - (-diff) * 0.001;
00111 
00112     moved = true;
00113   }
00114 
00115   if (moved)
00116   {
00117     manager_->queueRender();
00118     emitConfigChanged();
00119   }
00120 }
00121 
00122 void FixedOrientationOrthoViewController::orientCamera()
00123 {
00124   camera_->setOrientation( Ogre::Quaternion( Ogre::Radian( angle_ ), Ogre::Vector3::UNIT_Z ));
00125 }
00126 
00127 void FixedOrientationOrthoViewController::onActivate()
00128 {
00129   camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC);
00130   camera_->setFixedYawAxis(false);
00131   setPosition( camera_->getPosition() );
00132   orientCamera();
00133 }
00134 
00135 void FixedOrientationOrthoViewController::onDeactivate()
00136 {
00137   camera_->setCustomProjectionMatrix(false);
00138 }
00139 
00140 void FixedOrientationOrthoViewController::onUpdate(float dt, float ros_dt)
00141 {
00142   updateCamera();
00143 }
00144 
00145 void FixedOrientationOrthoViewController::lookAt( const Ogre::Vector3& point )
00146 {
00147   setPosition( point - target_scene_node_->getPosition() );
00148   emitConfigChanged();
00149 }
00150 
00151 void FixedOrientationOrthoViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
00152 {
00153   move( old_reference_position.x - reference_position_.x,
00154         old_reference_position.y - reference_position_.y );
00155 }
00156 
00157 void FixedOrientationOrthoViewController::updateCamera()
00158 {
00159   orientCamera();
00160 
00161   float width = camera_->getViewport()->getActualWidth();
00162   float height = camera_->getViewport()->getActualHeight();
00163 
00164   Ogre::Matrix4 proj;
00165   buildScaledOrthoMatrix( proj, -width / scale_ / 2, width / scale_ / 2, -height / scale_ / 2, height / scale_ / 2,
00166                                       camera_->getNearClipDistance(), camera_->getFarClipDistance() );
00167   camera_->setCustomProjectionMatrix(true, proj);
00168 }
00169 
00170 void FixedOrientationOrthoViewController::setPosition( const Ogre::Vector3& pos_rel_target )
00171 {
00172   // For Z, we use half of the far-clip distance set in
00173   // selection_manager.cpp, so that the shader program which computes
00174   // depth can see equal distances above and below the Z=0 plane.
00175   camera_->setPosition( pos_rel_target.x, pos_rel_target.y, 500 );
00176 }
00177 
00178 void FixedOrientationOrthoViewController::move( float x, float y )
00179 {
00180   camera_->moveRelative( Ogre::Vector3( x, y, 0 ));
00181 }
00182 
00183 void FixedOrientationOrthoViewController::fromString(const std::string& str)
00184 {
00185   UniformStringStream iss(str);
00186 
00187   iss.parseFloat( scale_ );
00188 
00189   Ogre::Vector3 vec;
00190   iss.parseFloat( vec.x );
00191   iss.parseFloat( vec.y );
00192   setPosition(vec);
00193 
00194   iss.parseFloat( angle_ );
00195   emitConfigChanged();
00196 }
00197 
00198 std::string FixedOrientationOrthoViewController::toString()
00199 {
00200   UniformStringStream oss;
00201   oss << scale_ << " " << camera_->getPosition().x << " " << camera_->getPosition().y << " " << angle_;
00202   return oss.str();
00203 }
00204 
00205 }


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