camera.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, 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 "rve_render_client/camera.h"
#include "rve_render_client/scene.h"
#include "rve_render_client/perspective_projection_factory.h"

#include <rve_interfaces/Camera.h>
#include <rve_common/eigen_conversions.h>

#include <boost/bind.hpp>

#include <Eigen/Geometry>

using namespace rve_common;

namespace rve_render_client
{

CameraPtr createCamera(Scene* scene)
{
  CameraPtr cam(new Camera, destroySceneObject);
  scene->addObject(cam.get());
  return cam;
}

Camera::Camera()
  : scene_(0)
{
  orientation_.w = 1.0;
  proxy_index_ = addProxy("camera");
  setProjectionMatrixFactory( ProjectionMatrixFactoryPtr( new PerspectiveProjectionFactory() ));
}

Camera::~Camera()
{

}

void Camera::doCreate(ContextInfo& context)
{
  ROS_ASSERT(!scene_ || context.scene == scene_);
  scene_ = context.scene;

  rve_interfaces::CameraProxy* proxy = getProxy(context);
  const rve_common::UUID& scene_id = getSceneID(context.scene);
  proxy->create(scene_id, getID());
  proxy->setPosition(scene_id, getID(), position_);
  proxy->setOrientation(scene_id, getID(), orientation_);
  proxy->setProjectionMatrix(scene_id, getID(), projection_matrix_);
}

void Camera::doDestroy(ContextInfo& context)
{
  rve_interfaces::CameraProxy* proxy = getProxy(context);
  proxy->destroyAsync(getSceneID(context.scene), getID());

  if (getContexts().empty())
  {
    scene_ = 0;
  }
}

rve_interfaces::CameraProxy* Camera::getProxy(const ContextInfo& context)
{
  return SceneObject::getProxy<rve_interfaces::CameraProxy>(context, proxy_index_);
}

#define CAMERA_MULTIPLEX(func, ...) \
    multiplex<rve_interfaces::CameraProxy>(boost::bind(&rve_interfaces::CameraProxy::func, _1, _2, getID(), __VA_ARGS__), proxy_index_);

void Camera::setPosition(const rve_msgs::Vector3& pos)
{
  position_ = pos;
  CAMERA_MULTIPLEX(setPositionAsync, pos);
}

void Camera::setOrientation(const rve_msgs::Quaternion& orient)
{
  orientation_ = orient;
  CAMERA_MULTIPLEX(setOrientationAsync, orient);
}

void Camera::setDirection(const rve_msgs::Vector3& d)
{
  Eigen::Vector3f dir(msgToEigen(d));
  if (dir.isZero())
  {
    return;
  }

  Eigen::Quaternionf orient;
  orient.setFromTwoVectors(Eigen::Vector3f::UnitX(), dir);
  setOrientation(eigenToMsg(orient));
}

void Camera::lookAt(const rve_msgs::Vector3& point)
{
  Eigen::Vector3f dir = msgToEigen(point) - msgToEigen(position_);
  setDirection(eigenToMsg(dir));
}

void Camera::move(const rve_msgs::Vector3& v)
{
  Eigen::Vector3f vec(msgToEigen(v));
  Eigen::Vector3f pos(msgToEigen(position_));

  pos = pos + vec;
  setPosition(eigenToMsg(pos));
}

void Camera::moveRelative(const rve_msgs::Vector3& v)
{
  Eigen::Vector3f vec(msgToEigen(v));
  Eigen::Vector3f pos(msgToEigen(position_));
  Eigen::Quaternionf orient(msgToEigen(orientation_));

  pos = pos + (orient * vec);
  setPosition(eigenToMsg(pos));
}

void Camera::rotate(const rve_msgs::Quaternion& q)
{
  // Normalise the quat to avoid cumulative problems with precision
  Eigen::Quaternionf qnorm(msgToEigen(q));
  qnorm.normalize();
  Eigen::Quaternionf orient = msgToEigen(orientation_) * qnorm;
  setOrientation(eigenToMsg(orient));
}

void Camera::setTransform(const rve_msgs::Vector3& pos, const rve_msgs::Quaternion& orient)
{
  setPosition(pos);
  setOrientation(orient);
}

void Camera::setProjectionMatrix(const rve_msgs::Matrix4& matrix)
{
  projection_matrix_ = matrix;
  CAMERA_MULTIPLEX(setProjectionMatrixAsync, matrix);
}

void Camera::setProjectionMatrixFactory(const ProjectionMatrixFactoryPtr& factory)
{
  projection_matrix_factory_ = factory;
  if( projection_matrix_factory_ )
  {
    projection_matrix_factory_->setAspectRatio( aspect_ratio_ );
    projection_matrix_factory_->setCamera( this );
  }
}

void Camera::setAspectRatio( float width_over_height )
{
  aspect_ratio_ = width_over_height;
  if( projection_matrix_factory_ )
  {
    projection_matrix_factory_->setAspectRatio( aspect_ratio_ );
  }
}

}