$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 "orbit_view_controller.h" 00031 #include "rviz/viewport_mouse_event.h" 00032 #include "rviz/visualization_manager.h" 00033 #include "rviz/frame_manager.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_tools/shape.h> 00043 00044 #include <stdint.h> 00045 #include <sstream> 00046 00047 namespace rviz 00048 { 00049 00050 static const float MIN_DISTANCE = 0.01; 00051 static const float PITCH_LIMIT_LOW = 0.001; 00052 static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001; 00053 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0; 00054 static const float YAW_START = Ogre::Math::HALF_PI * 0.5; 00055 00056 OrbitViewController::OrbitViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node) 00057 : ViewController(manager, name, target_scene_node) 00058 , focal_point_( Ogre::Vector3::ZERO ) 00059 , yaw_( YAW_START ) 00060 , pitch_( PITCH_START ) 00061 , distance_( 10.0f ) 00062 { 00063 focal_shape_ = new ogre_tools::Shape(ogre_tools::Shape::Sphere, manager_->getSceneManager(), target_scene_node_); 00064 focal_shape_->setScale(Ogre::Vector3(0.05f, 0.01f, 0.05f)); 00065 focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f); 00066 focal_shape_->getRootNode()->setVisible(false); 00067 } 00068 00069 OrbitViewController::~OrbitViewController() 00070 { 00071 delete focal_shape_; 00072 } 00073 00074 void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event) 00075 { 00076 bool moved = false; 00077 if ( event.event.LeftDown() || event.event.RightDown() || event.event.MiddleDown() ) 00078 { 00079 focal_shape_->getRootNode()->setVisible(true); 00080 moved = true; 00081 } 00082 else if ( event.event.LeftUp() || event.event.RightUp() || event.event.MiddleUp() ) 00083 { 00084 focal_shape_->getRootNode()->setVisible(false); 00085 moved = true; 00086 } 00087 else if ( event.event.Dragging() ) 00088 { 00089 int32_t diff_x = event.event.GetX() - event.last_x; 00090 int32_t diff_y = event.event.GetY() - event.last_y; 00091 00092 if ( event.event.LeftIsDown() && !event.event.ShiftDown() ) 00093 { 00094 yaw( diff_x*0.005 ); 00095 pitch( -diff_y*0.005 ); 00096 } 00097 else if ( event.event.MiddleIsDown() || 00098 ( event.event.ShiftDown() && event.event.LeftIsDown() )) 00099 { 00100 float fovY = camera_->getFOVy().valueRadians(); 00101 float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() ); 00102 00103 int width = camera_->getViewport()->getActualWidth(); 00104 int height = camera_->getViewport()->getActualHeight(); 00105 00106 move( -((float)diff_x / (float)width) * distance_ * tan( fovX / 2.0f ) * 2.0f, ((float)diff_y / (float)height) * distance_ * tan( fovY / 2.0f ) * 2.0f, 0.0f ); 00107 } 00108 else if ( event.event.RightIsDown() ) 00109 { 00110 if (event.event.ShiftDown()) 00111 { 00112 move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f)); 00113 } 00114 else 00115 { 00116 zoom( -diff_y * 0.1 * (distance_ / 10.0f) ); 00117 } 00118 } 00119 00120 moved = true; 00121 } 00122 00123 if ( event.event.GetWheelRotation() != 0 ) 00124 { 00125 int diff = event.event.GetWheelRotation(); 00126 if (event.event.ShiftDown()) 00127 { 00128 move(0.0f, 0.0f, -diff * 0.01 * (distance_ / 10.0f)); 00129 } 00130 else 00131 { 00132 zoom( diff * 0.01 * (distance_ / 10.0f) ); 00133 } 00134 00135 moved = true; 00136 } 00137 00138 if (moved) 00139 { 00140 manager_->queueRender(); 00141 } 00142 } 00143 00144 void OrbitViewController::onActivate() 00145 { 00146 if (camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC) 00147 { 00148 camera_->setProjectionType(Ogre::PT_PERSPECTIVE); 00149 } 00150 else 00151 { 00152 Ogre::Vector3 position = camera_->getPosition(); 00153 Ogre::Quaternion orientation = camera_->getOrientation(); 00154 00155 // Determine the distance from here to the reference frame, and use that as the distance our focal point should be at 00156 distance_ = position.length(); 00157 00158 Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_); 00159 focal_point_ = position + direction; 00160 00161 calculatePitchYawFromPosition( position ); 00162 } 00163 } 00164 00165 void OrbitViewController::onDeactivate() 00166 { 00167 focal_shape_->getRootNode()->setVisible(false); 00168 camera_->setFixedYawAxis(false); 00169 } 00170 00171 void OrbitViewController::onUpdate(float dt, float ros_dt) 00172 { 00173 updateCamera(); 00174 } 00175 00176 void OrbitViewController::lookAt( const Ogre::Vector3& point ) 00177 { 00178 Ogre::Vector3 camera_position = camera_->getPosition(); 00179 focal_point_ = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()); 00180 distance_ = focal_point_.distance( camera_position ); 00181 00182 calculatePitchYawFromPosition(camera_position); 00183 } 00184 00185 void OrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation) 00186 { 00187 // Compute camera's global coords 00188 Ogre::Vector3 old_camera_pos = old_reference_position + old_reference_orientation * camera_->getPosition(); 00189 // Ogre::Vector3 old_camera_orient = old_reference_orientation * camera_->getOrientation(); 00190 00191 // Compute camera's new coords relative to the reference frame 00192 Ogre::Vector3 camera_position = target_scene_node_->getOrientation().Inverse() * (old_camera_pos - target_scene_node_->getPosition()); 00193 00194 focal_point_ = Ogre::Vector3::ZERO; 00195 distance_ = focal_point_.distance( camera_position ); 00196 00197 calculatePitchYawFromPosition(camera_position); 00198 } 00199 00200 void OrbitViewController::normalizePitch() 00201 { 00202 if ( pitch_ < PITCH_LIMIT_LOW ) 00203 { 00204 pitch_ = PITCH_LIMIT_LOW; 00205 } 00206 else if ( pitch_ > PITCH_LIMIT_HIGH ) 00207 { 00208 pitch_ = PITCH_LIMIT_HIGH; 00209 } 00210 } 00211 00212 void OrbitViewController::normalizeYaw() 00213 { 00214 yaw_ = fmod( yaw_, Ogre::Math::TWO_PI ); 00215 00216 if ( yaw_ < 0.0f ) 00217 { 00218 yaw_ = Ogre::Math::TWO_PI + yaw_; 00219 } 00220 } 00221 00222 void OrbitViewController::updateCamera() 00223 { 00224 float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + focal_point_.x; 00225 float y = distance_ * cos( pitch_ ) + focal_point_.y; 00226 float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + focal_point_.z; 00227 00228 Ogre::Vector3 pos( x, y, z ); 00229 00230 camera_->setPosition(pos); 00231 camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Y); 00232 camera_->setDirection(target_scene_node_->getOrientation() * (focal_point_ - pos)); 00233 00234 // ROS_INFO_STREAM( camera_->getDerivedDirection() ); 00235 00236 focal_shape_->setPosition(focal_point_); 00237 } 00238 00239 void OrbitViewController::yaw( float angle ) 00240 { 00241 yaw_ += angle; 00242 00243 normalizeYaw(); 00244 } 00245 00246 void OrbitViewController::pitch( float angle ) 00247 { 00248 pitch_ += angle; 00249 00250 normalizePitch(); 00251 } 00252 00253 void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position ) 00254 { 00255 float x = position.x - focal_point_.x; 00256 float y = position.y - focal_point_.y; 00257 pitch_ = acos( y / distance_ ); 00258 00259 normalizePitch(); 00260 00261 float val = x / ( distance_ * sin( pitch_ ) ); 00262 00263 yaw_ = acos( val ); 00264 00265 Ogre::Vector3 direction = focal_point_ - position; 00266 00267 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 ) 00268 { 00269 yaw_ = Ogre::Math::TWO_PI - yaw_; 00270 } 00271 } 00272 00273 void OrbitViewController::zoom( float amount ) 00274 { 00275 distance_ -= amount; 00276 00277 if ( distance_ <= MIN_DISTANCE ) 00278 { 00279 distance_ = MIN_DISTANCE; 00280 } 00281 } 00282 00283 void OrbitViewController::move( float x, float y, float z ) 00284 { 00285 focal_point_ += camera_->getOrientation() * Ogre::Vector3( x, y, z ); 00286 } 00287 00288 void OrbitViewController::fromString(const std::string& str) 00289 { 00290 std::istringstream iss(str); 00291 00292 iss >> pitch_; 00293 iss.ignore(); 00294 iss >> yaw_; 00295 iss.ignore(); 00296 iss >> distance_; 00297 iss.ignore(); 00298 iss >> focal_point_.x; 00299 iss.ignore(); 00300 iss >> focal_point_.y; 00301 iss.ignore(); 00302 iss >> focal_point_.z; 00303 00304 resetTargetSceneNodePosition(); 00305 } 00306 00307 std::string OrbitViewController::toString() 00308 { 00309 std::ostringstream oss; 00310 oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z; 00311 00312 return oss.str(); 00313 } 00314 00315 00316 }