libigl_Martin/include/igl/Camera.h

#ifndef IGL_CAMERA_H
#define IGL_CAMERA_H
#include <Eigen/Geometry>
#include <Eigen/Core>

namespace igl
{

  // A simple camera class. The camera stores projection parameters (field of
  // view angle, aspect ratio, near and far clips) as well as a rigid
  // tranformation *of the camera as if it were also a scene object*. Thus, the
  // **inverse** of this rigid transformation is the modelview transformation.
  class Camera
  {
    public:
      // On windows you might need: -fno-delayed-template-parsing
      static constexpr double MIN_ANGLE = 5.;
      //  m_angle  Field of view angle in degrees {45}
      //  m_aspect  Aspect ratio {1}
      //  m_near  near clipping plane {1e-2}
      //  m_far  far clipping plane {100}
      //  m_at_dist  distance of looking at point {1}
      //  m_rotation_conj  Conjugate of rotation part of rigid transformation of
      //    camera {identity}. Note: we purposefully store the conjugate because
      //    this is what TW_TYPE_QUAT4D is expecting.
      //  m_translation  Translation part of rigid transformation of camera
      //    {(0,0,1)}
      double m_angle, m_aspect, m_near, m_far, m_at_dist;
      Eigen::Quaterniond m_rotation_conj;
      Eigen::Vector3d m_translation;
    private:
      // m_at_dist_min_angle  m_at_dist from last time m_angle set to <= MIN_ANGLE
      double m_at_dist_min_angle;
      double m_angle_min_angle;
    //  // m_last_positive_m_angle
    //  // m_last_positive_m_angle_m_at_dist
    //  double m_last_positive_m_angle,m_last_positive_m_angle_m_at_dist;
    public:
      inline Camera();
      inline virtual ~Camera(){}
      // Return projection matrix that takes relative camera coordinates and
      // transforms it to viewport coordinates
      //
      // Note:
      //
      //     if(m_angle > 0)
      //     {
      //       gluPerspective(m_angle,m_aspect,m_near,m_at_dist+m_far);
      //     }else
      //     {
      //       gluOrtho(-0.5*aspect,0.5*aspect,-0.5,0.5,m_at_dist+m_near,m_far);
      //     }
      //
      // Is equivalent to
      //
      //     glMultMatrixd(projection().data());
      //
      inline Eigen::Matrix4d projection() const;
      // Return an Affine transformation (rigid actually) that takes a world 3d coordinate and
      // transforms it into the relative camera coordinates.
      inline Eigen::Affine3d affine() const;
      // Return an Affine transformation (rigid actually) that takes relative
      // coordinates and tramsforms them into world 3d coordinates.
      //
      // Note:
      //
      //     gluLookAt(
      //       eye()(0), eye()(1), eye()(2),
      //       at()(0), at()(1), at()(2),
      //       up()(0), up()(1), up()(2));
      //
      // Is equivalent to
      //
      //     glMultMatrixd(camera.affine().matrix().data());
      //
      // See also: affine, eye, at, up
      inline Eigen::Affine3d inverse() const;
      // Returns world coordinates position of center or "eye" of camera.
      inline Eigen::Vector3d eye() const;
      // Returns world coordinate position of a point "eye" is looking at.
      inline Eigen::Vector3d at() const;
      // Returns world coordinate unit vector of "up" vector
      inline Eigen::Vector3d up() const;
      // Return top right corner of unit plane in relative coordinates, that is
      // (w/2,h/2,1)
      inline Eigen::Vector3d unit_plane() const;
      // Move dv in the relative coordinate frame of the camera (move the FPS)
      //
      // Inputs:
      //   dv  (x,y,z) displacement vector
      //
      inline void dolly(const Eigen::Vector3d & dv);
      // "Scale zoom": Move `eye`, but leave `at`
      //
      // Input:
      //   s  amount to scale distance to at
      inline void push_away(const double s);
      // Aka "Hitchcock", "Vertigo", "Spielberg" or "Trombone" zoom:
      // simultaneously dolly while changing angle so that `at` not only stays
      // put in relative coordinates but also projected coordinates. That is
      //
      // Inputs:
      //   da  change in angle in degrees
      inline void dolly_zoom(const double da);
      // Turn around eye so that rotation is now q
      //
      // Inputs:
      //   q  new rotation as quaternion
      inline void turn_eye(const Eigen::Quaterniond & q);
      // Orbit around at so that rotation is now q
      //
      // Inputs:
      //   q  new rotation as quaternion
      inline void orbit(const Eigen::Quaterniond & q);
      // Rotate and translate so that camera is situated at "eye" looking at "at"
      // with "up" pointing up.
      //
      // Inputs:
      //   eye  (x,y,z) coordinates of eye position
      //   at   (x,y,z) coordinates of at position
      //   up   (x,y,z) coordinates of up vector
      inline void look_at(
        const Eigen::Vector3d & eye,
        const Eigen::Vector3d & at,
        const Eigen::Vector3d & up);
  };
}

// Implementation
#include "PI.h"
#include "EPS.h"
#include <cmath>
#include <iostream>
#include <cassert>

inline igl::Camera::Camera():
  m_angle(45.0),m_aspect(1),m_near(1e-2),m_far(100),m_at_dist(1),
  m_rotation_conj(1,0,0,0),
  m_translation(0,0,1),
  m_at_dist_min_angle(m_at_dist),
  m_angle_min_angle(m_angle)
{
}

inline Eigen::Matrix4d igl::Camera::projection() const
{
  Eigen::Matrix4d P;
  using namespace std;
  using namespace igl;
  // http://stackoverflow.com/a/3738696/148668
  if(m_angle >= MIN_ANGLE)
  {
    const double yScale = tan(PI*0.5 - 0.5*m_angle*PI/180.);
    // http://stackoverflow.com/a/14975139/148668
    const double xScale = yScale/m_aspect;
    const double far = m_at_dist + m_far;
    const double near = m_near;
    P<< 
      xScale, 0, 0, 0,
      0, yScale, 0, 0,
      0, 0, -(far+near)/(far-near), -1,
      0, 0, -2.*near*far/(far-near), 0;
    P = P.transpose().eval();
  }else
  {
    const double f = 0.5;
    const double left = -f*m_aspect;
    const double right = f*m_aspect;
    const double bottom = -f;
    const double top = f;
    const double near = m_near;
    const double far = m_at_dist + m_far;
    const double tx = (right+left)/(right-left);
    const double ty = (top+bottom)/(top-bottom);
    const double tz = (far+near)/(far-near);
    const double z_fix = 
      0.5/(m_at_dist_min_angle * tan(m_angle_min_angle/2./180.*M_PI))+
      (-m_at_dist+m_at_dist_min_angle)/m_at_dist_min_angle;
    P<<
      z_fix*2./(right-left), 0, 0, -tx,
      0, z_fix*2./(top-bottom), 0, -ty,
      0, 0, -z_fix*2./(far-near),  -tz,
      0, 0, 0, 1;
  }
  return P;
}

inline Eigen::Affine3d igl::Camera::affine() const
{
  using namespace Eigen;
  Affine3d t = Affine3d::Identity();
  t.rotate(m_rotation_conj.conjugate());
  t.translate(m_translation);
  return t;
}

inline Eigen::Affine3d igl::Camera::inverse() const
{
  using namespace Eigen;
  Affine3d t = Affine3d::Identity();
  t.translate(-m_translation);
  t.rotate(m_rotation_conj);
  return t;
}

inline Eigen::Vector3d igl::Camera::eye() const
{
  using namespace Eigen;
  return affine() * Vector3d(0,0,0);
}

inline Eigen::Vector3d igl::Camera::at() const
{
  using namespace Eigen;
  return affine() * (Vector3d(0,0,-1)*m_at_dist);
}

inline Eigen::Vector3d igl::Camera::up() const
{
  using namespace Eigen;
  Affine3d t = Affine3d::Identity();
  t.rotate(m_rotation_conj.conjugate());
  return t * Vector3d(0,1,0);
}

inline Eigen::Vector3d igl::Camera::unit_plane() const
{
  using namespace igl;
  // Distance of center pixel to eye
  const double d = 1.0;
  const double a = m_aspect;
  const double theta = m_angle*PI/180.;
  const double w =
    2.*sqrt(-d*d/(a*a*pow(tan(0.5*theta),2.)-1.))*a*tan(0.5*theta);
  const double h = w/a;
  return Eigen::Vector3d(w*0.5,h*0.5,-d);
}

inline void igl::Camera::dolly(const Eigen::Vector3d & dv)
{
  m_translation += dv;
}

inline void igl::Camera::push_away(const double s)
{
  using namespace Eigen;
  using namespace igl;
#ifndef NDEBUG
  Vector3d old_at = at();
#endif
  const double old_at_dist = m_at_dist;
  m_at_dist = old_at_dist * s;
  dolly(Vector3d(0,0,1)*(m_at_dist - old_at_dist));
  assert((old_at-at()).squaredNorm() < DOUBLE_EPS);
}

inline void igl::Camera::dolly_zoom(const double da)
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
#ifndef NDEBUG
  Vector3d old_at = at();
#endif
  const double old_angle = m_angle;
  m_angle += da;
  m_angle = min(89.,max(0.,m_angle));
  const double eff_angle = (MIN_ANGLE > m_angle ? MIN_ANGLE : m_angle);
  if(old_angle >= MIN_ANGLE)
  {
    // change in distance
    const double s = 
      (2.*tan(old_angle/2./180.*M_PI)) /
      (2.*tan(eff_angle/2./180.*M_PI)) ;
    const double old_at_dist = m_at_dist;
    m_at_dist = old_at_dist * s;
    dolly(Vector3d(0,0,1)*(m_at_dist - old_at_dist));
    if(eff_angle == MIN_ANGLE)
    {
      m_at_dist_min_angle = m_at_dist;
      m_angle_min_angle = eff_angle;
    }
    assert((old_at-at()).squaredNorm() < DOUBLE_EPS);
  }else if(old_angle < MIN_ANGLE && m_angle >= MIN_ANGLE)
  {
    // Restore decent length
    const double z_fix = 
      // There should be some factor here based on the incoming angle
      // (m_angle_min_angle) and outgoing angle (m_angle)... For now I set it
      // to 1. (assumes equality)
      //0.5/(m_at_dist_min_angle * tan(m_angle_min_angle/2./180.*M_PI))+
      1.+(-m_at_dist+m_at_dist_min_angle)/m_at_dist_min_angle;
    m_at_dist = m_at_dist_min_angle / z_fix;
  }
}

inline void igl::Camera::turn_eye(const Eigen::Quaterniond & q)
{
  using namespace Eigen;
  using namespace igl;
  Vector3d old_eye = eye();
  // eye should be fixed
  //
  // eye_1 = R_1 * t_1 = eye_0
  // t_1 = R_1' * eye_0
  m_rotation_conj = q.conjugate();
  m_translation = m_rotation_conj * old_eye;
  assert((old_eye - eye()).squaredNorm() < DOUBLE_EPS);
}

inline void igl::Camera::orbit(const Eigen::Quaterniond & q)
{
  using namespace Eigen;
  using namespace igl;
  Vector3d old_at = at();
  // at should be fixed
  //
  // at_1 = R_1 * t_1 - R_1 * z = at_0
  // t_1 = R_1' * (at_0 + R_1 * z)
  m_rotation_conj = q.conjugate();
  m_translation = 
    m_rotation_conj * 
      (old_at + 
         m_rotation_conj.conjugate() * Vector3d(0,0,1) * m_at_dist);
  assert((old_at - at()).squaredNorm() < DOUBLE_EPS);
}

inline void igl::Camera::look_at(
  const Eigen::Vector3d & eye,
  const Eigen::Vector3d & at,
  const Eigen::Vector3d & up)
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  // http://www.opengl.org/sdk/docs/man2/xhtml/gluLookAt.xml
  // Normalize vector from at to eye
  Vector3d F = eye-at;
  m_at_dist = F.norm();
  F.normalize();
  // Project up onto plane orthogonal to F and normalize
  const Vector3d proj_up = (up-(up.dot(F))*F).normalized();
  Quaterniond a,b;
  a.setFromTwoVectors(Vector3d(0,0,-1),-F);
  b.setFromTwoVectors(a*Vector3d(0,1,0),proj_up);
  m_rotation_conj = (a*b).conjugate();
  m_translation = m_rotation_conj * eye;
  assert(           (eye-this->eye()).squaredNorm() < DOUBLE_EPS);
  assert((F-(this->eye()-this->at()).normalized()).squaredNorm() < 
    DOUBLE_EPS);
  assert(           (at-this->at()).squaredNorm() < DOUBLE_EPS);
  assert(        (proj_up-this->up()).squaredNorm() < DOUBLE_EPS);
}

#endif