OGC3DTilesUtils.cpp

#include "OGC3DTilesUtils.h"
 
#include "OGC3DTilesTileset.h"
 
#include "Foundation/Logging/Logger.h"
#include "Foundation/Geometry/GeodeticUtils.h"
 
#include <glm/gtc/type_ptr.hpp>
#include <cmath>
 
namespace {
  Cogs::Logging::Log logger = Cogs::Logging::getLogger("OGC3DTilesUtils");
}
 
using namespace Cogs::Core;
 
bool
OGC3DTilesUtils::isInsideBox(const glm::dvec3& point, const Cogs::Geometry::DBoundingBox& box)
{
  return point.x > box.min.x && point.x < box.max.x &&
    point.y > box.min.y && point.y < box.max.y &&
    point.z > box.min.z && point.z < box.max.z;
};
 
double
OGC3DTilesUtils::distanceFromBBox(const glm::dvec3 point, const Cogs::Geometry::DBoundingBox& bbox)
{
  if (OGC3DTilesUtils::isInsideBox(point, bbox)) {
    return 0.0;
  }
 
  // Ref: https://stackoverflow.com/a/18157551
  const double dx = std::max({ bbox.min.x - point.x, 0.0, point.x - bbox.max.x });
  const double dy = std::max({ bbox.min.y - point.y, 0.0, point.y - bbox.max.y });
  const double dz = std::max({ bbox.min.z - point.z, 0.0, point.z - bbox.max.z });
  return glm::length(glm::dvec3(dx, dy, dz));
};
 
void
OGC3DTilesUtils::printBBox(const Cogs::Geometry::BoundingBox& bbox, const std::string& prefix)
{
  printf("%s: bbox=<%.2f, %.2f, %.2f> | <%.2f, %.2f, %.2f> (w=%.2f, h=%.2f, d=%.2f)\n",
    prefix.c_str(),
    bbox.min.x, bbox.min.y, bbox.min.z,
    bbox.max.x, bbox.max.y, bbox.max.z,
    float(bbox.max.x - bbox.min.x), float(bbox.max.y - bbox.min.y), float(bbox.max.z - bbox.min.z));
}
 
void
OGC3DTilesUtils::printBBox(const Cogs::Geometry::DBoundingBox& bbox, const std::string& prefix)
{
  printf("%s: bbox=<%.2f, %.2f, %.2f> | <%.2f, %.2f, %.2f> (w=%.2f, h=%.2f, d=%.2f)\n",
    prefix.c_str(),
    bbox.min.x, bbox.min.y, bbox.min.z,
    bbox.max.x, bbox.max.y, bbox.max.z,
    float(bbox.max.x - bbox.min.x), float(bbox.max.y - bbox.min.y), float(bbox.max.z - bbox.min.z));
}
 
void
OGC3DTilesUtils::transformBBox(Cogs::Geometry::DBoundingBox& bbox, const glm::mat4& mat)
{
  const glm::dvec3 points[2] = { bbox.min, bbox.max };
 
  static constexpr double mx = std::numeric_limits<double>::max();
  static constexpr double mn = -mx;
  bbox.min = glm::dvec3(mx);
  bbox.max = glm::dvec3(mn);
 
  for (int i = 0; i < 8; i++) {
    const glm::dvec4 corner = glm::dvec4(points[(i & 4) >> 2].x, points[(i & 2) >> 1].y, points[i & 1].z, 1.0);
    const glm::dvec4 xfmd = mat * corner;
    bbox += xfmd;
  }
};
 
void
OGC3DTilesUtils::translateBBox(Cogs::Geometry::DBoundingBox& bbox, const glm::dvec3& offset)
{
  bbox.min += offset;
  bbox.max += offset;
}
 
int
OGC3DTilesUtils::bboxInsideViewFrustum(const Cogs::Geometry::DBoundingBox& bbox, const glm::mat4& viewProjectionMatrix)
{
  // NOTE: Parts of this code is lifted from "ManageLodLevels.cpp::cellInsideFrustum()"
  const glm::dvec3 corners[8] = {
    bbox.min,
    glm::dvec3(bbox.min.x, bbox.min.y, bbox.max.z),
    glm::dvec3(bbox.min.x, bbox.max.y, bbox.min.z),
    glm::dvec3(bbox.min.x, bbox.max.y, bbox.max.z),
    glm::dvec3(bbox.max.x, bbox.min.y, bbox.min.z),
    glm::dvec3(bbox.max.x, bbox.min.y, bbox.max.z),
    glm::dvec3(bbox.max.x, bbox.max.y, bbox.min.z),
    bbox.max
  };
 
  uint32_t allInsideMask = 0b111111;
  {
    uint32_t anyInsideMask = 0u;
    for (size_t i = 0; i < 8; i++) {
      const glm::dvec4 p = viewProjectionMatrix * glm::dvec4(corners[i], 1.f);
      const uint32_t cornerMask =
        (p.x <= p.w ? 1 : 0) |
        (p.y <= p.w ? 2 : 0) |
        (p.z <= p.w ? 4 : 0) |
        (-p.w <= p.x ? 8 : 0) |
        (-p.w <= p.y ? 16 : 0) |
        (-p.w <= p.z ? 32 : 0);
      anyInsideMask = anyInsideMask | cornerMask;
      allInsideMask = allInsideMask & cornerMask;
    }
 
    if (anyInsideMask != 0b111111) {
      // At least one plane where all corners are outside => outside.
      return 0;
    }
  }
 
  // No need to do a separate response if completely or only partially inside. Implement later if needed.
  return 1;
}
 
// NOTE: This is a version of the LLtoECEF() in Foundation::GeodeticUtils.cpp. The difference is that
// this method takes radians as input instead of degrees. This function is called a bazillion times
// whenever a dataset uses LatLong-region based bbox'es and the 3DTiles spec uses radians for
// all LatLongs values.
glm::dvec3
latLngAltToVec3(double latitudeRad, double longitudeRad, double altitudeMeter)
{
  constexpr double semimajorAxis = 6378137.0;
  constexpr double semiminorAxis = 6356752.314245;
  const double sinLat = std::sin(latitudeRad);
  const double cosLat = std::cos(latitudeRad);
  const double sinLong = std::sin(longitudeRad);
  const double cosLong = std::cos(longitudeRad);
  constexpr double ellipsoidFlatness = ((semimajorAxis - semiminorAxis) / semimajorAxis);
  constexpr double firstEccentricitySquared = (ellipsoidFlatness * (2.0 - ellipsoidFlatness));
  const double n = semimajorAxis / std::sqrt(1.0 - firstEccentricitySquared * sinLat * sinLat);
 
  return glm::dvec3((altitudeMeter + n) * cosLat * cosLong,
    (altitudeMeter + n) * cosLat * sinLong,
    (altitudeMeter + (1.0 - firstEccentricitySquared) * n) * sinLat);
}
 
// See "https://github.com/CesiumGS/3d-tiles/tree/main/specification#core-bounding-volumes" for definition
Cogs::Geometry::DBoundingBox
OGC3DTilesUtils::boundingVolumeConverter(const Cogs::Core::OGC3DTiles::BoundingVolume& bv, const glm::mat4& transform)
{
  Cogs::Geometry::DBoundingBox bbox = Cogs::Geometry::makeEmptyBoundingBox<Cogs::Geometry::DBoundingBox>();
 
  if (bv.type == OGC3DTiles::BoundingVolumeType::BOX) { // Oriented bounding box
    // FIXME: Converting this type of bbox directly to a Cogs::Geometry::BoundingBox is
    // not 100% correct as the volume is not properly preserved. The only way around this is to make
    // our own BBox class which preserves the orientation.
    glm::vec3 center = glm::make_vec3(&bv.values[0]);
 
    glm::vec3 dx = glm::abs(glm::make_vec3(&bv.values[3]));
    glm::vec3 dy = glm::abs(glm::make_vec3(&bv.values[6]));
    glm::vec3 dz = glm::abs(glm::make_vec3(&bv.values[9]));
 
    bbox.min = center;
    bbox.max = center;
    bbox.min -= dx;
    bbox.min -= dy;
    bbox.min -= dz;
    bbox.max += dx;
    bbox.max += dy;
    bbox.max += dz;
  }
  else if (bv.type == OGC3DTiles::BoundingVolumeType::REGION) { // [west, south, east, north, minimum height, maximum height]
    // NOTE: west, south, east & north are defined in radians NOT degrees (as usual).
    // Min/max height is defined in meters. Latitude is South/North, Longitude is West/East.
    const double west = bv.values[0];
    const double south = bv.values[1];
    const double east = bv.values[2];
    const double north = bv.values[3];
    const double minHeight = bv.values[4];
    const double maxHeight = bv.values[5];
 
    // FIXME: I think this process can be improved. We only need to do multiple "samples" whenever the
    // east/west or north/south spans between +90 degrees (bbox gets too small) or 180-degrees (a 2-sample
    // version could then result in a flat box.).
 
    const double latRange = std::abs(east - west);
    const double lngRange = std::abs(north - south);
    int latSteps = 1;
    if (latRange > glm::pi<double>()) {
      latSteps = int(latRange / (glm::pi<double>() / 3.0)); // Divide by something which is not divisible by 2*PI.
      assert(latSteps >= 1);
      assert(latSteps >= 1);
    }
 
    int lngSteps = 1;
    if (lngRange > glm::pi<double>()) {
      lngSteps = int(lngRange / (glm::pi<double>() / 3.0));
      assert(lngSteps >= 1);
    }
 
    bbox += latLngAltToVec3(north, west, minHeight);
 
    for (int i = 1; i <= latSteps; ++i) {
      for (int j = 1; j <= lngSteps; ++j) {
        bbox += latLngAltToVec3((south + j * (lngRange / lngSteps)), (west + i * (latRange / latSteps)), minHeight);
        bbox += latLngAltToVec3((south + j * (lngRange / lngSteps)), (west + i * (latRange / latSteps)), maxHeight);
      }
    }
 
    bbox += latLngAltToVec3(south, east, maxHeight);
 
#if 1 // Sanity test
    assert(std::abs(bbox.min.x - bbox.max.x) > 0.1 &&
      std::abs(bbox.min.y - bbox.max.y) > 0.1 &&
      std::abs(bbox.min.z - bbox.max.z) > 0.1 && "BBox is flat!");
#endif
 
    // NOTE: Region boxes shall never be transformed according to the 3DTiles spec
    return bbox;
  }
  else if (bv.type == OGC3DTiles::BoundingVolumeType::SPHERE) { // Sphere
    glm::vec3 center = glm::vec3(bv.values[0], bv.values[1], bv.values[2]);
 
    bbox.min = center;
    bbox.max = center;
    glm::vec3 dx = center + glm::vec3(bv.values[3], 0, 0);
    glm::vec3 dy = center + glm::vec3(0, bv.values[3], 0);
    glm::vec3 dz = center + glm::vec3(0, 0, bv.values[3]);
    bbox += dx;
    bbox += dy;
    bbox += dz;
    bbox += -dx;
    bbox += -dy;
    bbox += -dz;
  }
  else {
    LOG_ERROR(logger, "Invalid BoundingVolume: No valid candidates.");
    assert(false && "Invalid BoundingVolume: No valid candidates.");
  }
 
  OGC3DTilesUtils::transformBBox(bbox, transform);
  return bbox;
}
 
/*
*  NOTE: If global a tile-coord is provided then the tileset's root-boundingbox needs to be provided as 'parentBBox'.
*  For local subtree tile-coords the subtree's bbox is needed as 'parentBBox'.
*/
Cogs::Geometry::DBoundingBox
Cogs::Core::OGC3DTilesUtils::getBBoxFromCoord(const OGC3DTiles::Coord& tileCoord, const Cogs::Geometry::DBoundingBox& parentBBox)
{
  const double width = parentBBox.max.x > parentBBox.min.x ? parentBBox.max.x - parentBBox.min.x : parentBBox.min.x - parentBBox.max.x;
  const double height = parentBBox.max.y > parentBBox.min.y ? parentBBox.max.y - parentBBox.min.y : parentBBox.min.y - parentBBox.max.y;
  const double depth = parentBBox.max.z > parentBBox.min.z ? parentBBox.max.z - parentBBox.min.z : parentBBox.min.z - parentBBox.max.z;
 
  assert(width >= 0 && "Invalid bbox width: Cannot be negative.");
  assert(height >= 0 && "Invalid bbox height: Cannot be negative.");
  assert(depth >= 0 && "Invalid bbox depth: Cannot be negative.");
 
  const bool isOctTree = tileCoord.z >= 0;
  const double range = static_cast<const double>(fastPow2(tileCoord.level));
 
  const double dx = width / range;
  const double dy = height / range;
  const double dz = depth / range;
 
  const glm::dvec3 min = glm::dvec3(parentBBox.min.x + tileCoord.x * dx,
    parentBBox.min.y + tileCoord.y * dy,
    isOctTree ? (parentBBox.min.z + tileCoord.z * dz) : parentBBox.min.z);
 
  const glm::dvec3 max = glm::dvec3(parentBBox.min.x + (tileCoord.x + 1) * dx,
    parentBBox.min.y + (tileCoord.y + 1) * dy,
    isOctTree ? (parentBBox.min.z + (tileCoord.z + 1) * dz) : parentBBox.max.z);
 
  Cogs::Geometry::DBoundingBox bbox = Cogs::Geometry::makeEmptyBoundingBox<Cogs::Geometry::DBoundingBox>();
  bbox += min;
  bbox += max;
 
  return bbox;
}
 
OGC3DTiles::BoundingVolume
Cogs::Core::OGC3DTilesUtils::getBoundingVolumeFromCoord(const OGC3DTiles::Coord& tileCoord, const OGC3DTiles::BoundingVolume& globalBoundingVolume)
{
  OGC3DTiles::BoundingVolume bv = globalBoundingVolume;
 
  const double range = static_cast<const double>(fastPow2(tileCoord.level));
  const bool isOctTree = tileCoord.z >= 0;
 
  if (globalBoundingVolume.type == OGC3DTiles::BoundingVolumeType::BOX) { // Oriented box [x, y, x,  xaxis.x, xaxis.y, xaxis.z,  yaxis.x, yaxis.y, yaxis.z,  zaxis.x, zaxis.y, zaxis.z] (axis are half-lengths)
    const glm::dvec3 center = glm::make_vec3(&globalBoundingVolume.values[0]);
    const glm::dvec3 xa = glm::abs(glm::make_vec3(&globalBoundingVolume.values[3]));
    const glm::dvec3 ya = glm::abs(glm::make_vec3(&globalBoundingVolume.values[6]));
    const glm::dvec3 za = glm::abs(glm::make_vec3(&globalBoundingVolume.values[9]));
 
    const glm::dvec3 xan = glm::normalize(xa);
    const glm::dvec3 yan = glm::normalize(ya);
    const glm::dvec3 zan = glm::normalize(za);
 
    const double dx = glm::length(xa) / range;
    const double dy = glm::length(ya) / range;
    const double dz = glm::length(za) / range;
 
    const glm::dvec3 corner = center - xa - ya - za;
    const glm::dvec3 newcenter = corner + xan * (dx * (tileCoord.x * 2 + 1)) + yan * (dy * (tileCoord.y * 2 + 1)) + zan * (dz * (tileCoord.z * 2 + 1));
 
    bv.values[0] = newcenter.x;
    bv.values[1] = newcenter.y;
 
    const glm::dvec3 newxa = xa / range;
    const glm::dvec3 newya = ya / range;
 
    bv.values[3] = newxa.x;
    bv.values[4] = newxa.y;
    bv.values[5] = newxa.z;
 
    bv.values[6] = newya.x;
    bv.values[7] = newya.y;
    bv.values[8] = newya.z;
 
    if (isOctTree) {
      bv.values[2] = newcenter.z;
      const glm::dvec3 newza = za / range;
      bv.values[9] = newza.x;
      bv.values[10] = newza.y;
      bv.values[11] = newza.z;
    }
    else {
      bv.values[2] = globalBoundingVolume.values[2];
      bv.values[9] = globalBoundingVolume.values[9];
      bv.values[10] = globalBoundingVolume.values[10];
      bv.values[11] = globalBoundingVolume.values[11];
    }
  }
  else if (globalBoundingVolume.type == OGC3DTiles::BoundingVolumeType::REGION) { // Region [west, south, east, north, minimum height, maximum height]
    // East/West
    const double width = globalBoundingVolume.values[2] > globalBoundingVolume.values[0] ?
      globalBoundingVolume.values[2] - globalBoundingVolume.values[0] :
      globalBoundingVolume.values[0] - globalBoundingVolume.values[2];
    // North/South
    const double height = globalBoundingVolume.values[3] > globalBoundingVolume.values[1] ?
      globalBoundingVolume.values[3] - globalBoundingVolume.values[1] :
      globalBoundingVolume.values[1] - globalBoundingVolume.values[3];
    // Top/Bottom
    const double depth = globalBoundingVolume.values[5] > globalBoundingVolume.values[4] ?
      globalBoundingVolume.values[5] - globalBoundingVolume.values[4] :
      globalBoundingVolume.values[4] - globalBoundingVolume.values[5];
 
    const double dx = width / range;
    const double dy = height / range;
    const double dz = depth / range;
 
    bv.values[0] = globalBoundingVolume.values[0] + tileCoord.x * dx; // west
    bv.values[1] = globalBoundingVolume.values[1] + tileCoord.y * dy; // south
    bv.values[2] = globalBoundingVolume.values[0] + (tileCoord.x + 1) * dx; // east
    bv.values[3] = globalBoundingVolume.values[1] + (tileCoord.y + 1) * dy; // north
 
    if (isOctTree) {
      bv.values[4] = globalBoundingVolume.values[4] + tileCoord.z * dz; // Min height
      bv.values[5] = globalBoundingVolume.values[4] + (tileCoord.z + 1) * dz; // Max height
    }
    else {
      bv.values[4] = globalBoundingVolume.values[4];
      bv.values[5] = globalBoundingVolume.values[5];
    }
 
#if 0
    printf("%s: %s (range=%f, w=%f, h=%f, d=%f, dx=%f, dy=%f, dz=%f)\n  (global: %s)\n", tileCoord.toString().c_str(), bv.toString().c_str(),
      range, width, height, depth, dx, dy, dz, globalBoundingVolume.toString().c_str());
#endif
  }
  else if (globalBoundingVolume.type == OGC3DTiles::BoundingVolumeType::SPHERE) { // Sphere [x, y, z, radius]
    const double delta = globalBoundingVolume.values[3] / range;
    const glm::dvec3 newcenter = glm::dvec3(delta * tileCoord.x, delta * tileCoord.y, delta * tileCoord.z);
    bv.values[0] = newcenter.x;
    bv.values[1] = newcenter.y;
    bv.values[2] = newcenter.z;
    bv.values[3] = delta;
    // FIXME: We need a dataset with sphere-bbox'es to be able to implement and test this.
    // Remove the warning if everything is fine.
    LOG_WARNING(logger, "getBoundingVolumeFromCoord(): Division of sphere-based boundingvolumes not tested yet.");
  }
  else {
    LOG_ERROR(logger, "Invalid BoundingVolume: No valid candidates.");
    assert(false && "Invalid BoundingVolume: No valid candidates.");
  }
 
  return bv;
}