interactive_marker_control.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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     // create a marker with the given type
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     // The marker will set its position relative to the fixed frame,
00149     // but we have attached it our own scene node, so we will have to
00150     // correct for that.
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   // highlight_passes_ have raw pointers into the markers_, so must
00198   // clear them at the same time.
00199   highlight_passes_.clear();
00200   markers_.clear();
00201   points_markers_.clear();
00202 
00203   // Initially, the pose of this marker's node and the interactive
00204   // marker are identical, but that may change.
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   // It's not clear to me why this one setOrientation() call needs to
00222   // be here and not above makeMarkers() with the other
00223   // setOrientation() calls, but it works correctly when here and
00224   // incorrectly when there.  Sorry. -hersh
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 // This is an Ogre::SceneManager::Listener function, and is configured
00235 // to be called only if this is a VIEW_FACING control.
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   // rotate so z axis is up
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   // rotate
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     // we need to refresh the node manually, since the scene manager will only do this one frame
00264     // later otherwise
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   // Find closest point on projected ray to mouse point
00352   // Math: if P is the start of the ray, v is the direction vector of
00353   //       the ray (not normalized), and X is the test point, then the
00354   //       closest point on the line to X is given by:
00355   //
00356   //               (X-P).v
00357   //       P + v * -------
00358   //                 v.v
00359   //       where "." is the dot product.
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   // Find rotation_center = 3D point closest to grab_point_ which is
00374   // on the rotation axis, relative to the reference frame.
00375   Ogre::Vector3 rotation_center = closestPointOnLineToPoint( control_frame_node_->getPosition(),
00376                                                              rotation_axis,
00377                                                              grab_point_ );
00378 
00379   // Find intersection of mouse_ray with plane centered at rotation_center.
00380   if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation_at_mouse_down_,
00381                             intersection_3d, intersection_2d, ray_t ))
00382   {
00383     // Find rotation
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     // Quaternion::ToAngleAxis() always gives a positive angle.  The
00394     // axis it emits (in this case) will either be equal to
00395     // rotation_axis or will be the negative of it.  Doing the
00396     // dot-product then gives either 1.0 or -1.0, which is just what
00397     // we need to get the sign for our angle.
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   // Find the closest point on target_ray to any point on mouse_ray.
00452   //
00453   // Math taken from http://paulbourke.net/geometry/lineline3d/
00454   // line P1->P2 is target_ray
00455   // line P3->P4 is mouse_ray
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   // compute control-axis ray based on grab_point_, etc.
00481   Ogre::Ray control_ray;
00482   control_ray.setOrigin( grab_point_ );
00483   control_ray.setDirection( control_frame_node_->getOrientation() * control_orientation_.xAxis() );
00484   
00485   // project control-axis ray onto screen.
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   // Find closest point on projected ray to mouse point
00493   // Math: if P is the start of the ray, v is the direction vector of
00494   //       the ray (not normalized), and X is the test point, then the
00495   //       closest point on the line to X is given by:
00496   //
00497   //               (X-P).v
00498   //       P + v * -------
00499   //                 v.v
00500   //       where "." is the dot product.
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 ) // If the control ray is not straight in line with the view.
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     // make a new "mouse ray" for the point on the projected ray
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     // find closest point on control-axis ray to new mouse ray (should intersect actually)
00519     Ogre::Vector3 closest_point;
00520     if( findClosestPoint( control_ray, new_mouse_ray, closest_point ))
00521     {
00522       // set position of parent to closest_point - grab_point_ + parent_position_at_mouse_down_.
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   // get rotation axis rel ref (constant for entire drag)
00542   //  - rotation_axis_
00543 
00544   // get current rotation center rel ref
00545   //  - compute rotation center rel control frame at mouse-down (constant for entire drag)
00546   //  - current rotation center rel ref = current control frame * above
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   // get previous 3D drag point rel ref
00554   //  - compute grab point rel control frame at mouse-down (constant for entire drag)
00555   //  - prev_drag_rel_ref = current control frame + above
00556   Ogre::Vector3 prev_drag_rel_ref = control_rel_ref * grab_point_rel_control_;
00557 
00558   // get new 3D drag point rel ref (this is "intersection_3d" in rotate().)
00559   //  - intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_orientation )
00560   if( intersectSomeYzPlane( mouse_ray, rotation_center, control_frame_node_->getOrientation(),
00561                             new_drag_rel_ref, intersection_2d, ray_t ))
00562   {
00563     // compute rotation angle from old drag point to new.
00564     //  - prev_rel_center = prev_drag_rel_ref - rotation_center
00565     //  - new_rel_center = new_drag_rel_ref - rotation_center
00566     //  - rotation_change = prev_rel_center.getRotationTo( new_rel_center, rotation_axis )
00567     //  - get Radians of angle change
00568     //  - rotation_ += angle_change
00569     //  - parent_->rotate(rotation_change)
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       // Quaternion::ToAngleAxis() always gives a positive angle.  The
00579       // axis it emits (in this case) will either be equal to
00580       // rotation_axis or will be the negative of it.  Doing the
00581       // dot-product then gives either 1.0 or -1.0, which is just what
00582       // we need to get the sign for our angle.
00583       Ogre::Radian angle_change = rot * axis.dotProduct( rotation_axis_ );
00584       rotation_ += angle_change;
00585       parent_->rotate( rotation_change, name_ );
00586 
00587       // compute translation from rotated old drag point to new drag point.
00588       //  - pos_change = new_rel_center * (1.0 - prev_rel_center.length() / new_rel_center.length())
00589       //  - parent_->translate(pos_change)
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   // * check if this is just a receive/lost focus event
00619   // * try to hand over the mouse event to the parent interactive marker
00620   // * otherwise, execute mouse move handling
00621 
00622   // handle receive/lose focus
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   // change dragging state
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         // If we couldn't get a 3D point for the grab, just use the
00667         // current relative position of the control frame.
00668         grab_point_ = control_frame_node_->getPosition();
00669       }
00670       else
00671       {
00672         // If we could get a 3D point for the grab, convert it from
00673         // the world frame to the reference frame (in case those are different).
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       // Find rotation_center = 3D point closest to grab_point_ which is
00692       // on the rotation axis, relative to the reference frame.
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   // handle mouse movement
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   //convert rays into reference frame
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 }


rviz
Author(s): Dave Hershberger, Josh Faust
autogenerated on Mon Jan 6 2014 11:54:32