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/visualization_manager.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( VisualizationManager* vis_manager,
00063 Ogre::SceneNode *reference_node,
00064 InteractiveMarker *parent )
00065 : dragging_(false)
00066 , drag_viewport_( NULL )
00067 , vis_manager_(vis_manager)
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, vis_manager_, markers_node_));
00094 }
00095 break;
00096
00097 case visualization_msgs::Marker::ARROW:
00098 {
00099 marker.reset(new ArrowMarker(0, vis_manager_, markers_node_));
00100 }
00101 break;
00102
00103 case visualization_msgs::Marker::LINE_STRIP:
00104 {
00105 marker.reset(new LineStripMarker(0, vis_manager_, markers_node_));
00106 }
00107 break;
00108 case visualization_msgs::Marker::LINE_LIST:
00109 {
00110 marker.reset(new LineListMarker(0, vis_manager_, 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, vis_manager_, 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, vis_manager_, markers_node_));
00126 }
00127 break;
00128 case visualization_msgs::Marker::MESH_RESOURCE:
00129 {
00130 marker.reset(new MeshResourceMarker(0, vis_manager_, markers_node_));
00131 }
00132 break;
00133
00134 case visualization_msgs::Marker::TRIANGLE_LIST:
00135 {
00136 marker.reset(new TriangleListMarker(0, vis_manager_, 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 vis_manager_->getSceneManager()->destroySceneNode(control_frame_node_);
00161 vis_manager_->getSceneManager()->destroySceneNode(markers_node_);
00162
00163 if( view_facing_ )
00164 {
00165 vis_manager_->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 vis_manager_->getSceneManager()->addListener(this);
00187 }
00188 else
00189 {
00190 vis_manager_->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(vis_manager_->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::hideVisible()
00270 {
00271 saved_visibility_state_ = visible_;
00272 setVisible(false);
00273 }
00274
00275 void InteractiveMarkerControl::restoreVisible()
00276 {
00277 setVisible(saved_visibility_state_);
00278 }
00279
00280 void InteractiveMarkerControl::setVisible( bool visible )
00281 {
00282 visible_ = visible;
00283
00284 if (always_visible_)
00285 {
00286 markers_node_->setVisible(visible_);
00287 } else
00288 {
00289 markers_node_->setVisible(interaction_enabled_ && visible_);
00290 }
00291 }
00292
00293 void InteractiveMarkerControl::update()
00294 {
00295 if( dragging_ )
00296 {
00297 handleMouseMovement( dragging_in_place_event_ );
00298 }
00299 }
00300
00301 void InteractiveMarkerControl::enableInteraction( bool enable )
00302 {
00303 interaction_enabled_ = enable;
00304 setVisible(visible_);
00305 if (!enable)
00306 {
00307 setHighlight(0.0);
00308 }
00309 }
00310
00311 void InteractiveMarkerControl::interactiveMarkerPoseChanged(
00312 Ogre::Vector3 int_marker_position, Ogre::Quaternion int_marker_orientation )
00313 {
00314 control_frame_node_->setPosition(int_marker_position);
00315 markers_node_->setPosition(int_marker_position);
00316
00317 switch (orientation_mode_)
00318 {
00319 case visualization_msgs::InteractiveMarkerControl::INHERIT:
00320 control_frame_node_->setOrientation(int_marker_orientation);
00321 markers_node_->setOrientation(control_frame_node_->getOrientation());
00322 break;
00323
00324 case visualization_msgs::InteractiveMarkerControl::FIXED:
00325 {
00326 control_frame_node_->setOrientation( Ogre::Quaternion( rotation_, control_orientation_.xAxis() ));
00327 markers_node_->setOrientation(control_frame_node_->getOrientation());
00328 break;
00329 }
00330
00331 case visualization_msgs::InteractiveMarkerControl::VIEW_FACING:
00332 if( drag_viewport_ )
00333 {
00334 updateControlOrientationForViewFacing( drag_viewport_ );
00335 }
00336 if ( independent_marker_orientation_ )
00337 {
00338 markers_node_->setOrientation(int_marker_orientation);
00339 }
00340 break;
00341
00342 default:
00343 break;
00344 }
00345 }
00346
00347 Ogre::Vector3 InteractiveMarkerControl::closestPointOnLineToPoint( const Ogre::Vector3& line_start,
00348 const Ogre::Vector3& line_dir,
00349 const Ogre::Vector3& test_point )
00350 {
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360 double factor = ( test_point - line_start ).dotProduct( line_dir ) / line_dir.dotProduct( line_dir );
00361 Ogre::Vector3 closest_point = line_start + line_dir * factor;
00362 return closest_point;
00363 }
00364
00365 void InteractiveMarkerControl::rotate( Ogre::Ray &mouse_ray )
00366 {
00367 Ogre::Vector3 intersection_3d;
00368 Ogre::Vector2 intersection_2d;
00369 float ray_t;
00370
00371 Ogre::Vector3 rotation_axis = control_frame_orientation_at_mouse_down_ * control_orientation_.xAxis();
00372
00373
00374
00375 Ogre::Vector3 rotation_center = closestPointOnLineToPoint( control_frame_node_->getPosition(),
00376 rotation_axis,
00377 grab_point_ );
00378
00379
00380 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation_at_mouse_down_,
00381 intersection_3d, intersection_2d, ray_t ))
00382 {
00383
00384 Ogre::Vector3 grab_rel_center = grab_point_ - rotation_center;
00385 Ogre::Vector3 mouse_rel_center = intersection_3d - rotation_center;
00386
00387 Ogre::Quaternion orientation_change_since_mouse_down =
00388 grab_rel_center.getRotationTo( mouse_rel_center, rotation_axis );
00389
00390 Ogre::Radian rot;
00391 Ogre::Vector3 axis;
00392 orientation_change_since_mouse_down.ToAngleAxis( rot, axis );
00393
00394
00395
00396
00397
00398 Ogre::Radian rotation_since_mouse_down = rot * axis.dotProduct( rotation_axis );
00399
00400 rotation_ = rotation_at_mouse_down_ + rotation_since_mouse_down;
00401 parent_->setPose( parent_->getPosition(),
00402 orientation_change_since_mouse_down * parent_orientation_at_mouse_down_,
00403 name_ );
00404 }
00405 }
00406
00407 void InteractiveMarkerControl::movePlane( Ogre::Ray &mouse_ray )
00408 {
00409 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00410 drag_viewport_ )
00411 {
00412 updateControlOrientationForViewFacing( drag_viewport_ );
00413 }
00414
00415 Ogre::Vector3 intersection_3d;
00416 Ogre::Vector2 intersection_2d;
00417 float ray_t;
00418
00419 if( intersectSomeYzPlane( mouse_ray, grab_point_, control_frame_node_->getOrientation(),
00420 intersection_3d, intersection_2d, ray_t ))
00421 {
00422 parent_->setPose( intersection_3d - grab_point_ + parent_position_at_mouse_down_, parent_->getOrientation(), name_ );
00423 }
00424 }
00425
00428 void InteractiveMarkerControl::worldToScreen( const Ogre::Vector3& pos_rel_reference,
00429 const Ogre::Viewport* viewport,
00430 Ogre::Vector2& screen_pos )
00431 {
00432 Ogre::Vector3 world_pos = reference_node_->convertLocalToWorldPosition( pos_rel_reference );
00433
00434 const Ogre::Camera* cam = viewport->getCamera();
00435 Ogre::Vector3 homogeneous_screen_position = cam->getProjectionMatrix() * (cam->getViewMatrix() * world_pos);
00436
00437 double half_width = viewport->getActualWidth() / 2.0;
00438 double half_height = viewport->getActualHeight() / 2.0;
00439
00440 screen_pos.x = half_width + (half_width * homogeneous_screen_position.x) - .5;
00441 screen_pos.y = half_height + (half_height * -homogeneous_screen_position.y) - .5;
00442 }
00443
00447 bool InteractiveMarkerControl::findClosestPoint( const Ogre::Ray& target_ray,
00448 const Ogre::Ray& mouse_ray,
00449 Ogre::Vector3& closest_point )
00450 {
00451
00452
00453
00454
00455
00456
00457 Ogre::Vector3 v13 = target_ray.getOrigin() - mouse_ray.getOrigin();
00458 Ogre::Vector3 v43 = mouse_ray.getDirection();
00459 Ogre::Vector3 v21 = target_ray.getDirection();
00460 double d1343 = v13.dotProduct( v43 );
00461 double d4321 = v43.dotProduct( v21 );
00462 double d1321 = v13.dotProduct( v21 );
00463 double d4343 = v43.dotProduct( v43 );
00464 double d2121 = v21.dotProduct( v21 );
00465
00466 double denom = d2121 * d4343 - d4321 * d4321;
00467 if( fabs( denom ) <= Ogre::Matrix3::EPSILON )
00468 {
00469 return false;
00470 }
00471 double numer = d1343 * d4321 - d1321 * d4343;
00472
00473 double mua = numer / denom;
00474 closest_point = target_ray.getPoint( mua );
00475 return true;
00476 }
00477
00478 void InteractiveMarkerControl::moveAxis( const Ogre::Ray& mouse_ray, const ViewportMouseEvent& event )
00479 {
00480
00481 Ogre::Ray control_ray;
00482 control_ray.setOrigin( grab_point_ );
00483 control_ray.setDirection( control_frame_node_->getOrientation() * control_orientation_.xAxis() );
00484
00485
00486 Ogre::Vector2 control_ray_screen_start, control_ray_screen_end;
00487 worldToScreen( control_ray.getOrigin(), event.viewport, control_ray_screen_start );
00488 worldToScreen( control_ray.getPoint( 1 ), event.viewport, control_ray_screen_end );
00489
00490 Ogre::Vector2 mouse_point( event.x, event.y );
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501 Ogre::Vector2 control_ray_screen_dir = control_ray_screen_end - control_ray_screen_start;
00502 double denominator = control_ray_screen_dir.dotProduct( control_ray_screen_dir );
00503 if( fabs( denominator ) > Ogre::Matrix3::EPSILON )
00504 {
00505 double factor =
00506 ( mouse_point - control_ray_screen_start ).dotProduct( control_ray_screen_dir ) / denominator;
00507
00508 Ogre::Vector2 closest_screen_point = control_ray_screen_start + control_ray_screen_dir * factor;
00509
00510
00511 int width = event.viewport->getActualWidth() - 1;
00512 int height = event.viewport->getActualHeight() - 1;
00513 Ogre::Ray new_mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (closest_screen_point.x+.5) / width,
00514 (closest_screen_point.y+.5) / height );
00515 new_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( new_mouse_ray.getOrigin() ) );
00516 new_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * new_mouse_ray.getDirection() );
00517
00518
00519 Ogre::Vector3 closest_point;
00520 if( findClosestPoint( control_ray, new_mouse_ray, closest_point ))
00521 {
00522
00523 parent_->setPose( closest_point - grab_point_ + parent_position_at_mouse_down_,
00524 parent_->getOrientation(), name_ );
00525 }
00526 }
00527 }
00528
00529 void InteractiveMarkerControl::moveRotate( Ogre::Ray &mouse_ray )
00530 {
00531 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00532 drag_viewport_ )
00533 {
00534 updateControlOrientationForViewFacing( drag_viewport_ );
00535 }
00536
00537 Ogre::Vector3 new_drag_rel_ref;
00538 Ogre::Vector2 intersection_2d;
00539 float ray_t;
00540
00541
00542
00543
00544
00545
00546
00547 Ogre::Matrix4 control_rel_ref;
00548 control_rel_ref.makeTransform( control_frame_node_->getPosition(),
00549 Ogre::Vector3::UNIT_SCALE,
00550 control_frame_node_->getOrientation() );
00551 Ogre::Vector3 rotation_center = control_rel_ref * rotation_center_rel_control_;
00552
00553
00554
00555
00556 Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
00557
00558
00559
00560 if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_node_->getOrientation(),
00561 new_drag_rel_ref, intersection_2d, ray_t ))
00562 {
00563
00564
00565
00566
00567
00568
00569
00570 Ogre::Vector3 prev_rel_center = prev_drag_rel_ref - rotation_center;
00571 Ogre::Vector3 new_rel_center = new_drag_rel_ref - rotation_center;
00572 if( new_rel_center.length() > Ogre::Matrix3::EPSILON )
00573 {
00574 Ogre::Quaternion rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis_ );
00575 Ogre::Radian rot;
00576 Ogre::Vector3 axis;
00577 rotation_change.ToAngleAxis( rot, axis );
00578
00579
00580
00581
00582
00583 Ogre::Radian angle_change = rot * axis.dotProduct( rotation_axis_ );
00584 rotation_ += angle_change;
00585 parent_->rotate( rotation_change, name_ );
00586
00587
00588
00589
00590 parent_->translate( new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length()), name_ );
00591 }
00592 }
00593 }
00594
00595 void InteractiveMarkerControl::setHighlight( float a )
00596 {
00597 std::set<Ogre::Pass*>::iterator it;
00598 for (it = highlight_passes_.begin(); it != highlight_passes_.end(); it++)
00599 {
00600 (*it)->setAmbient(a,a,a);
00601 }
00602
00603 std::vector<PointsMarkerPtr>::iterator pm_it;
00604 for( pm_it = points_markers_.begin(); pm_it != points_markers_.end(); pm_it++ )
00605 {
00606 (*pm_it)->setHighlightColor( a, a, a );
00607 }
00608 }
00609
00610 void InteractiveMarkerControl::recordDraggingInPlaceEvent( ViewportMouseEvent& event )
00611 {
00612 dragging_in_place_event_ = event;
00613 dragging_in_place_event_.type = QEvent::MouseMove;
00614 }
00615
00616 void InteractiveMarkerControl::handleMouseEvent( ViewportMouseEvent& event )
00617 {
00618
00619
00620
00621
00622
00623 if( event.type == QEvent::FocusIn )
00624 {
00625 has_focus_ = true;
00626 std::set<Ogre::Pass*>::iterator it;
00627 setHighlight( HOVER_HIGHLIGHT );
00628 }
00629 else if( event.type == QEvent::FocusOut )
00630 {
00631 has_focus_ = false;
00632 setHighlight(0.0);
00633 return;
00634 }
00635
00636
00637 switch( interaction_mode_ )
00638 {
00639 case visualization_msgs::InteractiveMarkerControl::BUTTON:
00640 if( event.leftUp() )
00641 {
00642 Ogre::Vector3 point_rel_world;
00643 bool got_3D_point = vis_manager_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, point_rel_world );
00644
00645 visualization_msgs::InteractiveMarkerFeedback feedback;
00646 feedback.event_type = visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK;
00647 feedback.control_name = name_;
00648 feedback.marker_name = parent_->getName();
00649 parent_->publishFeedback( feedback, got_3D_point, point_rel_world );
00650 }
00651 break;
00652
00653 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00654 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00655 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00656 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00657 if( event.leftDown() )
00658 {
00659 parent_->startDragging();
00660 dragging_ = true;
00661 drag_viewport_ = event.viewport;
00662
00663 recordDraggingInPlaceEvent( event );
00664 if( ! vis_manager_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, grab_point_ ))
00665 {
00666
00667
00668 grab_point_ = control_frame_node_->getPosition();
00669 }
00670 else
00671 {
00672
00673
00674 grab_point_ = reference_node_->convertWorldToLocalPosition(grab_point_);
00675 }
00676 grab_pixel_.x = event.x;
00677 grab_pixel_.y = event.y;
00678 parent_position_at_mouse_down_ = parent_->getPosition();
00679 parent_orientation_at_mouse_down_ = parent_->getOrientation();
00680
00681 if( orientation_mode_ == visualization_msgs::InteractiveMarkerControl::VIEW_FACING &&
00682 drag_viewport_ )
00683 {
00684 updateControlOrientationForViewFacing( drag_viewport_ );
00685 }
00686 control_frame_orientation_at_mouse_down_ = control_frame_node_->getOrientation();
00687 rotation_at_mouse_down_ = rotation_;
00688
00689 rotation_axis_ = control_frame_node_->getOrientation() * control_orientation_.xAxis();
00690
00691
00692
00693 Ogre::Vector3 rotation_center_rel_ref = closestPointOnLineToPoint( parent_->getPosition(),
00694 rotation_axis_,
00695 grab_point_ );
00696 Ogre::Matrix4 reference_rel_control_frame;
00697 reference_rel_control_frame.makeInverseTransform( control_frame_node_->getPosition(),
00698 Ogre::Vector3::UNIT_SCALE,
00699 control_frame_node_->getOrientation() );
00700 rotation_center_rel_control_ = reference_rel_control_frame * rotation_center_rel_ref;
00701 grab_point_rel_control_ = reference_rel_control_frame * grab_point_;
00702 }
00703 if( event.leftUp() )
00704 {
00705 dragging_ = false;
00706 drag_viewport_ = NULL;
00707 parent_->stopDragging();
00708 }
00709 break;
00710
00711 default:
00712 break;
00713 }
00714
00715 if( event.leftDown() )
00716 {
00717 setHighlight( ACTIVE_HIGHLIGHT );
00718 }
00719 else if( event.leftUp() )
00720 {
00721 setHighlight( HOVER_HIGHLIGHT );
00722 }
00723
00724 if (!parent_->handleMouseEvent(event, name_))
00725 {
00726 if( event.type == QEvent::MouseMove && event.left() )
00727 {
00728 recordDraggingInPlaceEvent( event );
00729 handleMouseMovement( event );
00730 }
00731 }
00732 }
00733
00734 void InteractiveMarkerControl::handleMouseMovement( ViewportMouseEvent& event )
00735 {
00736
00737 float width = event.viewport->getActualWidth() - 1;
00738 float height = event.viewport->getActualHeight() - 1;
00739
00740 Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (event.x + .5) / width,
00741 (event.y + .5) / height );
00742
00743 Ogre::Ray last_mouse_ray =
00744 event.viewport->getCamera()->getCameraToViewportRay(
00745 (event.last_x + .5) / width, (event.last_y + .5) / height);
00746
00747
00748 mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
00749 mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
00750
00751 last_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( last_mouse_ray.getOrigin() ) );
00752 last_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * last_mouse_ray.getDirection() );
00753
00754 switch (interaction_mode_)
00755 {
00756 case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
00757 moveAxis( mouse_ray, event );
00758 break;
00759
00760 case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
00761 movePlane( mouse_ray );
00762 break;
00763
00764 case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
00765 moveRotate( mouse_ray );
00766 break;
00767
00768 case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
00769 rotate(mouse_ray);
00770 break;
00771
00772 default:
00773 break;
00774 }
00775 }
00776
00777 bool InteractiveMarkerControl::intersectYzPlane( const Ogre::Ray& mouse_ray,
00778 Ogre::Vector3& intersection_3d,
00779 Ogre::Vector2& intersection_2d,
00780 float &ray_t )
00781 {
00782 return intersectSomeYzPlane( mouse_ray,
00783 control_frame_node_->getPosition(),
00784 control_frame_node_->getOrientation(),
00785 intersection_3d, intersection_2d, ray_t );
00786 }
00787
00788 bool InteractiveMarkerControl::intersectSomeYzPlane( const Ogre::Ray& mouse_ray,
00789 const Ogre::Vector3& point_on_plane,
00790 const Ogre::Quaternion& plane_orientation,
00791 Ogre::Vector3& intersection_3d,
00792 Ogre::Vector2& intersection_2d,
00793 float& ray_t )
00794 {
00795 Ogre::Vector3 normal = plane_orientation * control_orientation_.xAxis();
00796 Ogre::Vector3 axis_1 = plane_orientation * control_orientation_.yAxis();
00797 Ogre::Vector3 axis_2 = plane_orientation * control_orientation_.zAxis();
00798
00799 Ogre::Plane plane(normal, point_on_plane);
00800
00801 Ogre::Vector2 origin_2d(point_on_plane.dotProduct(axis_1), point_on_plane.dotProduct(axis_2));
00802
00803 std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects(plane);
00804 if (intersection.first)
00805 {
00806 intersection_3d = mouse_ray.getPoint(intersection.second);
00807 intersection_2d = Ogre::Vector2(intersection_3d.dotProduct(axis_1), intersection_3d.dotProduct(axis_2));
00808 intersection_2d -= origin_2d;
00809
00810 ray_t = intersection.second;
00811 return true;
00812 }
00813
00814 ray_t = 0;
00815 return false;
00816 }
00817
00818 void InteractiveMarkerControl::addHighlightPass( S_MaterialPtr materials )
00819 {
00820 S_MaterialPtr::iterator it;
00821
00822 for (it = materials.begin(); it != materials.end(); it++)
00823 {
00824 Ogre::MaterialPtr material = *it;
00825 Ogre::Pass *original_pass = material->getTechnique(0)->getPass(0);
00826 Ogre::Pass *pass = material->getTechnique(0)->createPass();
00827
00828 pass->setSceneBlending(Ogre::SBT_ADD);
00829 pass->setDepthWriteEnabled(true);
00830 pass->setDepthCheckEnabled(true);
00831 pass->setLightingEnabled(true);
00832 pass->setAmbient(0, 0, 0);
00833 pass->setDiffuse(0, 0, 0, 0);
00834 pass->setSpecular(0, 0, 0, 0);
00835 pass->setCullingMode(original_pass->getCullingMode());
00836
00837 highlight_passes_.insert(pass);
00838 }
00839 }
00840
00841 }