00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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
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
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
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
00130
00131
00132 float motion_distance_limit = 1;
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
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 }