orbit_view_controller.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <stdint.h>

#include <OgreCamera.h>
#include <OgreQuaternion.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#include <OgreVector3.h>
#include <OgreViewport.h>

#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"
#include "rviz/viewport_mouse_event.h"
#include "rviz/load_resource.h"
#include "rviz/render_panel.h"

#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"

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
{

OrbitViewController::OrbitViewController()
  : dragging_( false )
{
  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 );
  pitch_property_->setMax( Ogre::Math::HALF_PI - 0.001 );
  pitch_property_->setMin( -pitch_property_->getMax() );

  focal_point_property_ = new VectorProperty( "Focal Point", Ogre::Vector3::ZERO, "The center point which the camera orbits.", this );
}

void OrbitViewController::onInitialize()
{
  FramePositionTrackingViewController::onInitialize();

  camera_->setProjectionType( Ogre::PT_PERSPECTIVE );

  focal_shape_ = new Shape(Shape::Sphere, context_->getSceneManager(), target_scene_node_);
  updateFocalShapeSize();
  focal_shape_->setColor(1.0f, 1.0f, 0.0f, 0.5f);
  focal_shape_->getRootNode()->setVisible(false);
}

OrbitViewController::~OrbitViewController()
{
  delete focal_shape_;
}

void OrbitViewController::reset()
{
  dragging_ = false;
  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 );
}

void OrbitViewController::handleMouseEvent(ViewportMouseEvent& event)
{
  if ( event.shift() )
  {
    setStatus( "<b>Left-Click:</b> Move X/Y.  <b>Right-Click:</b>: Move Z.  <b>Mouse Wheel:</b>: Zoom.  " );
  }
  else
  {
    setStatus( "<b>Left-Click:</b> Rotate.  <b>Middle-Click:</b> Move X/Y.  <b>Right-Click/Mouse Wheel:</b>: Zoom.  <b>Shift</b>: More options." );
  }

  float distance = distance_property_->getFloat();
  updateFocalShapeSize();

  int32_t diff_x = 0;
  int32_t diff_y = 0;

  bool moved = false;

  if( event.type == QEvent::MouseButtonPress )
  {
    focal_shape_->getRootNode()->setVisible(true);
    moved = true;
    dragging_ = true;
  }
  else if( event.type == QEvent::MouseButtonRelease )
  {
    focal_shape_->getRootNode()->setVisible(false);
    moved = true;
    dragging_ = false;
  }
  else if( dragging_ && event.type == QEvent::MouseMove )
  {
    diff_x = event.x - event.last_x;
    diff_y = event.y - event.last_y;
    moved = true;
  }

  // regular left-button drag
  if( event.left() && !event.shift() )
  {
    setCursor( Rotate3D );
    yaw( diff_x*0.005 );
    pitch( -diff_y*0.005 );
  }
  // middle or shift-left drag
  else if( event.middle() || (event.shift() && event.left()) )
  {
    setCursor( MoveXY );
    float fovY = camera_->getFOVy().valueRadians();
    float fovX = 2.0f * atan( tan( fovY / 2.0f ) * camera_->getAspectRatio() );

    int width = camera_->getViewport()->getActualWidth();
    int height = camera_->getViewport()->getActualHeight();

    move( -((float)diff_x / (float)width) * distance * tan( fovX / 2.0f ) * 2.0f,
          ((float)diff_y / (float)height) * distance * tan( fovY / 2.0f ) * 2.0f,
          0.0f );
  }
  else if( event.right() )
  {
    if( event.shift() )
    {
      // move in z direction
      setCursor( MoveZ );
      move(0.0f, 0.0f, diff_y * 0.1 * (distance / 10.0f));
    }
    else
    {
      // zoom
      setCursor( Zoom );
      zoom( -diff_y * 0.1 * (distance / 10.0f) );
    }
  }
  else
  {
    setCursor( event.shift() ? MoveXY : Rotate3D );
  }

  moved = true;

  if( event.wheel_delta != 0 )
  {
    int diff = event.wheel_delta;
    if( event.shift() )
    {
      move( 0, 0, -diff * 0.001 * distance );
    }
    else
    {
      zoom( diff * 0.001 * distance );
    }

    moved = true;
  }

  if( moved )
  {
    context_->queueRender();
  }
}

void OrbitViewController::mimic( ViewController* source_view )
{
  FramePositionTrackingViewController::mimic( source_view );

  Ogre::Camera* source_camera = source_view->getCamera();
  Ogre::Vector3 position = source_camera->getPosition();
  Ogre::Quaternion orientation = source_camera->getOrientation();

  if( source_view->getClassId() == "rviz/Orbit" )
  {
    // 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() );
  focal_point_property_->setVector( position + direction );

  calculatePitchYawFromPosition( position );
}

void OrbitViewController::update(float dt, float ros_dt)
{
  FramePositionTrackingViewController::update( dt, ros_dt );
  updateCamera();
}

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);
}

void OrbitViewController::onTargetFrameChanged(const Ogre::Vector3& old_reference_position, const Ogre::Quaternion& old_reference_orientation)
{
  focal_point_property_->add( old_reference_position - reference_position_ );
}

void OrbitViewController::updateCamera()
{
  float distance = distance_property_->getFloat();
  float yaw = yaw_property_->getFloat();
  float pitch = pitch_property_->getFloat();
  Ogre::Vector3 camera_z = Ogre::Vector3::UNIT_Z;

  // If requested, turn the world upside down.
  if(this->invert_z_->getBool())
  {
    yaw = -yaw;
    pitch = -pitch;
    camera_z = -camera_z;
  }

  Ogre::Vector3 focal_point = focal_point_property_->getVector();

  float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
  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);
  camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * camera_z);
  camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));

  focal_shape_->setPosition( focal_point );
}

void OrbitViewController::yaw( float angle )
{
  yaw_property_->setFloat( mapAngleTo0_2Pi( yaw_property_->getFloat() - angle ));
}

void OrbitViewController::pitch( float angle )
{
  pitch_property_->add( -angle );
}

void OrbitViewController::calculatePitchYawFromPosition( const Ogre::Vector3& position )
{
  Ogre::Vector3 diff = position - focal_point_property_->getVector();
  pitch_property_->setFloat( asin( diff.z / distance_property_->getFloat() ));
  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 )
{
  focal_point_property_->add( camera_->getOrientation() * Ogre::Vector3( x, y, z ));
}

} // end namespace rviz

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS( rviz::OrbitViewController, rviz::ViewController )