fieldofview.h

Go to the documentation of this file.

#ifndef ION_MATH_FIELDOFVIEW_H_
#define ION_MATH_FIELDOFVIEW_H_

#include <cmath>

#include "ion/math/angle.h"
#include "ion/math/matrixutils.h"
#include "ion/math/transformutils.h"
#include "ion/math/vector.h"

namespace ion {
namespace math {

template <typename T>
class FieldOfView {
 public:
  FieldOfView();

  FieldOfView(const Angle<T>& left, const Angle<T>& right,
              const Angle<T>& bottom, const Angle<T>& top);

  static FieldOfView<T> FromProjectionMatrix(const Matrix<4, T>& matrix);

  static FieldOfView<T> FromTangents(const T& left, const T& right,
                                     const T& bottom, const T& top);

  template <typename U>
  explicit FieldOfView(const FieldOfView<U>& fov);

  bool SetFromTotalFovAndOpticalCenter(const Angle<T> fov_x,
                                       const Angle<T> fov_y,
                                       const Point<2, T>& optical_center_ndc);

   -1   p_ndc    0            1
   *      *
  static FieldOfView<T> FromCenteredFovAndOpticalCenter(
      const Angle<T> centered_fov_x, const Angle<T> centered_fov_y,
      const Point<2, T>& optical_center_ndc);

  Point<2, T> GetOpticalCenter() const;

  Matrix<4, T> GetProjectionMatrix(T near_p, T far_p) const;

  Angle<T> GetLeft() const { return left_; }
  Angle<T> GetRight() const { return right_; }
  Angle<T> GetBottom() const { return bottom_; }
  Angle<T> GetTop() const { return top_; }

  void SetLeft(const Angle<T>& left) { left_ = left; }
  void SetRight(const Angle<T>& right) { right_ = right; }
  void SetBottom(const Angle<T>& bottom) { bottom_ = bottom; }
  void SetTop(const Angle<T>& top) { top_ = top; }

  Angle<T> GetCenteredFovX() const;
  Angle<T> GetCenteredFovY() const;

  void Print(std::ostream& out) const;  // NOLINT

  void Read(std::istream& in);  // NOLINT

  bool IsZero() const;

  friend bool operator==(const FieldOfView& fov0, const FieldOfView& fov1) {
    return AreEqual(fov0, fov1);
  }

  friend bool operator!=(const FieldOfView& fov0, const FieldOfView& fov1) {
    return !AreEqual(fov0, fov1);
  }

 private:
  static bool ComputeHalfAnglesForTotalFovAndOpticalCenter1d(
      const Angle<T> total_fov, const T optical_center_ndc,
      Angle<T>* angle1_out, Angle<T>* angle2_out);

  static void ComputeHalfAnglesForCenteredFovAndOpticalCenter1d(
      const Angle<T> centered_fov, const T optical_center_ndc,
      Angle<T>* angle1_out, Angle<T>* angle2_out);

  static bool AreEqual(const FieldOfView& fov0, const FieldOfView& fov1);

  Angle<T> left_;
  Angle<T> right_;
  Angle<T> bottom_;
  Angle<T> top_;
};

typedef FieldOfView<float> FieldOfViewf;
typedef FieldOfView<double> FieldOfViewd;

template <typename T>
inline FieldOfView<T>::FieldOfView() {}

template <typename T>
inline FieldOfView<T>::FieldOfView(const Angle<T>& left, const Angle<T>& right,
                                   const Angle<T>& bottom, const Angle<T>& top)
    : left_(left), right_(right), bottom_(bottom), top_(top) {}

template <typename T>
inline Matrix<4, T> FieldOfView<T>::GetProjectionMatrix(T near_p,
                                                        T far_p) const {
  const T l = -std::tan(left_.Radians()) * near_p;
  const T r = std::tan(right_.Radians()) * near_p;
  const T b = -std::tan(bottom_.Radians()) * near_p;
  const T t = std::tan(top_.Radians()) * near_p;
  return ion::math::PerspectiveMatrixFromFrustum(l, r, b, t, near_p, far_p);
}

template <typename T>
inline FieldOfView<T> FieldOfView<T>::FromTangents(const T& left,
                                                   const T& right,
                                                   const T& bottom,
                                                   const T& top) {
  return FieldOfView<T>(Angle<T>::FromRadians(std::atan(left)),
                        Angle<T>::FromRadians(std::atan(right)),
                        Angle<T>::FromRadians(std::atan(bottom)),
                        Angle<T>::FromRadians(std::atan(top)));
}

template <typename T>
inline FieldOfView<T> FieldOfView<T>::FromProjectionMatrix(
    const Matrix<4, T>& m) {
  const T kOne = static_cast<T>(1.0);

  const T kTanVertFov = kOne / m(1, 1);
  const T kTanHorzFov = kOne / m(0, 0);
  const T t = (m(1, 2) + kOne) * kTanVertFov;
  const T b = (m(1, 2) - kOne) * kTanVertFov;
  const T l = (m(0, 2) - kOne) * kTanHorzFov;
  const T r = (m(0, 2) + kOne) * kTanHorzFov;

  return FromTangents(-l, r, -b, t);
}

template <typename T>
template <typename U>
FieldOfView<T>::FieldOfView(const FieldOfView<U>& fov)
    : left_(fov.GetLeft()),
      right_(fov.GetRight()),
      bottom_(fov.GetBottom()),
      top_(fov.GetTop()) {}

template <typename T>
inline Point<2, T> FieldOfView<T>::GetOpticalCenter() const {
  const T kOne = static_cast<T>(1.0);
  const T kTwo = static_cast<T>(2.0);
  const T tan_left = std::tan(left_.Radians());
  const T tan_right = std::tan(right_.Radians());
  const T tan_bottom = std::tan(bottom_.Radians());
  const T tan_top = std::tan(top_.Radians());
  const T x_ndc = kTwo * tan_left / (tan_left + tan_right) - kOne;
  const T y_ndc = kTwo * tan_bottom / (tan_bottom + tan_top) - kOne;
  return Point<2, T>(x_ndc, y_ndc);
}

template <typename T>
inline bool FieldOfView<T>::SetFromTotalFovAndOpticalCenter(
    const Angle<T> fov_x, const Angle<T> fov_y,
    const Point<2, T>& optical_center_ndc) {
  Angle<T> bottom, top, left, right;
  if (!ComputeHalfAnglesForTotalFovAndOpticalCenter1d(
          fov_x, optical_center_ndc[0], &left, &right))
    return false;
  if (!ComputeHalfAnglesForTotalFovAndOpticalCenter1d(
          fov_y, optical_center_ndc[1], &bottom, &top))
    return false;

  left_ = left;
  right_ = right;
  bottom_ = bottom;
  top_ = top;
  return true;
}

template <typename T>
inline bool FieldOfView<T>::ComputeHalfAnglesForTotalFovAndOpticalCenter1d(
    const Angle<T> total_fov, const T optical_center_ndc, Angle<T>* angle1_out,
    Angle<T>* angle2_out) {
  const T kOne = static_cast<T>(1.0);
  T p = optical_center_ndc;

  bool invert = false;
  if (p > 0.0) {
    invert = true;
    p = -p;
  }

   -1   p_ndc                  1
  const T inv_tan_total_fov = kOne / tan(total_fov.Radians());
  const T discriminant = kOne + Square(inv_tan_total_fov) - Square(p);
  if (discriminant < 0) return false;

  const double sign = p < -kOne ? -kOne : kOne;
  *angle1_out = Angle<T>::FromRadians(
      atan((sign * std::sqrt(discriminant) - inv_tan_total_fov) / (kOne - p)));

  *angle2_out = total_fov - *angle1_out;
  if (invert) std::swap(*angle1_out, *angle2_out);
  return true;
}

template <typename T>
inline FieldOfView<T> FieldOfView<T>::FromCenteredFovAndOpticalCenter(
    const Angle<T> fov_x, const Angle<T> fov_y,
    const Point<2, T>& optical_center_ndc) {
  Angle<T> bottom, top, left, right;
  ComputeHalfAnglesForCenteredFovAndOpticalCenter1d(
      fov_x, optical_center_ndc[0], &left, &right);
  ComputeHalfAnglesForCenteredFovAndOpticalCenter1d(
      fov_y, optical_center_ndc[1], &bottom, &top);

  return FieldOfView<T>(left, right, bottom, top);
}

template <typename T>
inline void FieldOfView<T>::ComputeHalfAnglesForCenteredFovAndOpticalCenter1d(
    const Angle<T> centered_fov, const T optical_center_ndc,
    Angle<T>* angle1_out, Angle<T>* angle2_out) {
  const T kOne = static_cast<T>(1.0);
  const T kTwo = static_cast<T>(2.0);
  const T p = optical_center_ndc;

   -1   p_ndc    0            1

  const T x = kOne / std::tan(centered_fov.Radians() / kTwo);

  *angle1_out = Angle<T>::FromRadians(std::atan2(p + kOne, x));
  *angle2_out = Angle<T>::FromRadians(std::atan2(kOne - p, x));
}

template <typename T>
inline Angle<T> FieldOfView<T>::GetCenteredFovX() const {

  const T tan_left = std::tan(left_.Radians());
  const T tan_right = std::tan(right_.Radians());
  return Angle<T>::FromRadians(2 * std::atan((tan_left + tan_right) / 2));
}

template <typename T>
inline Angle<T> FieldOfView<T>::GetCenteredFovY() const {

  const T tan_bottom = std::tan(bottom_.Radians());
  const T tan_top = std::tan(top_.Radians());
  return Angle<T>::FromRadians(2 * std::atan((tan_bottom + tan_top) / 2));
}

template <typename T>
inline void FieldOfView<T>::Print(std::ostream& out) const {  // NOLINT
  out << "FOV[" << left_ << ", " << right_ << ", " << bottom_ << ", " << top_
      << "]";
}

template <typename T>
inline void FieldOfView<T>::Read(std::istream& in) {  // NOLINT
  FieldOfView fov;
  if (base::GetExpectedString(in, "FOV[")) {
    in >> left_;
    if (!base::GetExpectedChar<','>(in)) {
      *this = fov;
      return;
    }
    in >> right_;
    if (!base::GetExpectedChar<','>(in)) {
      *this = fov;
      return;
    }
    in >> bottom_;
    if (!base::GetExpectedChar<','>(in)) {
      *this = fov;
      return;
    }
    in >> top_;
    if (!base::GetExpectedChar<']'>(in)) {
      *this = fov;
      return;
    }
  }
}

template <typename T>
inline std::ostream& operator<<(std::ostream& out,
                                const FieldOfView<T>& f) {  // NOLINT
  f.Print(out);
  return out;
}

template <typename T>
inline std::istream& operator>>(std::istream& in,
                                FieldOfView<T>& f) {  // NOLINT
  f.Read(in);
  return in;
}

template <typename T>
inline bool FieldOfView<T>::IsZero() const {
  const T kZero = static_cast<T>(0.0);
  if (left_.Radians() != kZero || right_.Radians() != kZero ||
      bottom_.Radians() != kZero || top_.Radians() != kZero)
    return false;
  return true;
}

template <typename T>
bool FieldOfView<T>::AreEqual(const FieldOfView& fov0,
                              const FieldOfView& fov1) {
  if (fov0.GetLeft() != fov1.GetLeft() || fov0.GetRight() != fov1.GetRight() ||
      fov0.GetBottom() != fov1.GetBottom() || fov0.GetTop() != fov1.GetTop())
    return false;
  return true;
}

}  // namespace math
}  // namespace ion

#endif  // ION_MATH_FIELDOFVIEW_H_