xy_orbit_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 "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_helpers/shape.h>
00044 
00045 #include <stdint.h>
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 // move camera up so the focal point appears in the lower image half
00057 static const float CAMERA_OFFSET = 0.2;
00058 
00059 
00060 XYOrbitViewController::XYOrbitViewController(VisualizationManager* manager, const std::string& name, Ogre::SceneNode* target_scene_node)
00061 : OrbitViewController(manager, name, target_scene_node)
00062 {
00063   focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f);
00064 }
00065 
00066 bool XYOrbitViewController::intersectGroundPlane( Ogre::Ray mouse_ray, Ogre::Vector3 &intersection_3d )
00067 {
00068   //convert rays into reference frame
00069   mouse_ray.setOrigin( target_scene_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00070   mouse_ray.setDirection( target_scene_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00071 
00072   Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0 );
00073 
00074   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(ground_plane);
00075   if (!intersection.first)
00076   {
00077     return false;
00078   }
00079 
00080   intersection_3d = mouse_ray.getPoint(intersection.second);
00081   return true;
00082 }
00083 
00084 void XYOrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
00085 {
00086   bool moved = false;
00087   if( event.type == QEvent::MouseButtonPress )
00088   {
00089     focal_shape_->getRootNode()->setVisible(true);
00090     moved = true;
00091   }
00092   else if( event.type == QEvent::MouseButtonRelease )
00093   {
00094     focal_shape_->getRootNode()->setVisible(false);
00095     moved = true;
00096   }
00097   else if( event.type == QEvent::MouseMove )
00098   {
00099     int32_t diff_x = event.x - event.last_x;
00100     int32_t diff_y = event.y - event.last_y;
00101 
00102     if( diff_x != 0 || diff_y != 0 )
00103     {
00104       if( event.left() && !event.shift() )
00105       {
00106         yaw( diff_x*0.005 );
00107         pitch( -diff_y*0.005 );
00108       }
00109       else if( event.middle() || (event.left() && event.shift()) )
00110       {
00111         // handle mouse movement
00112         int width = event.viewport->getActualWidth();
00113         int height = event.viewport->getActualHeight();
00114 
00115         Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( event.x / (float) width,
00116                                                                                    event.y / (float) height );
00117 
00118         Ogre::Ray last_mouse_ray =
00119           event.viewport->getCamera()->getCameraToViewportRay( event.last_x / (float) width,
00120                                                                event.last_y / (float) height );
00121 
00122         Ogre::Vector3 last_intersect, intersect;
00123 
00124         if( intersectGroundPlane( last_mouse_ray, last_intersect ) &&
00125             intersectGroundPlane( mouse_ray, intersect ))
00126         {
00127           Ogre::Vector3 motion = last_intersect - intersect;
00128 
00129           // When dragging near the horizon, the motion can get out of
00130           // control.  This throttles it to an arbitrary limit per mouse
00131           // event.
00132           float motion_distance_limit = 1; /*meter*/
00133           if( motion.length() > motion_distance_limit )
00134           {
00135             motion.normalise();
00136             motion *= motion_distance_limit;
00137           }
00138 
00139           focal_point_ += motion;
00140           emitConfigChanged();
00141         }
00142       }
00143       else if( event.right() )
00144       {
00145         zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
00146       }
00147 
00148       moved = true;
00149     }
00150   }
00151 
00152   if( event.wheel_delta != 0 )
00153   {
00154     int diff = event.wheel_delta;
00155     zoom( diff * 0.001 * distance_ );
00156     moved = true;
00157   }
00158 
00159   if( moved )
00160   {
00161     manager_->queueRender();
00162   }
00163 }
00164 
00165 void XYOrbitViewController::onActivate()
00166 {
00167   if( camera_->getProjectionType() == Ogre::PT_ORTHOGRAPHIC )
00168   {
00169     camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
00170   }
00171   else
00172   {
00173     // do some trigonometry
00174     Ogre::Ray camera_dir_ray( camera_->getRealPosition(), camera_->getRealDirection() );
00175     Ogre::Ray camera_down_ray( camera_->getRealPosition(), -1.0 * camera_->getRealUp() );
00176 
00177     Ogre::Vector3 a,b;
00178 
00179     if( intersectGroundPlane( camera_dir_ray, b ) &&
00180         intersectGroundPlane( camera_down_ray, a ) )
00181     {
00182       float l_a = camera_->getPosition().distance( b );
00183       float l_b = camera_->getPosition().distance( a );
00184 
00185       distance_ = ( l_a * l_b ) / ( CAMERA_OFFSET * l_a + l_b );
00186 
00187       camera_dir_ray.setOrigin( camera_->getRealPosition() - camera_->getRealUp() * distance_ * CAMERA_OFFSET );
00188       intersectGroundPlane( camera_dir_ray, focal_point_ );
00189 
00190       ROS_INFO_STREAM( distance_ << " xx " << (camera_->getPosition() - camera_->getUp() * distance_ * CAMERA_OFFSET).distance( focal_point_ ) );
00191 
00192       calculatePitchYawFromPosition( camera_->getPosition() - camera_->getUp() * distance_ * CAMERA_OFFSET );
00193     }
00194 
00195     updateCamera();
00196   }
00197 }
00198 
00199 void XYOrbitViewController::updateCamera()
00200 {
00201   OrbitViewController::updateCamera();
00202   camera_->setPosition( camera_->getPosition() + camera_->getUp() * distance_ * CAMERA_OFFSET );
00203 }
00204 
00205 void XYOrbitViewController::lookAt( const Ogre::Vector3& point )
00206 {
00207   Ogre::Vector3 camera_position = camera_->getPosition();
00208   focal_point_ = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition());
00209   focal_point_.z = 0;
00210   distance_ = focal_point_.distance( camera_position );
00211 
00212   calculatePitchYawFromPosition(camera_position);
00213 }
00214 
00215 }


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