Go to the documentation of this file.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 <stdint.h>
00031
00032 #include <ros/ros.h>
00033
00034 #include <OgreCamera.h>
00035 #include <OgrePlane.h>
00036 #include <OgreQuaternion.h>
00037 #include <OgreRay.h>
00038 #include <OgreSceneNode.h>
00039 #include <OgreVector3.h>
00040 #include <OgreViewport.h>
00041 #include <OgreMath.h>
00042
00043 #include "rviz/display_context.h"
00044 #include "rviz/ogre_helpers/shape.h"
00045 #include "rviz/properties/float_property.h"
00046 #include "rviz/properties/vector_property.h"
00047 #include "rviz/viewport_mouse_event.h"
00048
00049 #include "rviz/default_plugin/view_controllers/third_person_follower_view_controller.h"
00050
00051 namespace rviz
00052 {
00053
00054
00055 static const float CAMERA_OFFSET = 0.2;
00056
00057 void ThirdPersonFollowerViewController::onInitialize()
00058 {
00059 OrbitViewController::onInitialize();
00060 focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f);
00061 }
00062
00063 bool ThirdPersonFollowerViewController::intersectGroundPlane( Ogre::Ray mouse_ray, Ogre::Vector3 &intersection_3d )
00064 {
00065
00066 mouse_ray.setOrigin( target_scene_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00067 mouse_ray.setDirection( target_scene_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00068
00069 Ogre::Plane ground_plane( Ogre::Vector3::UNIT_Z, 0 );
00070
00071 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(ground_plane);
00072 if (!intersection.first)
00073 {
00074 return false;
00075 }
00076
00077 intersection_3d = mouse_ray.getPoint(intersection.second);
00078 return true;
00079 }
00080
00081 void ThirdPersonFollowerViewController::handleMouseEvent(ViewportMouseEvent& event)
00082 {
00083 if ( event.shift() )
00084 {
00085 setStatus( "<b>Left-Click:</b> Move X/Y. <b>Right-Click:</b>: Zoom." );
00086 }
00087 else
00088 {
00089 setStatus( "<b>Left-Click:</b> Rotate. <b>Middle-Click:</b> Move X/Y. <b>Right-Click:</b>: Move Z. <b>Shift</b>: More options." );
00090 }
00091
00092 int32_t diff_x = 0;
00093 int32_t diff_y = 0;
00094
00095 bool moved = false;
00096 if( event.type == QEvent::MouseButtonPress )
00097 {
00098 focal_shape_->getRootNode()->setVisible(true);
00099 moved = true;
00100 }
00101 else if( event.type == QEvent::MouseButtonRelease )
00102 {
00103 focal_shape_->getRootNode()->setVisible(false);
00104 moved = true;
00105 }
00106 else if( event.type == QEvent::MouseMove )
00107 {
00108 diff_x = event.x - event.last_x;
00109 diff_y = event.y - event.last_y;
00110 moved = true;
00111 }
00112
00113 if( event.left() && !event.shift() )
00114 {
00115 setCursor( Rotate3D );
00116 yaw( diff_x*0.005 );
00117 pitch( -diff_y*0.005 );
00118 }
00119 else if( event.middle() || (event.left() && event.shift()) )
00120 {
00121 setCursor( MoveXY );
00122
00123 int width = event.viewport->getActualWidth();
00124 int height = event.viewport->getActualHeight();
00125
00126 Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( event.x / (float) width,
00127 event.y / (float) height );
00128
00129 Ogre::Ray last_mouse_ray =
00130 event.viewport->getCamera()->getCameraToViewportRay( event.last_x / (float) width,
00131 event.last_y / (float) height );
00132
00133 Ogre::Vector3 last_intersect, intersect;
00134
00135 if( intersectGroundPlane( last_mouse_ray, last_intersect ) &&
00136 intersectGroundPlane( mouse_ray, intersect ))
00137 {
00138 Ogre::Vector3 motion = last_intersect - intersect;
00139
00140
00141
00142
00143 float motion_distance_limit = 1;
00144 if( motion.length() > motion_distance_limit )
00145 {
00146 motion.normalise();
00147 motion *= motion_distance_limit;
00148 }
00149
00150 focal_point_property_->add( motion );
00151 emitConfigChanged();
00152 }
00153 }
00154 else if( event.right() )
00155 {
00156 setCursor( Zoom );
00157 zoom( -diff_y * 0.1 * (distance_property_->getFloat() / 10.0f) );
00158 }
00159 else
00160 {
00161 setCursor( event.shift() ? MoveXY : Rotate3D );
00162 }
00163
00164 if( event.wheel_delta != 0 )
00165 {
00166 int diff = event.wheel_delta;
00167 zoom( diff * 0.001 * distance_property_->getFloat() );
00168 moved = true;
00169 }
00170
00171 if( moved )
00172 {
00173 context_->queueRender();
00174 }
00175 }
00176
00177 void ThirdPersonFollowerViewController::mimic( ViewController* source_view )
00178 {
00179 FramePositionTrackingViewController::mimic( source_view );
00180
00181 Ogre::Camera* source_camera = source_view->getCamera();
00182
00183 Ogre::Ray camera_dir_ray( source_camera->getRealPosition(), source_camera->getRealDirection() );
00184 Ogre::Ray camera_down_ray( source_camera->getRealPosition(), -1.0 * source_camera->getRealUp() );
00185
00186 Ogre::Vector3 a,b;
00187
00188 if( intersectGroundPlane( camera_dir_ray, b ) &&
00189 intersectGroundPlane( camera_down_ray, a ) )
00190 {
00191 float l_a = source_camera->getPosition().distance( b );
00192 float l_b = source_camera->getPosition().distance( a );
00193
00194 distance_property_->setFloat(( l_a * l_b ) / ( CAMERA_OFFSET * l_a + l_b ));
00195 float distance = distance_property_->getFloat();
00196
00197 camera_dir_ray.setOrigin( source_camera->getRealPosition() - source_camera->getRealUp() * distance * CAMERA_OFFSET );
00198 Ogre::Vector3 new_focal_point;
00199 intersectGroundPlane( camera_dir_ray, new_focal_point );
00200 focal_point_property_->setVector( new_focal_point );
00201
00202 calculatePitchYawFromPosition( source_camera->getPosition() - source_camera->getUp() * distance * CAMERA_OFFSET );
00203 }
00204 }
00205
00206 void ThirdPersonFollowerViewController::updateCamera()
00207 {
00208 OrbitViewController::updateCamera();
00209 camera_->setPosition( camera_->getPosition() + camera_->getUp() * distance_property_->getFloat() * CAMERA_OFFSET );
00210 }
00211
00212 void ThirdPersonFollowerViewController::updateTargetSceneNode()
00213 {
00214 if ( FramePositionTrackingViewController::getNewTransform() )
00215 {
00216 target_scene_node_->setPosition( reference_position_ );
00217 Ogre::Radian ref_yaw = reference_orientation_.getRoll( false );
00218 Ogre::Quaternion ref_yaw_quat(Ogre::Math::Cos(ref_yaw/2), 0, 0, Ogre::Math::Sin(ref_yaw/2));
00219 target_scene_node_->setOrientation( ref_yaw_quat );
00220
00221 context_->queueRender();
00222 }
00223 }
00224
00225 void ThirdPersonFollowerViewController::lookAt( const Ogre::Vector3& point )
00226 {
00227 Ogre::Vector3 camera_position = camera_->getPosition();
00228 Ogre::Vector3 new_focal_point = target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition());
00229 new_focal_point.z = 0;
00230 distance_property_->setFloat( new_focal_point.distance( camera_position ));
00231 focal_point_property_->setVector( new_focal_point );
00232
00233 calculatePitchYawFromPosition(camera_position);
00234 }
00235
00236 }
00237
00238 #include <pluginlib/class_list_macros.h>
00239 PLUGINLIB_EXPORT_CLASS( rviz::ThirdPersonFollowerViewController, rviz::ViewController )