$search
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 00034 #include <OGRE/OgreCamera.h> 00035 #include <OGRE/OgreSceneManager.h> 00036 #include <OGRE/OgreSceneNode.h> 00037 #include <OGRE/OgreVector3.h> 00038 #include <OGRE/OgreQuaternion.h> 00039 #include <OGRE/OgreViewport.h> 00040 00041 #include <ogre_tools/shape.h> 00042 #include <ogre_tools/orthographic.h> 00043 00044 #include <stdint.h> 00045 #include <sstream> 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 { 00055 } 00056 00057 FixedOrientationOrthoViewController::~FixedOrientationOrthoViewController() 00058 { 00059 } 00060 00061 void FixedOrientationOrthoViewController::handleMouseEvent(ViewportMouseEvent& event) 00062 { 00063 bool moved = false; 00064 00065 if( event.type == QEvent::MouseMove ) 00066 { 00067 int32_t diff_x = event.x - event.last_x; 00068 int32_t diff_y = event.y - event.last_y; 00069 00070 if( event.left() && !event.shift() ) 00071 { 00072 angle_ -= -diff_x * 0.005; 00073 orientCamera(); 00074 } 00075 else if( event.middle() || ( event.shift() && event.left() )) 00076 { 00077 move( -diff_x / scale_, diff_y / scale_ ); 00078 } 00079 else if( event.right() ) 00080 { 00081 scale_ *= 1.0 - diff_y * 0.01; 00082 } 00083 00084 moved = true; 00085 } 00086 00087 if ( event.wheel_delta != 0 ) 00088 { 00089 int diff = event.wheel_delta; 00090 scale_ *= 1.0 - (-diff) * 0.001; 00091 00092 moved = true; 00093 } 00094 00095 if (moved) 00096 { 00097 manager_->queueRender(); 00098 } 00099 } 00100 00101 void FixedOrientationOrthoViewController::orientCamera() 00102 { 00103 camera_->setOrientation( Ogre::Quaternion( Ogre::Radian( angle_ ), Ogre::Vector3::UNIT_Z )); 00104 } 00105 00106 void FixedOrientationOrthoViewController::onActivate() 00107 { 00108 camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC); 00109 camera_->setFixedYawAxis(false); 00110 setPosition( camera_->getPosition() ); 00111 orientCamera(); 00112 } 00113 00114 void FixedOrientationOrthoViewController::onDeactivate() 00115 { 00116 camera_->setCustomProjectionMatrix(false); 00117 } 00118 00119 void FixedOrientationOrthoViewController::onUpdate(float dt, float ros_dt) 00120 { 00121 updateCamera(); 00122 } 00123 00124 void FixedOrientationOrthoViewController::lookAt( const Ogre::Vector3& point ) 00125 { 00126 setPosition( point - target_scene_node_->getPosition() ); 00127 } 00128 00129 void FixedOrientationOrthoViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation) 00130 { 00131 move( old_reference_position.x - reference_position_.x, 00132 old_reference_position.y - reference_position_.y ); 00133 } 00134 00135 void FixedOrientationOrthoViewController::updateCamera() 00136 { 00137 orientCamera(); 00138 00139 float width = camera_->getViewport()->getActualWidth(); 00140 float height = camera_->getViewport()->getActualHeight(); 00141 00142 Ogre::Matrix4 proj; 00143 ogre_tools::buildScaledOrthoMatrix( proj, -width / scale_ / 2, width / scale_ / 2, -height / scale_ / 2, height / scale_ / 2, 00144 camera_->getNearClipDistance(), camera_->getFarClipDistance() ); 00145 camera_->setCustomProjectionMatrix(true, proj); 00146 } 00147 00148 void FixedOrientationOrthoViewController::setPosition( const Ogre::Vector3& pos_rel_target ) 00149 { 00150 // For Z, we use an arbitrary large number smaller than camera's 00151 // far-clip-distance (100k). Any objects above it will not show up. 00152 camera_->setPosition( pos_rel_target.x, pos_rel_target.y, 10000 ); 00153 } 00154 00155 void FixedOrientationOrthoViewController::move( float x, float y ) 00156 { 00157 camera_->moveRelative( Ogre::Vector3( x, y, 0 )); 00158 } 00159 00160 void FixedOrientationOrthoViewController::fromString(const std::string& str) 00161 { 00162 std::istringstream iss(str); 00163 00164 iss >> scale_; 00165 iss.ignore(); 00166 00167 Ogre::Vector3 vec; 00168 iss >> vec.x; 00169 iss.ignore(); 00170 iss >> vec.y; 00171 iss.ignore(); 00172 setPosition(vec); 00173 00174 iss >> angle_; 00175 } 00176 00177 std::string FixedOrientationOrthoViewController::toString() 00178 { 00179 std::ostringstream oss; 00180 oss << scale_ << " " << camera_->getPosition().x << " " << camera_->getPosition().y << " " << angle_; 00181 return oss.str(); 00182 } 00183 00184 }