Update ViewState and CullingVolume to take a general projection matrix

Cesium3DTilesSelection/include/Cesium3DTilesSelection/ViewState.h

@@ -8,9 +8,11 @@
 #include <CesiumGeospatial/Ellipsoid.h>
 
 #include <glm/mat3x3.hpp>
+#include <glm/mat4x4.hpp>
 #include <glm/vec2.hpp>
 #include <glm/vec3.hpp>
 
+#include <optional>
 #include <vector>
 
 namespace Cesium3DTilesSelection {
@@ -25,7 +27,8 @@ class CESIUM3DTILESSELECTION_API ViewState final {
   // TODO: Add support for orthographic and off-center perspective frustums
 public:
   /**
-   * @brief Creates a new instance of a view state.
+   * @brief Creates a new instance of a view state with a symmetric perspective
+   * projection.
    *
    * @param position The position of the eye point of the camera.
    * @param direction The view direction vector of the camera.
@@ -49,6 +52,24 @@ public:
       double verticalFieldOfView,
       const CesiumGeospatial::Ellipsoid& ellipsoid CESIUM_DEFAULT_ELLIPSOID);
 
+  /**
+   * @brief Creates a new instance of a view state with a general projection,
+   * including skewed perspective and orthographic projections.
+   *
+   * @param viewMatrix The view's view matrix i.e., the inverse of its pose
+   * @param projectionMatrix The view's Vulkan style reversed Z projection matrix
+   * @param viewportSize The size of the viewport, in pixels.
+   * @param ellipsoid The ellipsoid that will be used to compute the
+   * {@link ViewState#getPositionCartographic cartographic position}
+   * from the cartesian position.
+   * Default value: {@link CesiumGeospatial::Ellipsoid::WGS84}.
+   */
+  static ViewState create(
+      const glm::dmat4& viewMatrix,
+      const glm::dmat4& projectionMatrix,
+      const glm::dvec2& viewportSize,
+      const CesiumGeospatial::Ellipsoid& ellipsoid CESIUM_DEFAULT_ELLIPSOID);
+
   /**
    * @brief Gets the position of the camera in Earth-centered, Earth-fixed
    * coordinates.
@@ -164,6 +185,13 @@ private:
       const std::optional<CesiumGeospatial::Cartographic>& positionCartographic,
       const CesiumGeospatial::Ellipsoid& ellipsoid);
 
+  ViewState(
+      const glm::dmat4& viewMatrix,
+      const glm::dmat4& _projectionMatrix,
+      const glm::dvec2& viewportSize,
+      const std::optional<CesiumGeospatial::Cartographic>& positionCartographic,
+      const CesiumGeospatial::Ellipsoid& ellipsoid);
+
   const glm::dvec3 _position;
   const glm::dvec3 _direction;
   const glm::dvec3 _up;
@@ -176,6 +204,7 @@ private:
   const std::optional<CesiumGeospatial::Cartographic> _positionCartographic;
 
   const CesiumGeometry::CullingVolume _cullingVolume;
+  std::optional<const glm::dmat4> _projectionMatrix;
 };
 
 } // namespace Cesium3DTilesSelection

Cesium3DTilesSelection/src/ViewState.cpp

@@ -43,6 +43,35 @@ namespace Cesium3DTilesSelection {
       ellipsoid);
 }
 
+namespace {
+glm::dvec3 positionFromView(const glm::dmat4& viewMatrix) {
+  // Back out the world position by multiplying the view matrix translation by
+  // the rotation transpose (inverse) and negating.
+  glm::dvec3 position(0.0);
+  position.x = -glm::dot(glm::dvec3(viewMatrix[0]), glm::dvec3(viewMatrix[3]));
+  position.y = -glm::dot(glm::dvec3(viewMatrix[1]), glm::dvec3(viewMatrix[3]));
+  position.z = -glm::dot(glm::dvec3(viewMatrix[2]), glm::dvec3(viewMatrix[3]));
+  return position;
+}
+
+glm::dvec3 directionFromView(const glm::dmat4& viewMatrix) {
+  return glm::dvec3(viewMatrix[0][2], viewMatrix[1][2], viewMatrix[2][2]) * -1.0;
+}
+}
+
+/* static */ ViewState ViewState::create(
+      const glm::dmat4& viewMatrix,
+      const glm::dmat4& projectionMatrix,
+      const glm::dvec2& viewportSize,
+      const CesiumGeospatial::Ellipsoid& ellipsoid) {
+  return ViewState(
+      viewMatrix,
+      projectionMatrix,
+      viewportSize,
+      ellipsoid.cartesianToCartographic(positionFromView(viewMatrix)),
+      ellipsoid);
+}
+
 ViewState::ViewState(
     const glm::dvec3& position,
     const glm::dvec3& direction,
@@ -68,6 +97,24 @@ ViewState::ViewState(
           horizontalFieldOfView,
           verticalFieldOfView)) {}
 
+ViewState::ViewState(
+    const glm::dmat4& viewMatrix,
+    const glm::dmat4& projectionMatrix,
+    const glm::dvec2& viewportSize,
+    const std::optional<CesiumGeospatial::Cartographic>& positionCartographic,
+    const CesiumGeospatial::Ellipsoid& ellipsoid)
+    : _position(positionFromView(viewMatrix)),
+      _direction(directionFromView(viewMatrix)),
+      _up(0.0),
+      _viewportSize(viewportSize),
+      _horizontalFieldOfView(0.0),
+      _verticalFieldOfView(0.0),
+      _ellipsoid(ellipsoid),
+      _sseDenominator(0.0),
+      _positionCartographic(positionCartographic),
+      _cullingVolume(createCullingVolume(projectionMatrix * viewMatrix)),
+      _projectionMatrix(projectionMatrix) {}
+
 namespace {
 template <class T>
 bool isBoundingVolumeVisible(
@@ -205,7 +252,26 @@ double ViewState::computeScreenSpaceError(
     double distance) const noexcept {
   // Avoid divide by zero when viewer is inside the tile
   distance = glm::max(distance, 1e-7);
-  const double sseDenominator = this->_sseDenominator;
-  return (geometricError * this->_viewportSize.y) / (distance * sseDenominator);
+  if (this->_projectionMatrix) {
+    // This is a simplified version of the projection transform and homogeneous
+    // division. We transform the coordinate (0.0, geometric error, -distance, 1)
+    // and use the resulting NDC to find the screen space error.  That's not
+    // quite right: the distance is actually the slant distance, and the real
+    // transform contains a term for an offset due to a skewed projection which
+    // is ignored here.
+    const glm::dmat4& projMat = *this->_projectionMatrix;
+    glm::dvec4 centerNdc = projMat * glm::dvec4(0.0, 0.0, -distance, 1.0);
+    centerNdc /= centerNdc.w;
+    glm::dvec4 errorOffsetNdc = projMat * glm::dvec4(0.0, geometricError, -distance, 1.0);
+    errorOffsetNdc /= errorOffsetNdc.w;
+
+    double ndcError = (errorOffsetNdc - centerNdc).y;
+    // ndc bounds are [-1.0, 1.0]. Our projection matrix has the top of the
+    // screen at -1.0.
+    return -ndcError * this->_viewportSize.y / 2.0;
+  } else {
+    const double sseDenominator = this->_sseDenominator;
+    return (geometricError * this->_viewportSize.y) / (distance * sseDenominator);
+  }
 }
 } // namespace Cesium3DTilesSelection

CesiumGeometry/include/CesiumGeometry/CullingVolume.h

@@ -2,6 +2,8 @@
 
 #include <CesiumGeometry/Plane.h>
 
+#include <glm/ext/matrix_double4x4.hpp>
+
 namespace CesiumGeometry {
 
 /**
@@ -58,4 +60,61 @@ CullingVolume createCullingVolume(
     const glm::dvec3& up,
     double fovx,
     double fovy) noexcept;
+
+/**
+ * @brief Create a {@link CullingVolume} from a projection matrix
+ *
+ * The matrix can be a composite view - projection matrix; the volume will then
+ * cull world coordinates. It can also be a model - view - projection matrix,
+ * which will cull model coordinates.
+ */
+CullingVolume createCullingVolume(const glm::dmat4& clipMatrix);
+
+/**
+ * @brief Creates a {@link CullingVolume} for a perspective frustum.
+ *
+ * @param position The eye position
+ * @param direction The viewing direction
+ * @param up The up-vector of the frustum
+ * @param l left edge
+ * @param r right edge
+ * @param t top edge
+ * @param b bottom edge
+ * @param n near plane distance
+ * @return The {@link CullingVolume}
+ */
+CullingVolume createCullingVolume(
+    const glm::dvec3& position,
+    const glm::dvec3& direction,
+    const glm::dvec3& up,
+    double l,
+    double r,
+    double b,
+    double t,
+    double n) noexcept;
+
+/**
+ * @brief Creates a {@link CullingVolume} for an orthographic frustum.
+ *
+ * @param position The eye position
+ * @param direction The viewing direction
+ * @param up The up-vector of the frustum
+ * @param l left edge
+ * @param r right edge
+ * @param t top edge
+ * @param b bottom edge
+ * @param n near plane distance
+ * @return The {@link CullingVolume}
+ */
+
+CullingVolume createOrthoCullingVolume(
+    const glm::dvec3& position,
+    const glm::dvec3& direction,
+    const glm::dvec3& up,
+    double l,
+    double r,
+    double b,
+    double t,
+    double n) noexcept;
+
 } // namespace CesiumGeometry

CesiumGeometry/src/CullingVolume.cpp

@@ -1,6 +1,8 @@
 #include <CesiumGeometry/CullingVolume.h>
 #include <CesiumGeometry/Plane.h>
+#include <CesiumGeometry/Transforms.h>
 
+#include <glm/ext/matrix_transform.hpp>
 #include <glm/ext/vector_double3.hpp>
 #include <glm/geometric.hpp>
 #include <glm/trigonometric.hpp>
@@ -73,6 +75,80 @@ CullingVolume createCullingVolume(
 
   const CesiumGeometry::Plane topPlane(normal, -glm::dot(normal, position));
 
+  return {leftPlane, rightPlane, topPlane , bottomPlane};
+}
+
+CullingVolume createCullingVolume(const glm::dmat4& clipMatrix) {
+  // Gribb / Hartmann method.
+  // See https://www.gamedevs.org/uploads/fast-extraction-viewing-frustum-planes-from-world-view-projection-matrix.pdf
+  auto normalizePlane = [](double a, double b, double c, double d)
+  {
+    double len = sqrt(a * a + b * b + c * c);
+    return Plane(glm::dvec3(a / len, b / len, c / len), d / len);
+  };
+  const Plane leftPlane = normalizePlane(
+      clipMatrix[0][3] + clipMatrix[0][0],
+      clipMatrix[1][3] + clipMatrix[1][0],
+      clipMatrix[2][3] + clipMatrix[2][0],
+      clipMatrix[3][3] + clipMatrix[3][0]);
+  const Plane rightPlane = normalizePlane(
+      clipMatrix[0][3] - clipMatrix[0][0],
+      clipMatrix[1][3] - clipMatrix[1][0],
+      clipMatrix[2][3] - clipMatrix[2][0],
+      clipMatrix[3][3] - clipMatrix[3][0]);
+  const Plane bottomPlane = normalizePlane(
+      clipMatrix[0][3] - clipMatrix[0][1],
+      clipMatrix[1][3] - clipMatrix[1][1],
+      clipMatrix[2][3] - clipMatrix[2][1],
+      clipMatrix[3][3] - clipMatrix[3][1]);
+  const Plane topPlane = normalizePlane(
+      clipMatrix[0][3] + clipMatrix[0][1],
+      clipMatrix[1][3] + clipMatrix[1][1],
+      clipMatrix[2][3] + clipMatrix[2][1],
+      clipMatrix[3][3] + clipMatrix[3][1]);
   return {leftPlane, rightPlane, topPlane, bottomPlane};
 }
+
+CullingVolume createCullingVolume(
+    const glm::dvec3& position,
+    const glm::dvec3& direction,
+    const glm::dvec3& up,
+    double l,
+    double r,
+    double b,
+    double t,
+    double n) noexcept
+{
+  glm::dmat4 projMatrix = Transforms::createPerspectiveMatrix(
+      l,
+      r,
+      b,
+      t,
+      n,
+      std::numeric_limits<double>::infinity());
+  glm::dmat4 viewMatrix = glm::lookAt(position, position + direction, up);
+  glm::dmat4 clipMatrix = projMatrix * viewMatrix;
+  return createCullingVolume(clipMatrix);
+}
+
+CullingVolume createOrthoCullingVolume(
+    const glm::dvec3& position,
+    const glm::dvec3& direction,
+    const glm::dvec3& up,
+    double l,
+    double r,
+    double b,
+    double t,
+    double n) noexcept {
+  glm::dmat4 projMatrix = Transforms::createOrthographicMatrix(
+      l,
+      r,
+      b,
+      t,
+      n,
+      std::numeric_limits<double>::infinity());
+  glm::dmat4 viewMatrix = glm::lookAt(position, position + direction, up);
+  glm::dmat4 clipMatrix = projMatrix * viewMatrix;
+  return createCullingVolume(clipMatrix);
+}
 } // namespace CesiumGeometry

CesiumGeometry/test/TestCullingVolume.cpp

@@ -1,4 +1,5 @@
 #include <CesiumGeometry/CullingVolume.h>
+#include <CesiumGeometry/Transforms.h>
 #include <CesiumUtility/Math.h>
 
 #include <doctest/doctest.h>
@@ -13,6 +14,32 @@ using namespace doctest;
 using namespace CesiumGeometry;
 using namespace CesiumUtility;
 
+namespace {
+constexpr bool equalsEpsilon(
+    const Plane& left,
+    const Plane& right,
+    double relativeEpsilon) {
+  return Math::equalsEpsilon(
+      left.getNormal(),
+      right.getNormal(),
+      relativeEpsilon)
+      && Math::equalsEpsilon(
+          left.getDistance(),
+          right.getDistance(),
+          relativeEpsilon);
+}
+}
+
+constexpr bool equalsEpsilon(
+    const CullingVolume& left,
+    const CullingVolume& right,
+    double relativeEpsilon) {
+  return equalsEpsilon(left.leftPlane, right.leftPlane, relativeEpsilon)
+      && equalsEpsilon(left.rightPlane, right.rightPlane, relativeEpsilon)
+      && equalsEpsilon(left.topPlane, right.topPlane, relativeEpsilon)
+      && equalsEpsilon(left.bottomPlane, right.bottomPlane, relativeEpsilon);
+}
+
 TEST_CASE("CullingVolume::createCullingVolume") {
   SUBCASE("Shouldn't crash when too far from the globe") {
     CHECK_NOTHROW(createCullingVolume(
@@ -31,4 +58,28 @@ TEST_CASE("CullingVolume::createCullingVolume") {
         Math::PiOverTwo,
         Math::PiOverTwo));
   }
-}
+}
+
+TEST_CASE("CullingVolume construction") {
+  SUBCASE("Field of view and clip matrix") {
+    glm::dvec3 position(1e5, 1e5, 1e5);
+    glm::dvec3 direction(0, 0, 1);
+    glm::dvec3 up(0, 1, 0);
+    CullingVolume traditional = createCullingVolume(
+        position,
+        direction,
+        up,
+        Math::PiOverTwo,
+        Math::PiOverTwo);
+    CullingVolume rad = createCullingVolume(Transforms::createPerspectiveMatrix(
+                                                Math::PiOverTwo,
+                                                Math::PiOverTwo,
+                                                10,
+                                                200000)
+                                            * Transforms::createViewMatrix(
+                                                position,
+                                                direction,
+                                                up));
+    CHECK(equalsEpsilon(traditional, rad, 1e-10));
+  }
+}