Tweak focal shape size marker depending on focal distance (#1021)

src/rviz/default_plugin/view_controllers/orbit_view_controller.cpp

@@ -39,6 +39,7 @@
 #include "rviz/display_context.h"
 #include "rviz/geometry.h"
 #include "rviz/ogre_helpers/shape.h"
+#include "rviz/properties/bool_property.h"
 #include "rviz/properties/float_property.h"
 #include "rviz/properties/vector_property.h"
 #include "rviz/uniform_string_stream.h"
@@ -51,6 +52,8 @@
 static const float PITCH_START = Ogre::Math::HALF_PI / 2.0;
 static const float YAW_START = Ogre::Math::HALF_PI * 0.5;
 static const float DISTANCE_START = 10;
+static const float FOCAL_SHAPE_SIZE_START = 0.05;
+static const bool FOCAL_SHAPE_FIXED_SIZE = true;
 
 namespace rviz
 {
@@ -61,6 +64,11 @@ OrbitViewController::OrbitViewController()
   distance_property_ = new FloatProperty( "Distance", DISTANCE_START, "Distance from the focal point.", this );
   distance_property_->setMin( 0.01 );
 
+  focal_shape_size_property_ = new FloatProperty( "Focal Shape Size", FOCAL_SHAPE_SIZE_START, "Focal shape size.", this );
+  focal_shape_size_property_->setMin( 0.001 );
+
+  focal_shape_fixed_size_property_ = new BoolProperty ( "Focal Shape Fixed Size", FOCAL_SHAPE_FIXED_SIZE, "Focal shape size.", this );
+
   yaw_property_ = new FloatProperty( "Yaw", YAW_START, "Rotation of the camera around the Z (up) axis.", this );
 
   pitch_property_ = new FloatProperty( "Pitch", PITCH_START, "How much the camera is tipped downward.", this );
@@ -77,7 +85,7 @@ void OrbitViewController::onInitialize()
   camera_->setProjectionType( Ogre::PT_PERSPECTIVE );
 
   focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), target_scene_node_);
-  focal_shape_->setScale(Ogre::Vector3(0.05f, 0.05f, 0.01f));
+  updateFocalShapeSize();
   focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
   focal_shape_->getRootNode()->setVisible(false);
 }
@@ -93,6 +101,9 @@ void OrbitViewController::reset()
   yaw_property_->setFloat( YAW_START );
   pitch_property_->setFloat( PITCH_START );
   distance_property_->setFloat( DISTANCE_START );
+  focal_shape_size_property_->setFloat( FOCAL_SHAPE_SIZE_START );
+  focal_shape_fixed_size_property_->setBool( false );
+  updateFocalShapeSize();
   focal_point_property_->setVector( Ogre::Vector3::ZERO );
 }
 
@@ -108,6 +119,7 @@ void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
   }
 
   float distance = distance_property_->getFloat();
+  updateFocalShapeSize();
 
   int32_t diff_x = 0;
   int32_t diff_y = 0;
@@ -209,12 +221,14 @@ void OrbitViewController::mimic( ViewController* source_view )
   {
     // If I'm initializing from another instance of this same class, get the distance exactly.
     distance_property_->setFloat( source_view->subProp( "Distance" )->getValue().toFloat() );
+    updateFocalShapeSize();
   }
   else
   {
     // Determine the distance from here to the reference frame, and use
     // that as the distance our focal point should be at.
     distance_property_->setFloat( position.length() );
+    updateFocalShapeSize();
   }
 
   Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_property_->getFloat() );
@@ -234,7 +248,7 @@ void OrbitViewController::lookAt( const Ogre::Vector3& point )
   Ogre::Vector3 camera_position = camera_->getPosition();
   focal_point_property_->setVector( target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()) );
   distance_property_->setFloat( focal_point_property_->getVector().distance( camera_position ));
-
+  updateFocalShapeSize();
   calculatePitchYawFromPosition(camera_position);
 }
 
@@ -255,6 +269,7 @@ void OrbitViewController::updateCamera()
   float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
   float z = distance *              sin( pitch ) + focal_point.z;
 
+
   Ogre::Vector3 pos( x, y, z );
 
   camera_->setPosition(pos);
@@ -281,9 +296,22 @@ void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& po
   yaw_property_->setFloat( atan2( diff.y, diff.x ));
 }
 
+void OrbitViewController::updateFocalShapeSize()
+{
+  const double fshape_size( focal_shape_size_property_->getFloat() );
+  double distance_property( distance_property_->getFloat() );
+  if( focal_shape_fixed_size_property_->getBool() )
+    distance_property = 1;
+
+  focal_shape_->setScale( Ogre::Vector3( fshape_size * distance_property,
+                                         fshape_size * distance_property,
+                                         fshape_size * distance_property / 5.0 ) );
+}
+
 void OrbitViewController::zoom( float amount )
 {
   distance_property_->add( -amount );
+  updateFocalShapeSize();
 }
 
 void OrbitViewController::move( float x, float y, float z )

src/rviz/default_plugin/view_controllers/orbit_view_controller.h

@@ -38,6 +38,7 @@
 
 namespace rviz
 {
+class BoolProperty;
 class FloatProperty;
 class Shape;
 class SceneNode;
@@ -101,12 +102,19 @@ Q_OBJECT
    */
   void calculatePitchYawFromPosition( const Ogre::Vector3& position );
 
+  /**
+   * \brief Calculates the focal shape size and update it's geometry
+   */
+  void updateFocalShapeSize();
+
   virtual void updateCamera();
 
   FloatProperty* yaw_property_;                         ///< The camera's yaw (rotation around the y-axis), in radians
   FloatProperty* pitch_property_;                       ///< The camera's pitch (rotation around the x-axis), in radians
   FloatProperty* distance_property_;                    ///< The camera's distance from the focal point
   VectorProperty* focal_point_property_; ///< The point around which the camera "orbits".
+  BoolProperty* focal_shape_fixed_size_property_;       ///< Whether the focal shape size is fixed or not
+  FloatProperty* focal_shape_size_property_;            ///< The focal shape size
 
   Shape* focal_shape_;
   bool dragging_;