source: pacpussensors/trunk/Vislab/lib3dv-1.2.0/lib3dv/eigen/demos/opengl/camera.cpp

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "camera.h"

#include "gpuhelper.h"
#include <GL/glu.h>

#include "Eigen/LU"
using namespace Eigen;

Camera::Camera()
    : mViewIsUptodate(false), mProjIsUptodate(false)
{
    mViewMatrix.setIdentity();
    
    mFovY = M_PI/3.;
    mNearDist = 1.;
    mFarDist = 50000.;
    
    mVpX = 0;
    mVpY = 0;

    setPosition(Vector3f::Constant(100.));
    setTarget(Vector3f::Zero());
}

Camera& Camera::operator=(const Camera& other)
{
    mViewIsUptodate = false;
    mProjIsUptodate = false;
    
    mVpX = other.mVpX;
    mVpY = other.mVpY;
    mVpWidth = other.mVpWidth;
    mVpHeight = other.mVpHeight;

    mTarget = other.mTarget;
    mFovY = other.mFovY;
    mNearDist = other.mNearDist;
    mFarDist = other.mFarDist;
    
    mViewMatrix = other.mViewMatrix;
    mProjectionMatrix = other.mProjectionMatrix;

    return *this;
}

Camera::Camera(const Camera& other)
{
    *this = other;
}

Camera::~Camera()
{
}


void Camera::setViewport(uint offsetx, uint offsety, uint width, uint height)
{
    mVpX = offsetx;
    mVpY = offsety;
    mVpWidth = width;
    mVpHeight = height;
    
    mProjIsUptodate = false;
}

void Camera::setViewport(uint width, uint height)
{
    mVpWidth = width;
    mVpHeight = height;
    
    mProjIsUptodate = false;
}

void Camera::setFovY(float value)
{
    mFovY = value;
    mProjIsUptodate = false;
}

Vector3f Camera::direction(void) const
{
    return - (orientation() * Vector3f::UnitZ());
}
Vector3f Camera::up(void) const
{
    return orientation() * Vector3f::UnitY();
}
Vector3f Camera::right(void) const
{
    return orientation() * Vector3f::UnitX();
}

void Camera::setDirection(const Vector3f& newDirection)
{
    // TODO implement it computing the rotation between newDirection and current dir ?
    Vector3f up = this->up();
    
    Matrix3f camAxes;

    camAxes.col(2) = (-newDirection).normalized();
    camAxes.col(0) = up.cross( camAxes.col(2) ).normalized();
    camAxes.col(1) = camAxes.col(2).cross( camAxes.col(0) ).normalized();
    setOrientation(Quaternionf(camAxes));
    
    mViewIsUptodate = false;
}

void Camera::setTarget(const Vector3f& target)
{
    mTarget = target;
    if (!mTarget.isApprox(position()))
    {
        Vector3f newDirection = mTarget - position();
        setDirection(newDirection.normalized());
    }
}

void Camera::setPosition(const Vector3f& p)
{
    mFrame.position = p;
    mViewIsUptodate = false;
}

void Camera::setOrientation(const Quaternionf& q)
{
    mFrame.orientation = q;
    mViewIsUptodate = false;
}

void Camera::setFrame(const Frame& f)
{
  mFrame = f;
  mViewIsUptodate = false;
}

void Camera::rotateAroundTarget(const Quaternionf& q)
{
    Matrix4f mrot, mt, mtm;
    
    // update the transform matrix
    updateViewMatrix();
    Vector3f t = mViewMatrix * mTarget;

    mViewMatrix = Translation3f(t)
                * q
                * Translation3f(-t)
                * mViewMatrix;
    
    Quaternionf qa(mViewMatrix.linear());
    qa = qa.conjugate();
    setOrientation(qa);
    setPosition(- (qa * mViewMatrix.translation()) );

    mViewIsUptodate = true;
}

void Camera::localRotate(const Quaternionf& q)
{
    float dist = (position() - mTarget).norm();
    setOrientation(orientation() * q);
    mTarget = position() + dist * direction();
    mViewIsUptodate = false;
}

void Camera::zoom(float d)
{
    float dist = (position() - mTarget).norm();
    if(dist > d)
    {
        setPosition(position() + direction() * d);
        mViewIsUptodate = false;
    }
}

void Camera::localTranslate(const Vector3f& t)
{
  Vector3f trans = orientation() * t;
  setPosition( position() + trans );
  setTarget( mTarget + trans );

  mViewIsUptodate = false;
}

void Camera::updateViewMatrix(void) const
{
    if(!mViewIsUptodate)
    {
        Quaternionf q = orientation().conjugate();
        mViewMatrix.linear() = q.toRotationMatrix();
        mViewMatrix.translation() = - (mViewMatrix.linear() * position());

        mViewIsUptodate = true;
    }
}

const Affine3f& Camera::viewMatrix(void) const
{
  updateViewMatrix();
  return mViewMatrix;
}

void Camera::updateProjectionMatrix(void) const
{
  if(!mProjIsUptodate)
  {
    mProjectionMatrix.setIdentity();
    float aspect = float(mVpWidth)/float(mVpHeight);
    float theta = mFovY*0.5;
    float range = mFarDist - mNearDist;
    float invtan = 1./tan(theta);

    mProjectionMatrix(0,0) = invtan / aspect;
    mProjectionMatrix(1,1) = invtan;
    mProjectionMatrix(2,2) = -(mNearDist + mFarDist) / range;
    mProjectionMatrix(3,2) = -1;
    mProjectionMatrix(2,3) = -2 * mNearDist * mFarDist / range;
    mProjectionMatrix(3,3) = 0;
    
    mProjIsUptodate = true;
  }
}

const Matrix4f& Camera::projectionMatrix(void) const
{
  updateProjectionMatrix();
  return mProjectionMatrix;
}

void Camera::activateGL(void)
{
  glViewport(vpX(), vpY(), vpWidth(), vpHeight());
  gpu.loadMatrix(projectionMatrix(),GL_PROJECTION);
  gpu.loadMatrix(viewMatrix().matrix(),GL_MODELVIEW);
}


Vector3f Camera::unProject(const Vector2f& uv, float depth) const
{
    Matrix4f inv = mViewMatrix.inverse().matrix();
    return unProject(uv, depth, inv);
}

Vector3f Camera::unProject(const Vector2f& uv, float depth, const Matrix4f& invModelview) const
{
    updateViewMatrix();
    updateProjectionMatrix();
    
    Vector3f a(2.*uv.x()/float(mVpWidth)-1., 2.*uv.y()/float(mVpHeight)-1., 1.);
    a.x() *= depth/mProjectionMatrix(0,0);
    a.y() *= depth/mProjectionMatrix(1,1);
    a.z() = -depth;
    // FIXME /\/|
    Vector4f b = invModelview * Vector4f(a.x(), a.y(), a.z(), 1.);
    return Vector3f(b.x(), b.y(), b.z());
}