orbit_camera.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, 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 "orbit_camera.h"
#include "shape.h"

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

#include <stdint.h>
#include <sstream>

#define MIN_DISTANCE 0.01

namespace rviz
{

static const float PITCH_LIMIT_LOW = 0.001;
static const float PITCH_LIMIT_HIGH = Ogre::Math::PI - 0.001;
static const float YAW_START = Ogre::Math::PI;// - 0.001;
static const float PITCH_START = Ogre::Math::HALF_PI;

OrbitCamera::OrbitCamera( Ogre::SceneManager* scene_manager )
: CameraBase( scene_manager )
, focal_point_( Ogre::Vector3::ZERO )
, yaw_( YAW_START )
, pitch_( PITCH_START )
, distance_( 10.0f )
{
  focal_point_object_ = new Shape( Shape::Sphere, scene_manager );
  focal_point_object_->setScale( Ogre::Vector3( 0.05f, 0.01f, 0.05f ) );
  focal_point_object_->setColor( 1.0f, 1.0f, 0.0f, 0.5f );
  focal_point_object_->getRootNode()->setVisible( false );

  update();
}

OrbitCamera::~OrbitCamera()
{
  delete focal_point_object_;
}

void OrbitCamera::normalizePitch()
{
  if ( pitch_ < PITCH_LIMIT_LOW )
  {
    pitch_ = PITCH_LIMIT_LOW;
  }
  else if ( pitch_ > PITCH_LIMIT_HIGH )
  {
    pitch_ = PITCH_LIMIT_HIGH;
  }
}

void OrbitCamera::normalizeYaw()
{
  yaw_ = fmod( yaw_, Ogre::Math::TWO_PI );

  if ( yaw_ < 0.0f )
  {
    yaw_ = Ogre::Math::TWO_PI + yaw_;
  }
}

Ogre::Vector3 OrbitCamera::getGlobalFocalPoint()
{
  Ogre::Vector3 global_focal_point = focal_point_;

  if ( relative_node_ )
  {
    global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
  }

  return global_focal_point;
}

void OrbitCamera::update()
{
  Ogre::Vector3 global_focal_point = getGlobalFocalPoint();

  float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + global_focal_point.x;
  float y = distance_ * cos( pitch_ ) + global_focal_point.y;
  float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + global_focal_point.z;

  Ogre::Vector3 pos( x, y, z );

  if ( relative_node_ )
  {
    Ogre::Vector3 vec = pos - global_focal_point;
    pos = relative_node_->getOrientation() * vec + global_focal_point;

    camera_->setFixedYawAxis(true, relative_node_->getOrientation() * Ogre::Vector3::UNIT_Y);
  }

  camera_->setPosition( pos );
  camera_->lookAt( global_focal_point );

  focal_point_object_->setPosition( global_focal_point );
}

void OrbitCamera::yaw( float angle )
{
  yaw_ += angle;

  normalizeYaw();

  update();
}

void OrbitCamera::pitch( float angle )
{
  pitch_ += angle;

  normalizePitch();

  update();
}

void OrbitCamera::roll( float angle )
{
}

Ogre::Vector3 OrbitCamera::getPosition()
{
  return camera_->getPosition();
}

Ogre::Quaternion OrbitCamera::getOrientation()
{
  return camera_->getOrientation();
}

void OrbitCamera::calculatePitchYawFromPosition( const Ogre::Vector3& position )
{
  float x = position.x - focal_point_.x;
  float y = position.y - focal_point_.y;
  pitch_ = acos( y / distance_ );

  normalizePitch();

  float val = x / ( distance_ * sin( pitch_ ) );

  yaw_ = acos( val );

  Ogre::Vector3 direction = focal_point_ - position;

  if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
  {
    yaw_ = Ogre::Math::TWO_PI - yaw_;
  }
}

void OrbitCamera::setFrom( CameraBase* camera )
{
  Ogre::Vector3 position = camera->getPosition();
  Ogre::Quaternion orientation = camera->getOrientation();

  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
  focal_point_ = position + direction;

  calculatePitchYawFromPosition( position );

  update();
}

void OrbitCamera::setOrientation( float x, float y, float z, float w )
{
  Ogre::Vector3 position = camera_->getPosition();
  Ogre::Quaternion orientation( w, x, y, z );

  Ogre::Vector3 direction = orientation * (Ogre::Vector3::NEGATIVE_UNIT_Z * distance_);
  focal_point_ = position + direction;

  calculatePitchYawFromPosition( position );

  update();
}

void OrbitCamera::zoom( float amount )
{
  distance_ -= amount;

  if ( distance_ <= MIN_DISTANCE )
  {
    distance_ = MIN_DISTANCE;
  }

  update();
}

void OrbitCamera::setFocalPoint( const Ogre::Vector3& focal_point )
{
  focal_point_ = focal_point;

  update();
}

void OrbitCamera::move( float x, float y, float z )
{
  Ogre::Quaternion orientation = camera_->getOrientation();

  if ( relative_node_ )
  {
    orientation = relative_node_->getOrientation().Inverse() * orientation;
  }

  focal_point_ += orientation * Ogre::Vector3( x, y, z );

  update();
}

void OrbitCamera::setPosition( float x, float y, float z )
{
  Ogre::Vector3 pos( x, y, z );
  distance_ = (pos - getGlobalFocalPoint()).length();

  calculatePitchYawFromPosition( Ogre::Vector3( x, y, z ) );

  update();
}

void OrbitCamera::lookAt( const Ogre::Vector3& point )
{
  Ogre::Vector3 focal_point = point;
  Ogre::Vector3 camera_position = camera_->getPosition();

  if ( relative_node_ )
  {
    Ogre::Vector3 rel_pos = relative_node_->getPosition();
    Ogre::Quaternion rel_orient = relative_node_->getOrientation();

    focal_point = rel_orient.Inverse() * ( focal_point - rel_pos );
    camera_position = rel_orient.Inverse() * ( camera_position - rel_pos );
  }

  distance_ = focal_point.distance( camera_position );
  focal_point_ = focal_point;

  update();
}

void OrbitCamera::mouseLeftDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
{
  yaw( diff_x*0.005 );
  pitch( -diff_y*0.005 );
}

void OrbitCamera::mouseMiddleDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
{
  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 );

}

void OrbitCamera::mouseRightDrag( int diff_x, int diff_y, bool ctrl, bool alt, bool shift )
{
  if (shift)
  {
    move(0.0f, 0.0f, diff_y * 0.1 * (distance_ / 10.0f));
  }
  else
  {
    zoom( -diff_y * 0.1 * (distance_ / 10.0f) );
  }
}

void OrbitCamera::scrollWheel( int diff, bool ctrl, bool alt, bool shift )
{
  if (shift)
  {
    move(0.0f, 0.0f, -diff * 0.01 * (distance_ / 10.0f));
  }
  else
  {
    zoom( diff * 0.01 * (distance_ / 10.0f) );
  }
}

void OrbitCamera::mouseLeftDown( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( true );
}

void OrbitCamera::mouseMiddleDown( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( true );
}

void OrbitCamera::mouseRightDown( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( true );
}

void OrbitCamera::mouseLeftUp( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( false );
}

void OrbitCamera::mouseMiddleUp( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( false );
}

void OrbitCamera::mouseRightUp( int x, int y )
{
  focal_point_object_->getRootNode()->setVisible( false );
}

void OrbitCamera::fromString(const std::string& str)
{
  std::istringstream iss(str);

  iss >> pitch_;
  iss.ignore();
  iss >> yaw_;
  iss.ignore();
  iss >> distance_;
  iss.ignore();
  iss >> focal_point_.x;
  iss.ignore();
  iss >> focal_point_.y;
  iss.ignore();
  iss >> focal_point_.z;

  update();
}

std::string OrbitCamera::toString()
{
  std::ostringstream oss;
  oss << pitch_ << " " << yaw_ << " " << distance_ << " " << focal_point_.x << " " << focal_point_.y << " " << focal_point_.z;

  return oss.str();
}

} // namespace rviz