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 "interactive_marker_control.h"
00031
00032 #include "rviz/default_plugin/markers/marker_base.h"
00033 #include "rviz/display_context.h"
00034 #include "rviz/selection/selection_manager.h"
00035 #include "rviz/render_panel.h"
00036 #include "interactive_marker.h"
00037
00038 #include <OGRE/OgreViewport.h>
00039 #include <OGRE/OgreCamera.h>
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042 #include <OGRE/OgrePass.h>
00043 #include <OGRE/OgreMaterial.h>
00044 #include <OGRE/OgreEntity.h>
00045 #include <OGRE/OgreSubEntity.h>
00046
00047 #include "markers/shape_marker.h"
00048 #include "markers/arrow_marker.h"
00049 #include "markers/line_list_marker.h"
00050 #include "markers/line_strip_marker.h"
00051 #include "markers/points_marker.h"
00052 #include "markers/text_view_facing_marker.h"
00053 #include "markers/mesh_resource_marker.h"
00054 #include "markers/triangle_list_marker.h"
00055
00056 #define ACTIVE_HIGHLIGHT 0.5
00057 #define HOVER_HIGHLIGHT 0.3
00058
00059 namespace rviz
00060 {
00061
00062 InteractiveMarkerControl::InteractiveMarkerControl( DisplayContext* context,
00063 Ogre::SceneNode *reference_node,
00064 InteractiveMarker *parent )
00065 : dragging_(false)
00066 , drag_viewport_( NULL )
00067 , context_( context )
00068 , reference_node_(reference_node)
00069 , control_frame_node_(reference_node_->createChildSceneNode())
00070 , markers_node_(reference_node_->createChildSceneNode())
00071 , parent_(parent)
00072 , rotation_(0)
00073 , grab_point_(0,0,0)
00074 , interaction_enabled_(false)
00075 , visible_(true)
00076 , view_facing_( false )
00077 {
00078 }
00079
00080 void InteractiveMarkerControl::makeMarkers( const visualization_msgs::InteractiveMarkerControl& message )
00081 {
00082 for (unsigned i = 0; i < message.markers.size(); i++)
00083 {
00084 MarkerBasePtr marker;
00085
00086
00087 switch (message.markers[i].type)
00088 {
00089 case visualization_msgs::Marker::CUBE:
00090 case visualization_msgs::Marker::CYLINDER:
00091 case visualization_msgs::Marker::SPHERE:
00092 {
00093 marker.reset(new ShapeMarker(0, context_, markers_node_));
00094 }
00095 break;
00096
00097 case visualization_msgs::Marker::ARROW:
00098 {
00099 marker.reset(new ArrowMarker(0, context_, markers_node_));
00100 }
00101 break;
00102
00103 case visualization_msgs::Marker::LINE_STRIP:
00104 {
00105 marker.reset(new LineStripMarker(0, context_, markers_node_));
00106 }
00107 break;
00108 case visualization_msgs::Marker::LINE_LIST:
00109 {
00110 marker.reset(new LineListMarker(0, context_, markers_node_));
00111 }
00112 break;
00113 case visualization_msgs::Marker::SPHERE_LIST:
00114 case visualization_msgs::Marker::CUBE_LIST:
00115 case visualization_msgs::Marker::POINTS:
00116 {
00117 PointsMarkerPtr points_marker;
00118 points_marker.reset(new PointsMarker(0, context_, markers_node_));
00119 points_markers_.push_back( points_marker );
00120 marker = points_marker;
00121 }
00122 break;
00123 case visualization_msgs::Marker::TEXT_VIEW_FACING:
00124 {
00125 marker.reset(new TextViewFacingMarker(0, context_, markers_node_));
00126 }
00127 break;
00128 case visualization_msgs::Marker::MESH_RESOURCE:
00129 {
00130 marker.reset(new MeshResourceMarker(0, context_, markers_node_));
00131 }
00132 break;
00133
00134 case visualization_msgs::Marker::TRIANGLE_LIST:
00135 {
00136 marker.reset(new TriangleListMarker(0, context_, markers_node_));
00137 }
00138 break;
00139 default:
00140 ROS_ERROR( "Unknown marker type: %d", message.markers[i].type );
00141 }
00142
00143 marker->setMessage( message.markers[ i ]);
00144 marker->setInteractiveObject( shared_from_this() );
00145
00146 addHighlightPass(marker->getMaterials());
00147
00148
00149
00150
00151 marker->setPosition( markers_node_->convertWorldToLocalPosition( marker->getPosition() ) );
00152 marker->setOrientation( markers_node_->convertWorldToLocalOrientation( marker->getOrientation() ) );
00153
00154 markers_.push_back(marker);
00155 }
00156 }
00157
00158 InteractiveMarkerControl::~InteractiveMarkerControl()
00159 {
00160 context_->getSceneManager()->destroySceneNode(control_frame_node_);
00161 context_->getSceneManager()->destroySceneNode(markers_node_);
00162
00163 if( view_facing_ )
00164 {
00165 context_->getSceneManager()->removeListener(this);
00166 }
00167 }
00168
00169 void InteractiveMarkerControl::processMessage( const visualization_msgs::InteractiveMarkerControl &message )
00170 {
00171 name_ = message.name;
00172 description_ = message.description;
00173 interaction_mode_ = message.interaction_mode;
00174 always_visible_ = message.always_visible;
00175 orientation_mode_ = message.orientation_mode;
00176
00177 control_orientation_ = Ogre::Quaternion(message.orientation.w,
00178 message.orientation.x, message.orientation.y, message.orientation.z);
00179 control_orientation_.normalise();
00180
00181 bool new_view_facingness = (message.orientation_mode == visualization_msgs::InteractiveMarkerControl::VIEW_FACING);
00182 if( new_view_facingness != view_facing_ )
00183 {
00184 if( new_view_facingness )
00185 {
00186 context_->getSceneManager()->addListener(this);
00187 }
00188 else
00189 {
00190 context_->getSceneManager()->removeListener(this);
00191 }
00192 view_facing_ = new_view_facingness;
00193 }
00194
00195 independent_marker_orientation_ = message.independent_marker_orientation;
00196
00197
00198
00199 highlight_passes_.clear();
00200 markers_.clear();
00201 points_markers_.clear();
00202
00203
00204
00205 control_frame_node_->setPosition(parent_->getPosition());
00206 markers_node_->setPosition(parent_->getPosition());
00207
00208 if ( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::INHERIT )
00209 {
00210 control_frame_node_->setOrientation(parent_->getOrientation());
00211 markers_node_->setOrientation(parent_->getOrientation());
00212 }
00213 else
00214 {
00215 control_frame_node_->setOrientation( Ogre::Quaternion::IDENTITY );
00216 markers_node_->setOrientation( Ogre::Quaternion::IDENTITY );
00217 }
00218
00219 makeMarkers( message );
00220
00221
00222
00223
00224
00225 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00226 independent_marker_orientation_ )
00227 {
00228 markers_node_->setOrientation( parent_->getOrientation() );
00229 }
00230
00231 enableInteraction(context_->getSelectionManager()->getInteractionEnabled());
00232 }
00233
00234
00235
00236 void InteractiveMarkerControl::preFindVisibleObjects(
00237 Ogre::SceneManager *source,
00238 Ogre::SceneManager::IlluminationRenderStage irs, Ogre::Viewport *v )
00239 {
00240 updateControlOrientationForViewFacing( v );
00241 }
00242
00243 void InteractiveMarkerControl::updateControlOrientationForViewFacing( Ogre::Viewport* v )
00244 {
00245 Ogre::Quaternion x_view_facing_rotation =
00246 control_orientation_.xAxis().getRotationTo( v->getCamera()->getDerivedDirection());
00247
00248
00249 Ogre::Vector3 z_axis_2 = x_view_facing_rotation * control_orientation_.zAxis();
00250 Ogre::Quaternion align_yz_rotation = z_axis_2.getRotationTo(v->getCamera()->getDerivedUp());
00251
00252
00253 Ogre::Quaternion rotate_around_x = Ogre::Quaternion(rotation_, v->getCamera()->getDerivedDirection());
00254
00255 Ogre::Quaternion rotation = reference_node_->convertWorldToLocalOrientation(
00256 rotate_around_x * align_yz_rotation * x_view_facing_rotation );
00257
00258 control_frame_node_->setOrientation( rotation );
00259
00260 if ( !independent_marker_orientation_ )
00261 {
00262 markers_node_->setOrientation( rotation );
00263
00264
00265 markers_node_->_update(true, false);
00266 }
00267 }
00268
00269 void InteractiveMarkerControl::setVisible( bool visible )
00270 {
00271 visible_ = visible;
00272
00273 if (always_visible_)
00274 {
00275 markers_node_->setVisible(visible_);
00276 } else
00277 {
00278 markers_node_->setVisible(interaction_enabled_ && visible_);
00279 }
00280 }
00281
00282 void InteractiveMarkerControl::update()
00283 {
00284 if( dragging_ )
00285 {
00286 handleMouseMovement( dragging_in_place_event_ );
00287 }
00288 }
00289
00290 void InteractiveMarkerControl::enableInteraction( bool enable )
00291 {
00292 interaction_enabled_ = enable;
00293 setVisible(visible_);
00294 if (!enable)
00295 {
00296 setHighlight(0.0);
00297 }
00298 }
00299
00300 void InteractiveMarkerControl::interactiveMarkerPoseChanged(
00301 Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation )
00302 {
00303 control_frame_node_->setPosition(int_marker_position);
00304 markers_node_->setPosition(int_marker_position);
00305
00306 switch (orientation_mode_)
00307 {
00308 case visualization_msgs::InteractiveMarkerControl::INHERIT:
00309 control_frame_node_->setOrientation(int_marker_orientation);
00310 markers_node_->setOrientation(control_frame_node_->getOrientation());
00311 break;
00312
00313 case visualization_msgs::InteractiveMarkerControl::FIXED:
00314 {
00315 control_frame_node_->setOrientation( Ogre::Quaternion( rotation_, control_orientation_.xAxis() ));
00316 markers_node_->setOrientation(control_frame_node_->getOrientation());
00317 break;
00318 }
00319
00320 case visualization_msgs::InteractiveMarkerControl::VIEW_FACING:
00321 if( drag_viewport_ )
00322 {
00323 updateControlOrientationForViewFacing( drag_viewport_ );
00324 }
00325 if ( independent_marker_orientation_ )
00326 {
00327 markers_node_->setOrientation(int_marker_orientation);
00328 }
00329 break;
00330
00331 default:
00332 break;
00333 }
00334 }
00335
00336 Ogre::Vector3 InteractiveMarkerControl::closestPointOnLineToPoint( const Ogre::Vector3& line_start,
00337 const Ogre::Vector3& line_dir,
00338 const Ogre::Vector3& test_point )
00339 {
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349 double factor = ( test_point - line_start ).dotProduct( line_dir ) / line_dir.dotProduct( line_dir );
00350 Ogre::Vector3 closest_point = line_start + line_dir * factor;
00351 return closest_point;
00352 }
00353
00354 void InteractiveMarkerControl::rotate( Ogre::Ray &mouse_ray )
00355 {
00356 Ogre::Vector3 intersection_3d;
00357 Ogre::Vector2 intersection_2d;
00358 float ray_t;
00359
00360 Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
00361
00362
00363
00364 Ogre::Vector3 rotation_center = closestPointOnLineToPoint( control_frame_node_->getPosition(),
00365 rotation_axis,
00366 grab_point_ );
00367
00368
00369 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation_at_mouse_down_,
00370 intersection_3d, intersection_2d, ray_t ))
00371 {
00372
00373 Ogre::Vector3 grab_rel_center = grab_point_ - rotation_center;
00374 Ogre::Vector3 mouse_rel_center = intersection_3d - rotation_center;
00375
00376 Ogre::Quaternion orientation_change_since_mouse_down =
00377 grab_rel_center.getRotationTo( mouse_rel_center, rotation_axis );
00378
00379 Ogre::Radian rot;
00380 Ogre::Vector3 axis;
00381 orientation_change_since_mouse_down.ToAngleAxis( rot, axis );
00382
00383
00384
00385
00386
00387 Ogre::Radian rotation_since_mouse_down = rot * axis.dotProduct( rotation_axis );
00388
00389 rotation_ = rotation_at_mouse_down_ + rotation_since_mouse_down;
00390 parent_->setPose( parent_->getPosition(),
00391 orientation_change_since_mouse_down * parent_orientation_at_mouse_down_,
00392 name_ );
00393 }
00394 }
00395
00396 void InteractiveMarkerControl::movePlane( Ogre::Ray &mouse_ray )
00397 {
00398 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00399 drag_viewport_ )
00400 {
00401 updateControlOrientationForViewFacing( drag_viewport_ );
00402 }
00403
00404 Ogre::Vector3 intersection_3d;
00405 Ogre::Vector2 intersection_2d;
00406 float ray_t;
00407
00408 if( intersectSomeYzPlane( mouse_ray, grab_point_, control_frame_node_->getOrientation(),
00409 intersection_3d, intersection_2d, ray_t ))
00410 {
00411 parent_->setPose( intersection_3d - grab_point_ + parent_position_at_mouse_down_, parent_->getOrientation(), name_ );
00412 }
00413 }
00414
00417 void InteractiveMarkerControl::worldToScreen( const Ogre::Vector3& pos_rel_reference,
00418 const Ogre::Viewport* viewport,
00419 Ogre::Vector2& screen_pos )
00420 {
00421 Ogre::Vector3 world_pos = reference_node_->convertLocalToWorldPosition( pos_rel_reference );
00422
00423 const Ogre::Camera* cam = viewport->getCamera();
00424 Ogre::Vector3 homogeneous_screen_position = cam->getProjectionMatrix() * (cam->getViewMatrix() * world_pos);
00425
00426 double half_width = viewport->getActualWidth() / 2.0;
00427 double half_height = viewport->getActualHeight() / 2.0;
00428
00429 screen_pos.x = half_width + (half_width * homogeneous_screen_position.x) - .5;
00430 screen_pos.y = half_height + (half_height * -homogeneous_screen_position.y) - .5;
00431 }
00432
00436 bool InteractiveMarkerControl::findClosestPoint( const Ogre::Ray& target_ray,
00437 const Ogre::Ray& mouse_ray,
00438 Ogre::Vector3& closest_point )
00439 {
00440
00441
00442
00443
00444
00445
00446 Ogre::Vector3 v13 = target_ray.getOrigin() - mouse_ray.getOrigin();
00447 Ogre::Vector3 v43 = mouse_ray.getDirection();
00448 Ogre::Vector3 v21 = target_ray.getDirection();
00449 double d1343 = v13.dotProduct( v43 );
00450 double d4321 = v43.dotProduct( v21 );
00451 double d1321 = v13.dotProduct( v21 );
00452 double d4343 = v43.dotProduct( v43 );
00453 double d2121 = v21.dotProduct( v21 );
00454
00455 double denom = d2121 * d4343 - d4321 * d4321;
00456 if( fabs( denom ) <= Ogre::Matrix3::EPSILON )
00457 {
00458 return false;
00459 }
00460 double numer = d1343 * d4321 - d1321 * d4343;
00461
00462 double mua = numer / denom;
00463 closest_point = target_ray.getPoint( mua );
00464 return true;
00465 }
00466
00467 void InteractiveMarkerControl::moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event )
00468 {
00469
00470 Ogre::Ray control_ray;
00471 control_ray.setOrigin( grab_point_ );
00472 control_ray.setDirection( control_frame_node_->getOrientation() * control_orientation_.xAxis() );
00473
00474
00475 Ogre::Vector2 control_ray_screen_start, control_ray_screen_end;
00476 worldToScreen( control_ray.getOrigin(), event.viewport, control_ray_screen_start );
00477 worldToScreen( control_ray.getPoint( 1 ), event.viewport, control_ray_screen_end );
00478
00479 Ogre::Vector2 mouse_point( event.x, event.y );
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490 Ogre::Vector2 control_ray_screen_dir = control_ray_screen_end - control_ray_screen_start;
00491 double denominator = control_ray_screen_dir.dotProduct( control_ray_screen_dir );
00492 if( fabs( denominator ) > Ogre::Matrix3::EPSILON )
00493 {
00494 double factor =
00495 ( mouse_point - control_ray_screen_start ).dotProduct( control_ray_screen_dir ) / denominator;
00496
00497 Ogre::Vector2 closest_screen_point = control_ray_screen_start + control_ray_screen_dir * factor;
00498
00499
00500 int width = event.viewport->getActualWidth() - 1;
00501 int height = event.viewport->getActualHeight() - 1;
00502 Ogre::Ray new_mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (closest_screen_point.x+.5) / width,
00503 (closest_screen_point.y+.5) / height );
00504 new_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( new_mouse_ray.getOrigin() ) );
00505 new_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * new_mouse_ray.getDirection() );
00506
00507
00508 Ogre::Vector3 closest_point;
00509 if( findClosestPoint( control_ray, new_mouse_ray, closest_point ))
00510 {
00511
00512 parent_->setPose( closest_point - grab_point_ + parent_position_at_mouse_down_,
00513 parent_->getOrientation(), name_ );
00514 }
00515 }
00516 }
00517
00518 void InteractiveMarkerControl::moveRotate( Ogre::Ray &mouse_ray )
00519 {
00520 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00521 drag_viewport_ )
00522 {
00523 updateControlOrientationForViewFacing( drag_viewport_ );
00524 }
00525
00526 Ogre::Vector3 new_drag_rel_ref;
00527 Ogre::Vector2 intersection_2d;
00528 float ray_t;
00529
00530
00531
00532
00533
00534
00535
00536 Ogre::Matrix4 control_rel_ref;
00537 control_rel_ref.makeTransform( control_frame_node_->getPosition(),
00538 Ogre::Vector3::UNIT_SCALE,
00539 control_frame_node_->getOrientation() );
00540 Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
00541
00542
00543
00544
00545 Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
00546
00547
00548
00549 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_node_->getOrientation(),
00550 new_drag_rel_ref, intersection_2d, ray_t ))
00551 {
00552
00553
00554
00555
00556
00557
00558
00559 Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
00560 Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
00561 if( new_rel_center.length() > Ogre::Matrix3::EPSILON )
00562 {
00563 Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis_ );
00564 Ogre::Radian rot;
00565 Ogre::Vector3 axis;
00566 rotation_change.ToAngleAxis( rot, axis );
00567
00568
00569
00570
00571
00572 Ogre::Radian angle_change = rot * axis.dotProduct( rotation_axis_ );
00573 rotation_ += angle_change;
00574 parent_->rotate( rotation_change, name_ );
00575
00576
00577
00578
00579 parent_->translate( new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()), name_ );
00580 }
00581 }
00582 }
00583
00584 void InteractiveMarkerControl::setHighlight( float a )
00585 {
00586 std::set<Ogre::Pass*>::iterator it;
00587 for (it = highlight_passes_.begin(); it != highlight_passes_.end(); it++)
00588 {
00589 (*it)->setAmbient(a,a,a);
00590 }
00591
00592 std::vector<PointsMarkerPtr>::iterator pm_it;
00593 for( pm_it = points_markers_.begin(); pm_it != points_markers_.end(); pm_it++ )
00594 {
00595 (*pm_it)->setHighlightColor( a, a, a );
00596 }
00597 }
00598
00599 void InteractiveMarkerControl::recordDraggingInPlaceEvent( ViewportMouseEvent& event )
00600 {
00601 dragging_in_place_event_ = event;
00602 dragging_in_place_event_.type = QEvent::MouseMove;
00603 }
00604
00605 void InteractiveMarkerControl::handleMouseEvent( ViewportMouseEvent& event )
00606 {
00607
00608
00609
00610
00611
00612 if( event.type == QEvent::FocusIn )
00613 {
00614 has_focus_ = true;
00615 std::set<Ogre::Pass*>::iterator it;
00616 setHighlight( HOVER_HIGHLIGHT );
00617 }
00618 else if( event.type == QEvent::FocusOut )
00619 {
00620 has_focus_ = false;
00621 setHighlight(0.0);
00622 return;
00623 }
00624
00625
00626 switch( interaction_mode_ )
00627 {
00628 case visualization_msgs::InteractiveMarkerControl::BUTTON:
00629 if( event.leftUp() )
00630 {
00631 Ogre::Vector3 point_rel_world;
00632 bool got_3D_point = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
00633
00634 visualization_msgs::InteractiveMarkerFeedback feedback;
00635 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
00636 feedback.control_name = name_;
00637 feedback.marker_name = parent_->getName();
00638 parent_->publishFeedback( feedback, got_3D_point, point_rel_world );
00639 }
00640 break;
00641
00642 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00643 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00644 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00645 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00646 if( event.leftDown() )
00647 {
00648 parent_->startDragging();
00649 dragging_ = true;
00650 drag_viewport_ = event.viewport;
00651
00652 recordDraggingInPlaceEvent( event );
00653 if( ! context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, grab_point_ ))
00654 {
00655
00656
00657 grab_point_ = control_frame_node_->getPosition();
00658 }
00659 else
00660 {
00661
00662
00663 grab_point_ = reference_node_->convertWorldToLocalPosition(grab_point_);
00664 }
00665 grab_pixel_.x = event.x;
00666 grab_pixel_.y = event.y;
00667 parent_position_at_mouse_down_ = parent_->getPosition();
00668 parent_orientation_at_mouse_down_ = parent_->getOrientation();
00669
00670 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00671 drag_viewport_ )
00672 {
00673 updateControlOrientationForViewFacing( drag_viewport_ );
00674 }
00675 control_frame_orientation_at_mouse_down_ = control_frame_node_->getOrientation();
00676 rotation_at_mouse_down_ = rotation_;
00677
00678 rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
00679
00680
00681
00682 Ogre::Vector3 rotation_center_rel_ref = closestPointOnLineToPoint( parent_->getPosition(),
00683 rotation_axis_,
00684 grab_point_ );
00685 Ogre::Matrix4 reference_rel_control_frame;
00686 reference_rel_control_frame.makeInverseTransform( control_frame_node_->getPosition(),
00687 Ogre::Vector3::UNIT_SCALE,
00688 control_frame_node_->getOrientation() );
00689 rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
00690 grab_point_rel_control_ = reference_rel_control_frame * grab_point_;
00691 }
00692 if( event.leftUp() )
00693 {
00694 dragging_ = false;
00695 drag_viewport_ = NULL;
00696 parent_->stopDragging();
00697 }
00698 break;
00699
00700 default:
00701 break;
00702 }
00703
00704 if( event.leftDown() )
00705 {
00706 setHighlight( ACTIVE_HIGHLIGHT );
00707 }
00708 else if( event.leftUp() )
00709 {
00710 setHighlight( HOVER_HIGHLIGHT );
00711 }
00712
00713 if (!parent_->handleMouseEvent(event, name_))
00714 {
00715 if( event.type == QEvent::MouseMove && event.left() )
00716 {
00717 recordDraggingInPlaceEvent( event );
00718 handleMouseMovement( event );
00719 }
00720 }
00721 }
00722
00723 void InteractiveMarkerControl::handleMouseMovement( ViewportMouseEvent& event )
00724 {
00725
00726 float width = event.viewport->getActualWidth() - 1;
00727 float height = event.viewport->getActualHeight() - 1;
00728
00729 Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (event.x + .5) / width,
00730 (event.y + .5) / height );
00731
00732 Ogre::Ray last_mouse_ray =
00733 event.viewport->getCamera()->getCameraToViewportRay(
00734 (event.last_x + .5) / width, (event.last_y + .5) / height);
00735
00736
00737 mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00738 mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00739
00740 last_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( last_mouse_ray.getOrigin() ) );
00741 last_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * last_mouse_ray.getDirection() );
00742
00743 switch (interaction_mode_)
00744 {
00745 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00746 moveAxis( mouse_ray, event );
00747 break;
00748
00749 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00750 movePlane( mouse_ray );
00751 break;
00752
00753 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00754 moveRotate( mouse_ray );
00755 break;
00756
00757 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00758 rotate(mouse_ray);
00759 break;
00760
00761 default:
00762 break;
00763 }
00764 }
00765
00766 bool InteractiveMarkerControl::intersectYzPlane( const Ogre::Ray& mouse_ray,
00767 Ogre::Vector3& intersection_3d,
00768 Ogre::Vector2& intersection_2d,
00769 float &ray_t )
00770 {
00771 return intersectSomeYzPlane( mouse_ray,
00772 control_frame_node_->getPosition(),
00773 control_frame_node_->getOrientation(),
00774 intersection_3d, intersection_2d, ray_t );
00775 }
00776
00777 bool InteractiveMarkerControl::intersectSomeYzPlane( const Ogre::Ray& mouse_ray,
00778 const Ogre::Vector3& point_on_plane,
00779 const Ogre::Quaternion& plane_orientation,
00780 Ogre::Vector3& intersection_3d,
00781 Ogre::Vector2& intersection_2d,
00782 float& ray_t )
00783 {
00784 Ogre::Vector3 normal = plane_orientation * control_orientation_.xAxis();
00785 Ogre::Vector3 axis_1 = plane_orientation * control_orientation_.yAxis();
00786 Ogre::Vector3 axis_2 = plane_orientation * control_orientation_.zAxis();
00787
00788 Ogre::Plane plane(normal, point_on_plane);
00789
00790 Ogre::Vector2 origin_2d(point_on_plane.dotProduct(axis_1), point_on_plane.dotProduct(axis_2));
00791
00792 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
00793 if (intersection.first)
00794 {
00795 intersection_3d = mouse_ray.getPoint(intersection.second);
00796 intersection_2d = Ogre::Vector2(intersection_3d.dotProduct(axis_1), intersection_3d.dotProduct(axis_2));
00797 intersection_2d -= origin_2d;
00798
00799 ray_t = intersection.second;
00800 return true;
00801 }
00802
00803 ray_t = 0;
00804 return false;
00805 }
00806
00807 void InteractiveMarkerControl::addHighlightPass( S_MaterialPtr materials )
00808 {
00809 S_MaterialPtr::iterator it;
00810
00811 for (it = materials.begin(); it != materials.end(); it++)
00812 {
00813 Ogre::MaterialPtr material = *it;
00814 Ogre::Pass *original_pass = material->getTechnique(0)->getPass(0);
00815 Ogre::Pass *pass = material->getTechnique(0)->createPass();
00816
00817 pass->setSceneBlending(Ogre::SBT_ADD);
00818 pass->setDepthWriteEnabled(true);
00819 pass->setDepthCheckEnabled(true);
00820 pass->setLightingEnabled(true);
00821 pass->setAmbient(0, 0, 0);
00822 pass->setDiffuse(0, 0, 0, 0);
00823 pass->setSpecular(0, 0, 0, 0);
00824 pass->setCullingMode(original_pass->getCullingMode());
00825
00826 highlight_passes_.insert(pass);
00827 }
00828 }
00829
00830 }