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 <OGRE/OgreViewport.h>
00031 #include <OGRE/OgreCamera.h>
00032 #include <OGRE/OgreSceneNode.h>
00033 #include <OGRE/OgreSceneManager.h>
00034 #include <OGRE/OgrePass.h>
00035 #include <OGRE/OgreMaterial.h>
00036 #include <OGRE/OgreEntity.h>
00037 #include <OGRE/OgreSubEntity.h>
00038
00039 #include "rviz/display_context.h"
00040 #include "rviz/selection/selection_manager.h"
00041 #include "rviz/render_panel.h"
00042 #include "rviz/load_resource.h"
00043 #include "rviz/window_manager_interface.h"
00044 #include "rviz/geometry.h"
00045 #include "rviz/frame_manager.h"
00046
00047 #include "rviz/ogre_helpers/line.h"
00048
00049 #include "rviz/default_plugin/markers/shape_marker.h"
00050 #include "rviz/default_plugin/markers/arrow_marker.h"
00051 #include "rviz/default_plugin/markers/line_list_marker.h"
00052 #include "rviz/default_plugin/markers/line_strip_marker.h"
00053 #include "rviz/default_plugin/markers/points_marker.h"
00054 #include "rviz/default_plugin/markers/text_view_facing_marker.h"
00055 #include "rviz/default_plugin/markers/mesh_resource_marker.h"
00056 #include "rviz/default_plugin/markers/triangle_list_marker.h"
00057 #include "rviz/default_plugin/markers/marker_base.h"
00058
00059 #include "rviz/default_plugin/interactive_markers/interactive_marker_control.h"
00060 #include "rviz/default_plugin/interactive_markers/interactive_marker.h"
00061
00062 #define NO_HIGHLIGHT_VALUE 0.0
00063 #define ACTIVE_HIGHLIGHT_VALUE 0.5
00064 #define HOVER_HIGHLIGHT_VALUE 0.3
00065
00066 namespace rviz
00067 {
00068
00069 InteractiveMarkerControl::InteractiveMarkerControl( DisplayContext* context,
00070 Ogre::SceneNode *reference_node,
00071 InteractiveMarker *parent )
00072 : mouse_dragging_(false)
00073 , drag_viewport_( NULL )
00074 , context_( context )
00075 , reference_node_(reference_node)
00076 , control_frame_node_(reference_node_->createChildSceneNode())
00077 , markers_node_(reference_node_->createChildSceneNode())
00078 , parent_(parent)
00079 , rotation_(0)
00080 , grab_point_in_reference_frame_(0,0,0)
00081 , interaction_enabled_(false)
00082 , visible_(true)
00083 , view_facing_( false )
00084 , mouse_down_(false)
00085 , show_visual_aids_(false)
00086 , line_(new Line(context->getSceneManager(),control_frame_node_))
00087 {
00088 line_->setVisible(false);
00089 }
00090
00091 void InteractiveMarkerControl::makeMarkers( const visualization_msgs::InteractiveMarkerControl& message )
00092 {
00093 for (unsigned i = 0; i < message.markers.size(); i++)
00094 {
00095 MarkerBasePtr marker;
00096
00097
00098 switch (message.markers[i].type)
00099 {
00100 case visualization_msgs::Marker::CUBE:
00101 case visualization_msgs::Marker::CYLINDER:
00102 case visualization_msgs::Marker::SPHERE:
00103 {
00104 marker.reset(new ShapeMarker(0, context_, markers_node_));
00105 }
00106 break;
00107
00108 case visualization_msgs::Marker::ARROW:
00109 {
00110 marker.reset(new ArrowMarker(0, context_, markers_node_));
00111 }
00112 break;
00113
00114 case visualization_msgs::Marker::LINE_STRIP:
00115 {
00116 marker.reset(new LineStripMarker(0, context_, markers_node_));
00117 }
00118 break;
00119 case visualization_msgs::Marker::LINE_LIST:
00120 {
00121 marker.reset(new LineListMarker(0, context_, markers_node_));
00122 }
00123 break;
00124 case visualization_msgs::Marker::SPHERE_LIST:
00125 case visualization_msgs::Marker::CUBE_LIST:
00126 case visualization_msgs::Marker::POINTS:
00127 {
00128 PointsMarkerPtr points_marker;
00129 points_marker.reset(new PointsMarker(0, context_, markers_node_));
00130 points_markers_.push_back( points_marker );
00131 marker = points_marker;
00132 }
00133 break;
00134 case visualization_msgs::Marker::TEXT_VIEW_FACING:
00135 {
00136 marker.reset(new TextViewFacingMarker(0, context_, markers_node_));
00137 }
00138 break;
00139 case visualization_msgs::Marker::MESH_RESOURCE:
00140 {
00141 marker.reset(new MeshResourceMarker(0, context_, markers_node_));
00142 }
00143 break;
00144
00145 case visualization_msgs::Marker::TRIANGLE_LIST:
00146 {
00147 marker.reset(new TriangleListMarker(0, context_, markers_node_));
00148 }
00149 break;
00150 default:
00151 ROS_ERROR( "Unknown marker type: %d", message.markers[i].type );
00152 break;
00153 }
00154
00155 visualization_msgs::MarkerPtr marker_msg( new visualization_msgs::Marker(message.markers[ i ]) );
00156
00157 if ( marker_msg->header.frame_id.empty() )
00158 {
00159
00160
00161
00162 marker_msg->header.frame_id = context_->getFrameManager()->getFixedFrame();
00163 marker->setMessage( marker_msg );
00164 }
00165 else
00166 {
00167 marker->setMessage( marker_msg );
00168
00169
00170
00171 marker->setPosition( markers_node_->convertWorldToLocalPosition( marker->getPosition() ) );
00172 marker->setOrientation( markers_node_->convertWorldToLocalOrientation( marker->getOrientation() ) );
00173 }
00174 marker->setInteractiveObject( shared_from_this() );
00175
00176 addHighlightPass(marker->getMaterials());
00177
00178 markers_.push_back(marker);
00179 }
00180 }
00181
00182 InteractiveMarkerControl::~InteractiveMarkerControl()
00183 {
00184 context_->getSceneManager()->destroySceneNode(control_frame_node_);
00185 context_->getSceneManager()->destroySceneNode(markers_node_);
00186
00187 if( view_facing_ )
00188 {
00189 context_->getSceneManager()->removeListener(this);
00190 }
00191 }
00192
00193 void InteractiveMarkerControl::processMessage( const visualization_msgs::InteractiveMarkerControl &message )
00194 {
00195 name_ = message.name;
00196 description_ = QString::fromStdString( message.description );
00197 interaction_mode_ = message.interaction_mode;
00198 always_visible_ = message.always_visible;
00199 orientation_mode_ = message.orientation_mode;
00200
00201 control_orientation_ = Ogre::Quaternion(message.orientation.w,
00202 message.orientation.x, message.orientation.y, message.orientation.z);
00203 control_orientation_.normalise();
00204
00205 bool new_view_facingness = (message.orientation_mode == visualization_msgs::InteractiveMarkerControl::VIEW_FACING);
00206 if( new_view_facingness != view_facing_ )
00207 {
00208 if( new_view_facingness )
00209 {
00210 context_->getSceneManager()->addListener(this);
00211 }
00212 else
00213 {
00214 context_->getSceneManager()->removeListener(this);
00215 }
00216 view_facing_ = new_view_facingness;
00217 }
00218
00219 independent_marker_orientation_ = message.independent_marker_orientation;
00220
00221
00222
00223 highlight_passes_.clear();
00224 markers_.clear();
00225 points_markers_.clear();
00226
00227
00228
00229 control_frame_node_->setPosition(parent_->getPosition());
00230 markers_node_->setPosition(parent_->getPosition());
00231
00232 if ( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::INHERIT )
00233 {
00234 control_frame_node_->setOrientation(parent_->getOrientation());
00235 markers_node_->setOrientation(parent_->getOrientation());
00236 }
00237 else
00238 {
00239 control_frame_node_->setOrientation( Ogre::Quaternion::IDENTITY );
00240 markers_node_->setOrientation( Ogre::Quaternion::IDENTITY );
00241 }
00242
00243 makeMarkers( message );
00244
00245 status_msg_ = description_+" ";
00246
00247 Ogre::Vector3 control_dir = control_orientation_.xAxis()*10000.0;
00248 line_->setPoints( control_dir, -1*control_dir );
00249 line_->setVisible(false);
00250
00251
00252 switch( interaction_mode_ )
00253 {
00254 case visualization_msgs::InteractiveMarkerControl::NONE:
00255 cursor_ = rviz::getDefaultCursor();
00256 break;
00257 case visualization_msgs::InteractiveMarkerControl::MENU:
00258 cursor_ = rviz::makeIconCursor( "package://rviz/icons/menu.svg" );
00259 status_msg_ += "<b>Left-Click:</b> Show menu.";
00260 break;
00261 case visualization_msgs::InteractiveMarkerControl::BUTTON:
00262 cursor_ = rviz::getDefaultCursor();
00263 status_msg_ += "<b>Left-Click:</b> Activate. ";
00264 break;
00265 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00266 cursor_ = rviz::makeIconCursor( "package://rviz/icons/move1d.svg" );
00267 status_msg_ += "<b>Left-Click:</b> Move. ";
00268 break;
00269 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00270 cursor_ = rviz::makeIconCursor( "package://rviz/icons/move2d.svg" );
00271 status_msg_ += "<b>Left-Click:</b> Move. ";
00272 break;
00273 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00274 cursor_ = rviz::makeIconCursor( "package://rviz/icons/rotate.svg" );
00275 status_msg_ += "<b>Left-Click:</b> Rotate. ";
00276 break;
00277 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00278 cursor_ = rviz::makeIconCursor( "package://rviz/icons/moverotate.svg" );
00279 status_msg_ += "<b>Left-Click:</b> Move / Rotate. ";
00280 break;
00281 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
00282 cursor_ = rviz::makeIconCursor( "package://rviz/icons/move2d.svg" );
00283 status_msg_ += "<b>Left-Click:</b> Move X/Y. <b>Shift + Left-Click / Left-Click + Wheel:</b> Move Z. ";
00284 break;
00285 case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
00286 cursor_ = rviz::makeIconCursor( "package://rviz/icons/rotate.svg" );
00287 status_msg_ += "<b>Left-Click:</b> Rotate around X/Y. <b>Shift-Left-Click:</b> Rotate around Z. ";
00288 break;
00289 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
00290 cursor_ = rviz::makeIconCursor( "package://rviz/icons/moverotate.svg" );
00291 status_msg_ += "<b>Left-Click:</b> Move X/Y. <b>Shift + Left-Click / Left-Click + Wheel:</b> Move Z. <b>Ctrl + Left-Click:</b> Rotate around X/Y. <b>Ctrl + Shift + Left-Click:</b> Rotate around Z. ";
00292 break;
00293 }
00294
00295 if ( parent_->hasMenu() && interaction_mode_ != visualization_msgs::InteractiveMarkerControl::MENU )
00296 {
00297 status_msg_ += "<b>Right-Click:</b> Show context menu.";
00298 }
00299
00300
00301
00302
00303
00304 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00305 independent_marker_orientation_ )
00306 {
00307 markers_node_->setOrientation( parent_->getOrientation() );
00308 }
00309
00310 enableInteraction(context_->getSelectionManager()->getInteractionEnabled());
00311 }
00312
00313
00314
00315 void InteractiveMarkerControl::preFindVisibleObjects(
00316 Ogre::SceneManager *source,
00317 Ogre::SceneManager::IlluminationRenderStage irs, Ogre::Viewport *v )
00318 {
00319 updateControlOrientationForViewFacing( v );
00320 }
00321
00322 void InteractiveMarkerControl::updateControlOrientationForViewFacing( Ogre::Viewport* v )
00323 {
00324 Ogre::Quaternion x_view_facing_rotation =
00325 control_orientation_.xAxis().getRotationTo( v->getCamera()->getDerivedDirection());
00326
00327
00328 Ogre::Vector3 z_axis_2 = x_view_facing_rotation * control_orientation_.zAxis();
00329 Ogre::Quaternion align_yz_rotation = z_axis_2.getRotationTo(v->getCamera()->getDerivedUp());
00330
00331
00332 Ogre::Quaternion rotate_around_x = Ogre::Quaternion(rotation_, v->getCamera()->getDerivedDirection());
00333
00334 Ogre::Quaternion rotation = reference_node_->convertWorldToLocalOrientation(
00335 rotate_around_x * align_yz_rotation * x_view_facing_rotation );
00336
00337 control_frame_node_->setOrientation( rotation );
00338
00339 if ( !independent_marker_orientation_ )
00340 {
00341 markers_node_->setOrientation( rotation );
00342
00343
00344 markers_node_->_update(true, false);
00345 }
00346 }
00347
00348 bool InteractiveMarkerControl::getVisible()
00349 {
00350 return visible_ || always_visible_;
00351 }
00352
00353 void InteractiveMarkerControl::setVisible( bool visible )
00354 {
00355 visible_ = visible;
00356
00357 if (always_visible_)
00358 {
00359 markers_node_->setVisible(visible_);
00360 } else
00361 {
00362 markers_node_->setVisible(interaction_enabled_ && visible_);
00363 }
00364 }
00365
00366 void InteractiveMarkerControl::update()
00367 {
00368 if( mouse_dragging_ )
00369 {
00370 handleMouseMovement( dragging_in_place_event_ );
00371 }
00372 }
00373
00374 void InteractiveMarkerControl::enableInteraction( bool enable )
00375 {
00376 if (mouse_down_)
00377 {
00378 return;
00379 }
00380
00381 interaction_enabled_ = enable;
00382 setVisible(visible_);
00383 if (!enable)
00384 {
00385 setHighlight(NO_HIGHLIGHT_VALUE);
00386 }
00387 }
00388
00389 void InteractiveMarkerControl::interactiveMarkerPoseChanged(
00390 Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation )
00391 {
00392 control_frame_node_->setPosition(int_marker_position);
00393 markers_node_->setPosition(int_marker_position);
00394
00395 switch (orientation_mode_)
00396 {
00397 case visualization_msgs::InteractiveMarkerControl::INHERIT:
00398 control_frame_node_->setOrientation(int_marker_orientation);
00399 markers_node_->setOrientation(control_frame_node_->getOrientation());
00400 break;
00401
00402 case visualization_msgs::InteractiveMarkerControl::FIXED:
00403 {
00404 control_frame_node_->setOrientation( Ogre::Quaternion( rotation_, control_orientation_.xAxis() ));
00405 markers_node_->setOrientation(control_frame_node_->getOrientation());
00406 break;
00407 }
00408
00409 case visualization_msgs::InteractiveMarkerControl::VIEW_FACING:
00410 if( drag_viewport_ )
00411 {
00412 updateControlOrientationForViewFacing( drag_viewport_ );
00413 }
00414 if ( independent_marker_orientation_ )
00415 {
00416 markers_node_->setOrientation(int_marker_orientation);
00417 }
00418 break;
00419
00420 default:
00421 break;
00422 }
00423 }
00424
00425 Ogre::Vector3 InteractiveMarkerControl::closestPointOnLineToPoint( const Ogre::Vector3& line_start,
00426 const Ogre::Vector3& line_dir,
00427 const Ogre::Vector3& test_point )
00428 {
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438 double factor = ( test_point - line_start ).dotProduct( line_dir ) / line_dir.dotProduct( line_dir );
00439 Ogre::Vector3 closest_point = line_start + line_dir * factor;
00440 return closest_point;
00441 }
00442
00443 void InteractiveMarkerControl::rotate( Ogre::Ray &mouse_ray )
00444 {
00445 Ogre::Vector3 intersection_3d;
00446 Ogre::Vector2 intersection_2d;
00447 float ray_t;
00448
00449 Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
00450
00451
00452
00453 Ogre::Vector3 rotation_center = closestPointOnLineToPoint( control_frame_node_->getPosition(),
00454 rotation_axis,
00455 grab_point_in_reference_frame_ );
00456
00457
00458 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation_at_mouse_down_,
00459 intersection_3d, intersection_2d, ray_t ))
00460 {
00461
00462 rotate(intersection_3d);
00463 }
00464 }
00465
00466 void InteractiveMarkerControl::rotate( const Ogre::Vector3& cursor_in_reference_frame )
00467 {
00468 Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
00469
00470
00471
00472 Ogre::Vector3 rotation_center = closestPointOnLineToPoint( control_frame_node_->getPosition(),
00473 rotation_axis,
00474 grab_point_in_reference_frame_ );
00475
00476
00477 Ogre::Vector3 grab_rel_center = grab_point_in_reference_frame_ - rotation_center;
00478 Ogre::Vector3 center_to_cursor = cursor_in_reference_frame - rotation_center;
00479 Ogre::Vector3 center_to_cursor_radial = center_to_cursor - center_to_cursor.dotProduct(rotation_axis)*rotation_axis;
00480
00481 Ogre::Quaternion orientation_change_since_mouse_down =
00482 grab_rel_center.getRotationTo( center_to_cursor_radial, rotation_axis );
00483
00484 Ogre::Radian rot;
00485 Ogre::Vector3 axis;
00486 orientation_change_since_mouse_down.ToAngleAxis( rot, axis );
00487
00488
00489
00490
00491
00492 Ogre::Radian rotation_since_mouse_down = rot * axis.dotProduct( rotation_axis );
00493
00494 rotation_ = rotation_at_mouse_down_ + rotation_since_mouse_down;
00495 parent_->setPose( parent_->getPosition(),
00496 orientation_change_since_mouse_down * parent_orientation_at_mouse_down_,
00497 name_ );
00498 }
00499
00500 Ogre::Ray InteractiveMarkerControl::getMouseRayInReferenceFrame( const ViewportMouseEvent& event, int x, int y )
00501 {
00502 float width = event.viewport->getActualWidth() - 1;
00503 float height = event.viewport->getActualHeight() - 1;
00504
00505 Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (x + .5) / width,
00506 (y + .5) / height );
00507
00508
00509 mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00510 mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00511
00512 return mouse_ray;
00513 }
00514
00515 void InteractiveMarkerControl::beginRelativeMouseMotion( const ViewportMouseEvent& event )
00516 {
00517 mouse_x_at_drag_begin_ = event.x;
00518 mouse_y_at_drag_begin_ = event.y;
00519 modifiers_at_drag_begin_ = event.modifiers;
00520
00521 mouse_ray_at_drag_begin_ = getMouseRayInReferenceFrame( event, event.x, event.y );
00522
00523
00524 mouse_ray_at_drag_begin_.setDirection(mouse_ray_at_drag_begin_.getDirection().normalisedCopy());
00525 }
00526
00527 bool InteractiveMarkerControl::getRelativeMouseMotion( const ViewportMouseEvent& event, int& dx, int& dy )
00528 {
00529 dx = event.x - mouse_x_at_drag_begin_;
00530 dy = event.y - mouse_y_at_drag_begin_;
00531 if (dx == 0 && dy == 0)
00532 return false;
00533
00534 QCursor::setPos( mouse_x_at_drag_begin_ + mouse_relative_to_absolute_x_,
00535 mouse_y_at_drag_begin_ + mouse_relative_to_absolute_y_ );
00536 return true;
00537 }
00538
00539 void InteractiveMarkerControl::rotateXYRelative( const ViewportMouseEvent& event )
00540 {
00541 int dx;
00542 int dy;
00543
00544 if (!getRelativeMouseMotion( event, dx, dy ))
00545 return;
00546
00547 static const double MOUSE_SCALE = 2 * 3.14 / 300;
00548 Ogre::Radian rx(dx * MOUSE_SCALE);
00549 Ogre::Radian ry(dy * MOUSE_SCALE);
00550
00551 Ogre::Quaternion up_rot(rx, event.viewport->getCamera()->getRealUp());
00552 Ogre::Quaternion right_rot(ry, event.viewport->getCamera()->getRealRight());
00553
00554 parent_->setPose( parent_->getPosition(),
00555 up_rot * right_rot * parent_->getOrientation(),
00556 name_ );
00557 }
00558
00559 void InteractiveMarkerControl::rotateZRelative( const ViewportMouseEvent& event )
00560 {
00561 int dx;
00562 int dy;
00563
00564 getRelativeMouseMotion( event, dx, dy );
00565 if (std::abs(dy) > std::abs(dx))
00566 dx = dy;
00567 if (dx == 0)
00568 return;
00569
00570 static const double MOUSE_SCALE = 2 * 3.14 / 300;
00571 Ogre::Radian rx(dx * MOUSE_SCALE);
00572
00573 Ogre::Quaternion rot(rx, event.viewport->getCamera()->getRealDirection());
00574
00575 parent_->setPose( parent_->getPosition(),
00576 rot * parent_->getOrientation(),
00577 name_ );
00578 }
00579
00580 void InteractiveMarkerControl::moveZAxisRelative( const ViewportMouseEvent& event )
00581 {
00582 int dx;
00583 int dy;
00584
00585 getRelativeMouseMotion( event, dx, dy );
00586 if (std::abs(dx) > std::abs(dy))
00587 dy = -dx;
00588 if (dy == 0)
00589 return;
00590
00591 double distance = -dy * mouse_z_scale_;
00592 Ogre::Vector3 delta = distance * mouse_ray_at_drag_begin_.getDirection();
00593
00594 parent_->setPose( parent_->getPosition() + delta,
00595 parent_->getOrientation(),
00596 name_ );
00597
00598 parent_position_at_mouse_down_ = parent_->getPosition();
00599 }
00600
00601 void InteractiveMarkerControl::moveZAxisWheel( const ViewportMouseEvent& event )
00602 {
00603
00604 static const double WHEEL_TO_PIXEL_SCALE = (1.0/8.0) * (2.0/15.0);
00605
00606 double distance = event.wheel_delta * WHEEL_TO_PIXEL_SCALE;
00607 Ogre::Vector3 delta = distance * mouse_ray_at_drag_begin_.getDirection();
00608
00609 parent_->setPose( parent_->getPosition() + delta,
00610 parent_->getOrientation(),
00611 name_ );
00612
00613 parent_position_at_mouse_down_ = parent_->getPosition();
00614 }
00615
00616 void InteractiveMarkerControl::moveViewPlane( Ogre::Ray &mouse_ray, const ViewportMouseEvent& event )
00617 {
00618
00619 Ogre::Plane plane( event.viewport->getCamera()->getRealDirection(),
00620 parent_position_at_mouse_down_ + parent_to_grab_position_);
00621
00622
00623 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
00624 if (!intersection.first)
00625 return;
00626 Ogre::Vector3 mouse_position_on_plane = mouse_ray.getPoint(intersection.second);
00627
00628
00629 parent_->setPose( mouse_position_on_plane - parent_to_grab_position_,
00630 parent_->getOrientation(),
00631 name_ );
00632 }
00633
00634 void InteractiveMarkerControl::movePlane( Ogre::Ray &mouse_ray )
00635 {
00636 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00637 drag_viewport_ )
00638 {
00639 updateControlOrientationForViewFacing( drag_viewport_ );
00640 }
00641
00642 Ogre::Vector3 intersection_3d;
00643 Ogre::Vector2 intersection_2d;
00644 float ray_t;
00645
00646 if( intersectSomeYzPlane( mouse_ray, grab_point_in_reference_frame_, control_frame_node_->getOrientation(),
00647 intersection_3d, intersection_2d, ray_t ))
00648 {
00649 parent_->setPose( intersection_3d - grab_point_in_reference_frame_ + parent_position_at_mouse_down_, parent_->getOrientation(), name_ );
00650 }
00651 }
00652
00653 void InteractiveMarkerControl::movePlane( const Ogre::Vector3& cursor_position_in_reference_frame )
00654 {
00655 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00656 drag_viewport_ )
00657 {
00658 updateControlOrientationForViewFacing( drag_viewport_ );
00659 }
00660
00661 Ogre::Vector3 plane_normal = (control_frame_node_->getOrientation() * control_orientation_.xAxis());
00662 Ogre::Vector3 displacement = (cursor_position_in_reference_frame - grab_point_in_reference_frame_);
00663 Ogre::Vector3 displacement_on_plane = displacement - displacement.dotProduct(plane_normal) * plane_normal;
00664
00665
00666 parent_->setPose( parent_position_at_mouse_down_ + displacement_on_plane,
00667 parent_->getOrientation(), name_ );
00668 }
00669
00672 void InteractiveMarkerControl::worldToScreen( const Ogre::Vector3& pos_rel_reference,
00673 const Ogre::Viewport* viewport,
00674 Ogre::Vector2& screen_pos )
00675 {
00676 Ogre::Vector3 world_pos = reference_node_->convertLocalToWorldPosition( pos_rel_reference );
00677
00678 const Ogre::Camera* cam = viewport->getCamera();
00679 Ogre::Vector3 homogeneous_screen_position = cam->getProjectionMatrix() * (cam->getViewMatrix() * world_pos);
00680
00681 double half_width = viewport->getActualWidth() / 2.0;
00682 double half_height = viewport->getActualHeight() / 2.0;
00683
00684 screen_pos.x = half_width + (half_width * homogeneous_screen_position.x) - .5;
00685 screen_pos.y = half_height + (half_height * -homogeneous_screen_position.y) - .5;
00686 }
00687
00691 bool InteractiveMarkerControl::findClosestPoint( const Ogre::Ray& target_ray,
00692 const Ogre::Ray& mouse_ray,
00693 Ogre::Vector3& closest_point )
00694 {
00695
00696
00697
00698
00699
00700
00701 Ogre::Vector3 v13 = target_ray.getOrigin() - mouse_ray.getOrigin();
00702 Ogre::Vector3 v43 = mouse_ray.getDirection();
00703 Ogre::Vector3 v21 = target_ray.getDirection();
00704 double d1343 = v13.dotProduct( v43 );
00705 double d4321 = v43.dotProduct( v21 );
00706 double d1321 = v13.dotProduct( v21 );
00707 double d4343 = v43.dotProduct( v43 );
00708 double d2121 = v21.dotProduct( v21 );
00709
00710 double denom = d2121 * d4343 - d4321 * d4321;
00711 if( fabs( denom ) <= Ogre::Matrix3::EPSILON )
00712 {
00713 return false;
00714 }
00715 double numer = d1343 * d4321 - d1321 * d4343;
00716
00717 double mua = numer / denom;
00718 closest_point = target_ray.getPoint( mua );
00719 return true;
00720 }
00721
00722 void InteractiveMarkerControl::moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event )
00723 {
00724
00725 Ogre::Ray control_ray;
00726 control_ray.setOrigin( grab_point_in_reference_frame_ );
00727 control_ray.setDirection( control_frame_node_->getOrientation() * control_orientation_.xAxis() );
00728
00729
00730 Ogre::Vector2 control_ray_screen_start, control_ray_screen_end;
00731 worldToScreen( control_ray.getOrigin(), event.viewport, control_ray_screen_start );
00732 worldToScreen( control_ray.getPoint( 1 ), event.viewport, control_ray_screen_end );
00733
00734 Ogre::Vector2 mouse_point( event.x, event.y );
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745 Ogre::Vector2 control_ray_screen_dir = control_ray_screen_end - control_ray_screen_start;
00746 double denominator = control_ray_screen_dir.dotProduct( control_ray_screen_dir );
00747 if( fabs( denominator ) > Ogre::Matrix3::EPSILON )
00748 {
00749 double factor =
00750 ( mouse_point - control_ray_screen_start ).dotProduct( control_ray_screen_dir ) / denominator;
00751
00752 Ogre::Vector2 closest_screen_point = control_ray_screen_start + control_ray_screen_dir * factor;
00753
00754
00755 Ogre::Ray new_mouse_ray = getMouseRayInReferenceFrame( event, closest_screen_point.x, closest_screen_point.y );
00756
00757
00758 Ogre::Vector3 closest_point;
00759 if( findClosestPoint( control_ray, new_mouse_ray, closest_point ))
00760 {
00761
00762 parent_->setPose( closest_point - grab_point_in_reference_frame_ + parent_position_at_mouse_down_,
00763 parent_->getOrientation(), name_ );
00764 }
00765 }
00766 }
00767
00768 void InteractiveMarkerControl::moveAxis( const Ogre::Vector3& cursor_position_in_reference_frame )
00769 {
00770 Ogre::Vector3 control_unit_direction = (control_frame_node_->getOrientation() * control_orientation_.xAxis());
00771 Ogre::Vector3 displacement_along_axis = (cursor_position_in_reference_frame - grab_point_in_reference_frame_).dotProduct(control_unit_direction) * control_unit_direction;
00772
00773
00774 parent_->setPose( parent_position_at_mouse_down_ + displacement_along_axis,
00775 parent_->getOrientation(), name_ );
00776
00777 }
00778
00779 void InteractiveMarkerControl::moveRotate( Ogre::Ray &mouse_ray )
00780 {
00781 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00782 drag_viewport_ )
00783 {
00784 updateControlOrientationForViewFacing( drag_viewport_ );
00785 }
00786
00787 Ogre::Vector3 new_drag_rel_ref;
00788 Ogre::Vector2 intersection_2d;
00789 float ray_t;
00790
00791
00792
00793
00794
00795
00796
00797 Ogre::Matrix4 control_rel_ref;
00798 control_rel_ref.makeTransform( control_frame_node_->getPosition(),
00799 Ogre::Vector3::UNIT_SCALE,
00800 control_frame_node_->getOrientation() );
00801 Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
00802
00803
00804
00805
00806 Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
00807
00808
00809
00810 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_node_->getOrientation(),
00811 new_drag_rel_ref, intersection_2d, ray_t ))
00812 {
00813
00814
00815
00816
00817
00818
00819
00820 Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
00821 Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
00822 if( new_rel_center.length() > Ogre::Matrix3::EPSILON )
00823 {
00824 Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis_ );
00825 Ogre::Radian rot;
00826 Ogre::Vector3 axis;
00827 rotation_change.ToAngleAxis( rot, axis );
00828
00829
00830
00831
00832
00833 Ogre::Radian angle_change = rot * axis.dotProduct( rotation_axis_ );
00834 rotation_ += angle_change;
00835 parent_->rotate( rotation_change, name_ );
00836
00837
00838
00839
00840 parent_->translate( new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()), name_ );
00841 }
00842 }
00843 }
00844
00845 void InteractiveMarkerControl::moveRotate( const Ogre::Vector3& cursor_position_in_reference_frame, bool lock_axis )
00846 {
00847 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00848 drag_viewport_ )
00849 {
00850 updateControlOrientationForViewFacing( drag_viewport_ );
00851 }
00852
00853
00854
00855
00856
00857
00858
00859 Ogre::Matrix4 control_rel_ref;
00860 control_rel_ref.makeTransform( control_frame_node_->getPosition(),
00861 Ogre::Vector3::UNIT_SCALE,
00862 control_frame_node_->getOrientation() );
00863 Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
00864
00865
00866
00867
00868 Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
00869
00870
00871 Ogre::Vector3 new_drag_rel_ref = cursor_position_in_reference_frame;
00872 if (lock_axis)
00873 {
00874 Ogre::Vector3 plane_normal = (control_frame_node_->getOrientation() * control_orientation_.xAxis());
00875 Ogre::Vector3 perpendicular_offset = (new_drag_rel_ref - prev_drag_rel_ref)
00876 .dotProduct(plane_normal)*plane_normal;
00877 new_drag_rel_ref -= perpendicular_offset;
00878 }
00879
00880
00881
00882
00883
00884
00885
00886
00887
00888 Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
00889 Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
00890 if( new_rel_center.length() > Ogre::Matrix3::EPSILON )
00891 {
00892 Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis_ );
00893 Ogre::Radian rot;
00894 Ogre::Vector3 axis;
00895 rotation_change.ToAngleAxis( rot, axis );
00896
00897
00898
00899
00900
00901 Ogre::Radian angle_change = rot * axis.dotProduct( rotation_axis_ );
00902 rotation_ += angle_change;
00903 parent_->rotate( rotation_change, name_ );
00904
00905
00906
00907
00908 parent_->translate( new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()), name_ );
00909 }
00910 }
00911
00912 void InteractiveMarkerControl::move3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00913 const Ogre::Quaternion& cursor_orientation_in_reference_frame )
00914 {
00915 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00916 drag_viewport_ )
00917 {
00918 updateControlOrientationForViewFacing( drag_viewport_ );
00919 }
00920
00921
00922
00923
00924
00925 Ogre::Vector3 world_to_cursor_in_world_frame = reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
00926 Ogre::Quaternion rotation_world_to_cursor = reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
00927
00928
00929
00930 Ogre::Vector3 world_to_cursor_in_cursor_frame = rotation_world_to_cursor.Inverse()*world_to_cursor_in_world_frame;
00931 Ogre::Vector3 world_to_marker_in_world_frame = rotation_world_to_cursor*(world_to_cursor_in_cursor_frame - parent_to_cursor_in_cursor_frame_at_grab_);
00932 Ogre::Vector3 marker_position_in_reference_frame = reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
00933
00934
00935 parent_->setPose( marker_position_in_reference_frame,
00936 parent_->getOrientation(), name_ );
00937 }
00938
00939 void InteractiveMarkerControl::rotate3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00940 const Ogre::Quaternion& cursor_orientation_in_reference_frame )
00941 {
00942 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00943 drag_viewport_ )
00944 {
00945 updateControlOrientationForViewFacing( drag_viewport_ );
00946 }
00947
00948
00949
00950
00951
00952 Ogre::Vector3 world_to_cursor_in_world_frame = reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
00953 Ogre::Quaternion rotation_world_to_cursor = reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
00954
00955
00956
00957 Ogre::Vector3 world_to_cursor_in_cursor_frame = rotation_world_to_cursor.Inverse()*world_to_cursor_in_world_frame;
00958 Ogre::Vector3 world_to_marker_in_world_frame = rotation_world_to_cursor*(world_to_cursor_in_cursor_frame - parent_to_cursor_in_cursor_frame_at_grab_);
00959 Ogre::Vector3 marker_position_in_reference_frame = reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
00960 Ogre::Quaternion marker_orientation_in_reference_frame = reference_node_->convertWorldToLocalOrientation(rotation_world_to_cursor*rotation_cursor_to_parent_at_grab_);
00961
00962 parent_->setPose( parent_->getPosition(),
00963 marker_orientation_in_reference_frame, name_ );
00964 }
00965
00966
00967 void InteractiveMarkerControl::moveRotate3D( const Ogre::Vector3& cursor_position_in_reference_frame,
00968 const Ogre::Quaternion& cursor_orientation_in_reference_frame )
00969 {
00970 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00971 drag_viewport_ )
00972 {
00973 updateControlOrientationForViewFacing( drag_viewport_ );
00974 }
00975
00976
00977
00978
00979
00980 Ogre::Vector3 world_to_cursor_in_world_frame = reference_node_->convertLocalToWorldPosition(cursor_position_in_reference_frame);
00981 Ogre::Quaternion rotation_world_to_cursor = reference_node_->convertLocalToWorldOrientation(cursor_orientation_in_reference_frame);
00982
00983
00984
00985 Ogre::Vector3 world_to_cursor_in_cursor_frame = rotation_world_to_cursor.Inverse()*world_to_cursor_in_world_frame;
00986 Ogre::Vector3 world_to_marker_in_world_frame = rotation_world_to_cursor*(world_to_cursor_in_cursor_frame - parent_to_cursor_in_cursor_frame_at_grab_);
00987 Ogre::Vector3 marker_position_in_reference_frame = reference_node_->convertWorldToLocalPosition(world_to_marker_in_world_frame);
00988 Ogre::Quaternion marker_orientation_in_reference_frame = reference_node_->convertWorldToLocalOrientation(rotation_world_to_cursor*rotation_cursor_to_parent_at_grab_);
00989
00990 parent_->setPose( marker_position_in_reference_frame,
00991 marker_orientation_in_reference_frame, name_ );
00992 }
00993
00994 void InteractiveMarkerControl::setHighlight( const ControlHighlight &hl ){
00995 if(hl == NO_HIGHLIGHT) setHighlight(NO_HIGHLIGHT_VALUE);
00996 if(hl == HOVER_HIGHLIGHT) setHighlight(HOVER_HIGHLIGHT_VALUE);
00997 if(hl == ACTIVE_HIGHLIGHT) setHighlight(ACTIVE_HIGHLIGHT_VALUE);
00998 }
00999
01000 void InteractiveMarkerControl::setHighlight( float a )
01001 {
01002 std::set<Ogre::Pass*>::iterator it;
01003 for (it = highlight_passes_.begin(); it != highlight_passes_.end(); it++)
01004 {
01005 (*it)->setAmbient(a,a,a);
01006 }
01007
01008 std::vector<PointsMarkerPtr>::iterator pm_it;
01009 for( pm_it = points_markers_.begin(); pm_it != points_markers_.end(); pm_it++ )
01010 {
01011 (*pm_it)->setHighlightColor( a, a, a );
01012 }
01013 }
01014
01015 void InteractiveMarkerControl::recordDraggingInPlaceEvent( ViewportMouseEvent& event )
01016 {
01017 dragging_in_place_event_ = event;
01018 dragging_in_place_event_.type = QEvent::MouseMove;
01019 }
01020
01021 void InteractiveMarkerControl::stopDragging( bool force )
01022 {
01023
01024
01025
01026
01027 if ( mouse_dragging_ || force )
01028 {
01029 line_->setVisible(false);
01030 mouse_dragging_ = false;
01031 drag_viewport_ = NULL;
01032 parent_->stopDragging();
01033 }
01034 }
01035
01036
01037 void InteractiveMarkerControl::handle3DCursorEvent( ViewportMouseEvent event,
01038 const Ogre::Vector3& cursor_3D_pos,
01039 const Ogre::Quaternion& cursor_3D_orientation)
01040 {
01041
01042 switch( interaction_mode_ )
01043 {
01044 case visualization_msgs::InteractiveMarkerControl::BUTTON:
01045 if( event.leftUp() )
01046 {
01047 Ogre::Vector3 point_rel_world = cursor_3D_pos;
01048 bool got_3D_point = true;
01049
01050 visualization_msgs::InteractiveMarkerFeedback feedback;
01051 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
01052 feedback.control_name = name_;
01053 feedback.marker_name = parent_->getName();
01054 parent_->publishFeedback( feedback, got_3D_point, point_rel_world );
01055 }
01056 break;
01057
01058 case visualization_msgs::InteractiveMarkerControl::MENU:
01059 if( event.leftUp() )
01060 {
01061
01062 Ogre::Vector3 three_d_point = cursor_3D_pos;
01063 bool valid_point = true;
01064 Ogre::Vector2 mouse_pos = project3DPointToViewportXY(event.viewport, three_d_point);
01065 QCursor::setPos(event.panel->mapToGlobal(QPoint(mouse_pos.x, mouse_pos.y)));
01066 parent_->showMenu( event, name_, three_d_point, valid_point );
01067 }
01068 break;
01069
01070 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
01071 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
01072 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
01073 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
01074 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
01075 case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
01076 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
01077 if( event.leftDown() )
01078 {
01079 parent_->startDragging();
01080 drag_viewport_ = event.viewport;
01081
01082
01083 grab_point_in_reference_frame_ = reference_node_->convertWorldToLocalPosition( cursor_3D_pos );
01084 grab_orientation_in_reference_frame_ = reference_node_->convertWorldToLocalOrientation( cursor_3D_orientation );
01085
01086 parent_to_cursor_in_cursor_frame_at_grab_ = cursor_3D_orientation.Inverse()*(cursor_3D_pos - reference_node_->convertLocalToWorldPosition(parent_->getPosition()));
01087 rotation_cursor_to_parent_at_grab_ = cursor_3D_orientation.Inverse()*reference_node_->convertLocalToWorldOrientation(parent_->getOrientation());
01088
01089 parent_position_at_mouse_down_ = parent_->getPosition();
01090 parent_orientation_at_mouse_down_ = parent_->getOrientation();
01091
01092 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
01093 drag_viewport_ )
01094 {
01095 updateControlOrientationForViewFacing( drag_viewport_ );
01096 }
01097 control_frame_orientation_at_mouse_down_ = control_frame_node_->getOrientation();
01098 rotation_at_mouse_down_ = rotation_;
01099
01100 rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
01101
01102
01103
01104 Ogre::Vector3 rotation_center_rel_ref = closestPointOnLineToPoint( parent_->getPosition(),
01105 rotation_axis_,
01106 grab_point_in_reference_frame_ );
01107 Ogre::Matrix4 reference_rel_control_frame;
01108 reference_rel_control_frame.makeInverseTransform( control_frame_node_->getPosition(),
01109 Ogre::Vector3::UNIT_SCALE,
01110 control_frame_node_->getOrientation() );
01111 rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
01112 grab_point_rel_control_ = reference_rel_control_frame * grab_point_in_reference_frame_;
01113 }
01114 if( event.leftUp() )
01115 {
01116
01117 stopDragging( true );
01118 }
01119 break;
01120
01121 default:
01122 break;
01123 }
01124
01125 if( event.leftDown() )
01126 {
01127 setHighlight( ACTIVE_HIGHLIGHT_VALUE );
01128 }
01129 else if( event.leftUp() )
01130 {
01131 setHighlight( HOVER_HIGHLIGHT_VALUE );
01132 }
01133
01134 if (!parent_->handle3DCursorEvent(event, cursor_3D_pos, cursor_3D_orientation, name_))
01135 {
01136 if( event.type == QEvent::MouseMove && event.left())
01137 {
01138 Ogre::Vector3 cursor_position_in_reference_frame = reference_node_->convertWorldToLocalPosition( cursor_3D_pos );
01139 Ogre::Quaternion cursor_orientation_in_reference_frame = reference_node_->convertWorldToLocalOrientation( cursor_3D_orientation );
01140
01141 switch (interaction_mode_)
01142 {
01143 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
01144 rotate( cursor_position_in_reference_frame );
01145 break;
01146
01147 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
01148 moveAxis( cursor_position_in_reference_frame );
01149 break;
01150
01151 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
01152 movePlane( cursor_position_in_reference_frame );
01153 break;
01154
01155 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
01156 moveRotate( cursor_position_in_reference_frame, true );
01157 break;
01158
01159 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
01160 move3D( cursor_position_in_reference_frame, cursor_orientation_in_reference_frame );
01161 break;
01162
01163 case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
01164 rotate3D( cursor_position_in_reference_frame, cursor_orientation_in_reference_frame );
01165 break;
01166
01167 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
01168 moveRotate3D( cursor_position_in_reference_frame, cursor_orientation_in_reference_frame );
01169 break;
01170
01171 default:
01172 break;
01173 }
01174 }
01175 }
01176 }
01177
01178 void InteractiveMarkerControl::handleMouseEvent( ViewportMouseEvent& event )
01179 {
01180
01181
01182
01183
01184
01185
01186 if( event.type == QEvent::FocusIn )
01187 {
01188 has_focus_ = true;
01189 std::set<Ogre::Pass*>::iterator it;
01190 setHighlight( HOVER_HIGHLIGHT_VALUE );
01191 context_->setStatus( status_msg_ );
01192 }
01193 else if( event.type == QEvent::FocusOut )
01194 {
01195 stopDragging();
01196 has_focus_ = false;
01197 setHighlight(0.0);
01198 return;
01199 }
01200
01201 mouse_down_ = event.left() || event.middle() || event.right();
01202
01203
01204 switch( interaction_mode_ )
01205 {
01206 case visualization_msgs::InteractiveMarkerControl::BUTTON:
01207 if( event.leftUp() )
01208 {
01209 Ogre::Vector3 point_rel_world;
01210 bool got_3D_point = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
01211
01212 visualization_msgs::InteractiveMarkerFeedback feedback;
01213 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
01214 feedback.control_name = name_;
01215 feedback.marker_name = parent_->getName();
01216 parent_->publishFeedback( feedback, got_3D_point, point_rel_world );
01217 }
01218 break;
01219
01220 case visualization_msgs::InteractiveMarkerControl::MENU:
01221 if( event.leftUp() )
01222 {
01223 Ogre::Vector3 point_rel_world;
01224 bool got_3D_point = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
01225 parent_->showMenu( event, name_, point_rel_world, got_3D_point );
01226 }
01227 break;
01228
01229 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
01230 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
01231 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
01232 if ( event.leftDown() )
01233 beginMouseMovement( event,
01234 show_visual_aids_ &&
01235 orientation_mode_ != visualization_msgs::InteractiveMarkerControl::VIEW_FACING);
01236 break;
01237
01238 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
01239 if( event.leftDown() )
01240 beginMouseMovement( event, false);
01241 break;
01242
01243 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
01244 case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
01245 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
01246 if ( event.leftDown() )
01247 {
01248
01249
01250 beginMouseMovement( event, false );
01251 }
01252 else if ( event.left() &&
01253 ((modifiers_at_drag_begin_ ^ event.modifiers) & (Qt::ShiftModifier | Qt::ControlModifier)) )
01254 {
01255
01256 beginRelativeMouseMotion( event );
01257 }
01258 break;
01259
01260 default:
01261 break;
01262 }
01263
01264 if (!parent_->handleMouseEvent(event, name_))
01265 {
01266 if( event.type == QEvent::MouseMove && event.left() && mouse_dragging_)
01267 {
01268 recordDraggingInPlaceEvent( event );
01269 handleMouseMovement( event );
01270 }
01271 else if( event.type == QEvent::Wheel && event.left() && mouse_dragging_)
01272 {
01273 handleMouseWheelMovement( event );
01274 }
01275 }
01276
01277 if( event.leftDown() )
01278 {
01279 setHighlight( ACTIVE_HIGHLIGHT_VALUE );
01280 }
01281 else if( event.leftUp() )
01282 {
01283 setHighlight( HOVER_HIGHLIGHT_VALUE );
01284 stopDragging();
01285 }
01286 }
01287
01288 void InteractiveMarkerControl::beginMouseMovement( ViewportMouseEvent& event, bool line_visible )
01289 {
01290 line_->setVisible(line_visible);
01291
01292 parent_->startDragging();
01293 mouse_dragging_ = true;
01294 drag_viewport_ = event.viewport;
01295
01296 recordDraggingInPlaceEvent( event );
01297 Ogre::Vector3 grab_point_in_world_frame;
01298 if( ! context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, grab_point_in_world_frame ))
01299 {
01300
01301
01302 grab_point_in_reference_frame_ = control_frame_node_->getPosition();
01303 parent_to_grab_position_ = Ogre::Vector3(0,0,0);
01304 }
01305 else
01306 {
01307
01308
01309 grab_point_in_reference_frame_ = reference_node_->convertWorldToLocalPosition(grab_point_in_world_frame);
01310 parent_to_grab_position_ = grab_point_in_world_frame - parent_->getPosition();
01311 }
01312 parent_position_at_mouse_down_ = parent_->getPosition();
01313 parent_orientation_at_mouse_down_ = parent_->getOrientation();
01314
01315 QPoint absolute_mouse = QCursor::pos();
01316 mouse_relative_to_absolute_x_ = absolute_mouse.x() - event.x;
01317 mouse_relative_to_absolute_y_ = absolute_mouse.y() - event.y;
01318 beginRelativeMouseMotion( event );
01319
01320 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
01321 drag_viewport_ )
01322 {
01323 updateControlOrientationForViewFacing( drag_viewport_ );
01324 }
01325 control_frame_orientation_at_mouse_down_ = control_frame_node_->getOrientation();
01326 rotation_at_mouse_down_ = rotation_;
01327
01328 rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
01329
01330
01331
01332 Ogre::Vector3 rotation_center_rel_ref = closestPointOnLineToPoint( parent_->getPosition(),
01333 rotation_axis_,
01334 grab_point_in_reference_frame_ );
01335 Ogre::Matrix4 reference_rel_control_frame;
01336 reference_rel_control_frame.makeInverseTransform( control_frame_node_->getPosition(),
01337 Ogre::Vector3::UNIT_SCALE,
01338 control_frame_node_->getOrientation() );
01339 rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
01340 grab_point_rel_control_ = reference_rel_control_frame * grab_point_in_reference_frame_;
01341
01342
01343
01344 static const double DEFAULT_MOUSE_Z_SCALE = 0.001;
01345 mouse_z_scale_ = DEFAULT_MOUSE_Z_SCALE;
01346
01347 Ogre::Ray mouse_ray = getMouseRayInReferenceFrame( event, event.x, event.y );
01348 Ogre::Ray mouse_ray_10 = getMouseRayInReferenceFrame( event, event.x, event.y + 10 );
01349
01350 Ogre::Vector3 intersection_3d;
01351 Ogre::Vector3 intersection_3d_10;
01352 Ogre::Vector2 intersection_2d;
01353 float ray_t;
01354
01355 if( intersectSomeYzPlane( mouse_ray,
01356 grab_point_in_reference_frame_,
01357 control_frame_node_->getOrientation(),
01358 intersection_3d,
01359 intersection_2d,
01360 ray_t ))
01361 {
01362 if( intersectSomeYzPlane( mouse_ray_10,
01363 grab_point_in_reference_frame_,
01364 control_frame_node_->getOrientation(),
01365 intersection_3d_10,
01366 intersection_2d,
01367 ray_t ))
01368 {
01369 mouse_z_scale_ = (intersection_3d_10 - intersection_3d).length() / 10.0;
01370 if (mouse_z_scale_ < std::numeric_limits<float>::min() * 100.0)
01371 mouse_z_scale_ = DEFAULT_MOUSE_Z_SCALE;
01372 }
01373 }
01374 }
01375
01376 void InteractiveMarkerControl::handleMouseMovement( ViewportMouseEvent& event )
01377 {
01378 Ogre::Ray mouse_ray = getMouseRayInReferenceFrame( event, event.x, event.y );
01379 Ogre::Ray last_mouse_ray = getMouseRayInReferenceFrame( event, event.last_x, event.last_y );
01380
01381 bool do_rotation = false;
01382 switch (interaction_mode_)
01383 {
01384 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
01385 moveAxis( mouse_ray, event );
01386 break;
01387
01388 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
01389 movePlane( mouse_ray );
01390 break;
01391
01392 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
01393 moveRotate( mouse_ray );
01394 break;
01395
01396 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
01397 rotate( mouse_ray );
01398 break;
01399
01400 case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
01401 do_rotation = true;
01402
01403 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
01404 if ( event.control() )
01405 do_rotation = true;
01406
01407 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
01408 if ( do_rotation )
01409 {
01410 if (event.shift())
01411 rotateZRelative( event );
01412 else
01413 rotateXYRelative( event );
01414 }
01415 else
01416 {
01417 if (event.shift())
01418 moveZAxisRelative( event );
01419 else
01420 moveViewPlane( mouse_ray, event );
01421 }
01422 break;
01423
01424 default:
01425 break;
01426 }
01427 }
01428
01429 void InteractiveMarkerControl::handleMouseWheelMovement( ViewportMouseEvent& event )
01430 {
01431 switch (interaction_mode_)
01432 {
01433 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D:
01434 case visualization_msgs::InteractiveMarkerControl::MOVE_3D:
01435 moveZAxisWheel( event );
01436 break;
01437
01438 default:
01439 break;
01440 }
01441 }
01442
01443 bool InteractiveMarkerControl::intersectYzPlane( const Ogre::Ray& mouse_ray,
01444 Ogre::Vector3& intersection_3d,
01445 Ogre::Vector2& intersection_2d,
01446 float &ray_t )
01447 {
01448 return intersectSomeYzPlane( mouse_ray,
01449 control_frame_node_->getPosition(),
01450 control_frame_node_->getOrientation(),
01451 intersection_3d, intersection_2d, ray_t );
01452 }
01453
01454 bool InteractiveMarkerControl::intersectSomeYzPlane( const Ogre::Ray& mouse_ray,
01455 const Ogre::Vector3& point_on_plane,
01456 const Ogre::Quaternion& plane_orientation,
01457 Ogre::Vector3& intersection_3d,
01458 Ogre::Vector2& intersection_2d,
01459 float& ray_t )
01460 {
01461 Ogre::Vector3 normal = plane_orientation * control_orientation_.xAxis();
01462 Ogre::Vector3 axis_1 = plane_orientation * control_orientation_.yAxis();
01463 Ogre::Vector3 axis_2 = plane_orientation * control_orientation_.zAxis();
01464
01465 Ogre::Plane plane(normal, point_on_plane);
01466
01467 Ogre::Vector2 origin_2d(point_on_plane.dotProduct(axis_1), point_on_plane.dotProduct(axis_2));
01468
01469 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
01470 if (intersection.first)
01471 {
01472 intersection_3d = mouse_ray.getPoint(intersection.second);
01473 intersection_2d = Ogre::Vector2(intersection_3d.dotProduct(axis_1), intersection_3d.dotProduct(axis_2));
01474 intersection_2d -= origin_2d;
01475
01476 ray_t = intersection.second;
01477 return true;
01478 }
01479
01480 ray_t = 0;
01481 return false;
01482 }
01483
01484 void InteractiveMarkerControl::addHighlightPass( S_MaterialPtr materials )
01485 {
01486 S_MaterialPtr::iterator it;
01487
01488 for (it = materials.begin(); it != materials.end(); it++)
01489 {
01490 Ogre::MaterialPtr material = *it;
01491 Ogre::Pass *original_pass = material->getTechnique(0)->getPass(0);
01492 Ogre::Pass *pass = material->getTechnique(0)->createPass();
01493
01494 pass->setSceneBlending(Ogre::SBT_ADD);
01495 pass->setDepthWriteEnabled(false);
01496 pass->setDepthCheckEnabled(true);
01497 pass->setLightingEnabled(true);
01498 pass->setAmbient(0, 0, 0);
01499 pass->setDiffuse(0, 0, 0, 0);
01500 pass->setSpecular(0, 0, 0, 0);
01501 pass->setCullingMode(original_pass->getCullingMode());
01502
01503 highlight_passes_.insert(pass);
01504 }
01505 }
01506
01507 }