$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 "xy_orbit_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 #include <OGRE/OgrePlane.h> 00041 #include <OGRE/OgreRay.h> 00042 00043 #include <ogre_tools/shape.h> 00044 00045 #include <stdint.h> 00046 #include <sstream> 00047 00048 namespace rviz 00049 { 00050 00051 static const float MIN_DISTANCE = 0.01; 00052 static const float PITCH_LIMIT_LOW = 0.001; 00053 static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001; 00054 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0; 00055 static const float YAW_START = Ogre::Math::HALF_PI * 0.5; 00056 00057 // move camera up so the focal point appears in the lower image half 00058 static const float CAMERA_OFFSET = 0.2; 00059 00060 00061 XYOrbitViewController::XYOrbitViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node) 00062 : ViewController(manager, name, target_scene_node) 00063 , focal_point_( Ogre::Vector3::ZERO ) 00064 , yaw_( YAW_START ) 00065 , pitch_( PITCH_START ) 00066 , distance_( 10.0f ) 00067 { 00068 focal_shape_ = new ogre_tools::Shape(ogre_tools::Shape::Sphere, manager_->getSceneManager(), target_scene_node_); 00069 focal_shape_->setScale(Ogre::Vector3(0.05f, 0.01f, 0.05f)); 00070 focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f); 00071 focal_shape_->getRootNode()->setVisible(false); 00072 } 00073 00074 XYOrbitViewController::~XYOrbitViewController() 00075 { 00076 delete focal_shape_; 00077 } 00078 00079 bool XYOrbitViewController::intersectGroundPlane( Ogre::Ray mouse_ray, Ogre::Vector3 &intersection_3d ) 00080 { 00081 //convert rays into reference frame 00082 mouse_ray.setOrigin( target_scene_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) ); 00083 mouse_ray.setDirection( target_scene_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() ); 00084 00085 Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Y, 0 ); 00086 00087 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(ground_plane); 00088 if (!intersection.first) 00089 { 00090 return false; 00091 } 00092 00093 intersection_3d = mouse_ray.getPoint(intersection.second); 00094 return true; 00095 } 00096 00097 void XYOrbitViewController::handleMouseEvent(ViewportMouseEvent& event) 00098 { 00099 bool moved = false; 00100 if ( event.event.LeftDown() || event.event.RightDown() || event.event.MiddleDown() ) 00101 { 00102 focal_shape_->getRootNode()->setVisible(true); 00103 moved = true; 00104 } 00105 else if ( event.event.LeftUp() || event.event.RightUp() || event.event.MiddleUp() ) 00106 { 00107 focal_shape_->getRootNode()->setVisible(false); 00108 moved = true; 00109 } 00110 else if ( event.event.Dragging() ) 00111 { 00112 int32_t diff_x = event.event.GetX() - event.last_x; 00113 int32_t diff_y = event.event.GetY() - event.last_y; 00114 00115 if ( event.event.LeftIsDown() && !event.event.ShiftDown() ) 00116 { 00117 yaw( diff_x*0.005 ); 00118 pitch( -diff_y*0.005 ); 00119 } 00120 else if ( event.event.MiddleIsDown() || (event.event.LeftIsDown() && event.event.ShiftDown()) ) 00121 { 00122 // handle mouse movement 00123 int width = event.viewport->getActualWidth(); 00124 int height = event.viewport->getActualHeight(); 00125 00126 Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( 00127 (float) event.event.GetX() / (float) width, (float) event.event.GetY()/ (float) height); 00128 00129 Ogre::Ray last_mouse_ray = 00130 event.viewport->getCamera()->getCameraToViewportRay( 00131 (float) event.last_x / (float) width, (float) event.last_y / (float) height); 00132 00133 Ogre::Vector3 last_intersect, intersect; 00134 00135 if ( intersectGroundPlane( last_mouse_ray, last_intersect ) && intersectGroundPlane( mouse_ray, intersect ) ) 00136 { 00137 Ogre::Vector3 motion = last_intersect - intersect; 00138 00139 // When dragging near the horizon, the motion can get out of 00140 // control. This throttles it to an arbitrary limit per mouse 00141 // event. 00142 float motion_distance_limit = 1; /*meter*/ 00143 if( motion.length() > motion_distance_limit ) 00144 { 00145 motion.normalise(); 00146 motion *= motion_distance_limit; 00147 } 00148 00149 focal_point_ += motion; 00150 } 00151 } 00152 else if ( event.event.RightIsDown() ) 00153 { 00154 zoom( -diff_y * 0.1 * (distance_ / 10.0f) ); 00155 } 00156 00157 moved = true; 00158 } 00159 00160 if ( event.event.GetWheelRotation() != 0 ) 00161 { 00162 int diff = (float)event.event.GetWheelRotation() / (float)event.event.GetWheelDelta(); 00163 zoom( diff * 0.1 * distance_ ); 00164 moved = true; 00165 } 00166 00167 if (moved) 00168 { 00169 manager_->queueRender(); 00170 } 00171 } 00172 00173 void XYOrbitViewController::onActivate() 00174 { 00175 if (camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC) 00176 { 00177 camera_->setProjectionType(Ogre::PT_PERSPECTIVE); 00178 } 00179 else 00180 { 00181 // do some trigonometry 00182 Ogre::Ray camera_dir_ray( camera_->getRealPosition(), camera_->getRealDirection() ); 00183 Ogre::Ray camera_down_ray( camera_->getRealPosition(), -1.0 * camera_->getRealUp() ); 00184 00185 Ogre::Vector3 a,b; 00186 00187 if ( intersectGroundPlane( camera_dir_ray, b ) && 00188 intersectGroundPlane( camera_down_ray, a ) ) 00189 { 00190 float l_a = camera_->getPosition().distance( b ); 00191 float l_b = camera_->getPosition().distance( a ); 00192 00193 distance_ = ( l_a * l_b ) / ( CAMERA_OFFSET * l_a + l_b ); 00194 00195 camera_dir_ray.setOrigin( camera_->getRealPosition() - camera_->getRealUp() * distance_ * CAMERA_OFFSET ); 00196 intersectGroundPlane( camera_dir_ray, focal_point_ ); 00197 00198 ROS_INFO_STREAM( distance_ << " xx " << (camera_->getPosition() - camera_->getUp() * distance_ * CAMERA_OFFSET).distance( focal_point_ ) ); 00199 00200 calculatePitchYawFromPosition( camera_->getPosition() - camera_->getUp() * distance_ * CAMERA_OFFSET ); 00201 } 00202 00203 updateCamera(); 00204 } 00205 } 00206 00207 void XYOrbitViewController::onDeactivate() 00208 { 00209 focal_shape_->getRootNode()->setVisible(false); 00210 camera_->setFixedYawAxis(false); 00211 } 00212 00213 void XYOrbitViewController::onUpdate(float dt, float ros_dt) 00214 { 00215 updateCamera(); 00216 } 00217 00218 void XYOrbitViewController::lookAt( const Ogre::Vector3& point ) 00219 { 00220 Ogre::Vector3 camera_position = camera_->getPosition(); 00221 focal_point_ = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()); 00222 distance_ = focal_point_.distance( camera_position ); 00223 00224 calculatePitchYawFromPosition(camera_position); 00225 } 00226 00227 void XYOrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation) 00228 { 00229 // Compute camera's global coords 00230 Ogre::Vector3 old_camera_pos = old_reference_position + old_reference_orientation * camera_->getPosition(); 00231 // Ogre::Vector3 old_camera_orient = old_reference_orientation * camera_->getOrientation(); 00232 00233 // Compute camera's new coords relative to the reference frame 00234 Ogre::Vector3 camera_position = target_scene_node_->getOrientation().Inverse() * (old_camera_pos - target_scene_node_->getPosition()); 00235 00236 focal_point_ = Ogre::Vector3::ZERO; 00237 distance_ = focal_point_.distance( camera_position ); 00238 00239 calculatePitchYawFromPosition(camera_position); 00240 } 00241 00242 void XYOrbitViewController::normalizePitch() 00243 { 00244 if ( pitch_ < PITCH_LIMIT_LOW ) 00245 { 00246 pitch_ = PITCH_LIMIT_LOW; 00247 } 00248 else if ( pitch_ > PITCH_LIMIT_HIGH ) 00249 { 00250 pitch_ = PITCH_LIMIT_HIGH; 00251 } 00252 } 00253 00254 void XYOrbitViewController::normalizeYaw() 00255 { 00256 yaw_ = fmod( yaw_, Ogre::Math::TWO_PI ); 00257 00258 if ( yaw_ < 0.0f ) 00259 { 00260 yaw_ = Ogre::Math::TWO_PI + yaw_; 00261 } 00262 } 00263 00264 void XYOrbitViewController::updateCamera() 00265 { 00266 float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + focal_point_.x; 00267 float y = distance_ * cos( pitch_ ) + focal_point_.y; 00268 float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + focal_point_.z; 00269 00270 Ogre::Vector3 pos( x, y, z ); 00271 00272 camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Y); 00273 camera_->setDirection(target_scene_node_->getOrientation() * (focal_point_ - pos)); 00274 00275 pos += camera_->getUp() * distance_ * CAMERA_OFFSET; 00276 00277 camera_->setPosition(pos); 00278 00279 // ROS_INFO_STREAM( camera_->getDerivedDirection() ); 00280 00281 focal_shape_->setPosition(focal_point_); 00282 } 00283 00284 void XYOrbitViewController::yaw( float angle ) 00285 { 00286 yaw_ += angle; 00287 00288 normalizeYaw(); 00289 } 00290 00291 void XYOrbitViewController::pitch( float angle ) 00292 { 00293 pitch_ += angle; 00294 00295 normalizePitch(); 00296 } 00297 00298 void XYOrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position ) 00299 { 00300 float x = position.x - focal_point_.x; 00301 float y = position.y - focal_point_.y; 00302 pitch_ = acos( y / distance_ ); 00303 00304 normalizePitch(); 00305 00306 float val = x / ( distance_ * sin( pitch_ ) ); 00307 00308 yaw_ = acos( val ); 00309 00310 Ogre::Vector3 direction = focal_point_ - position; 00311 00312 if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 ) 00313 { 00314 yaw_ = Ogre::Math::TWO_PI - yaw_; 00315 } 00316 } 00317 00318 void XYOrbitViewController::zoom( float amount ) 00319 { 00320 distance_ -= amount; 00321 00322 if ( distance_ <= MIN_DISTANCE ) 00323 { 00324 distance_ = MIN_DISTANCE; 00325 } 00326 } 00327 00328 void XYOrbitViewController::fromString(const std::string& str) 00329 { 00330 std::istringstream iss(str); 00331 00332 iss >> pitch_; 00333 iss.ignore(); 00334 iss >> yaw_; 00335 iss.ignore(); 00336 iss >> distance_; 00337 iss.ignore(); 00338 iss >> focal_point_.x; 00339 iss.ignore(); 00340 iss >> focal_point_.y; 00341 iss.ignore(); 00342 iss >> focal_point_.z; 00343 00344 resetTargetSceneNodePosition(); 00345 } 00346 00347 std::string XYOrbitViewController::toString() 00348 { 00349 std::ostringstream oss; 00350 oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z; 00351 00352 return oss.str(); 00353 } 00354 00355 00356 }