EmbreeTargetShape.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "EmbreeTargetShape.h"
 
#include <iostream>
#include <iomanip>
#include <numeric>
#include <sstream>
 
#include "NaifDskApi.h"
 
#include "FileName.h"
#include "IException.h"
#include "IString.h"
#include "NaifStatus.h"
#include "Pvl.h"
 
namespace Isis {
 
  RTCMultiHitRay::RTCMultiHitRay() {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    hit.primID  = RTC_INVALID_GEOMETRY_ID;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = 0.0;
    ray.org_y = 0.0;
    ray.org_z = 0.0;
    ray.dir_x = 0.0;
    ray.dir_y = 0.0;
    ray.dir_z = 0.0;
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
 
  RTCMultiHitRay::RTCMultiHitRay(const std::vector<double> &origin,
                                 const std::vector<double> &direction) {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    hit.primID  = RTC_INVALID_GEOMETRY_ID;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = origin[0];
    ray.org_y = origin[1];
    ray.org_z = origin[2];
    ray.dir_x = direction[0];
    ray.dir_y = direction[1];
    ray.dir_z = direction[2];
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
 
  RTCMultiHitRay::RTCMultiHitRay(LinearAlgebra::Vector origin,
                                 LinearAlgebra::Vector direction) {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    hit.primID  = RTC_INVALID_GEOMETRY_ID;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = origin[0];
    ray.org_y = origin[1];
    ray.org_z = origin[2];
    ray.dir_x = direction[0];
    ray.dir_y = direction[1];
    ray.dir_z = direction[2];
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
 
  RTCOcclusionRay::RTCOcclusionRay() {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    ignorePrimID = -1;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    hit.primID  = RTC_INVALID_GEOMETRY_ID;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = 0.0;
    ray.org_y = 0.0;
    ray.org_z = 0.0;
    ray.dir_x = 0.0;
    ray.dir_y = 0.0;
    ray.dir_z = 0.0;
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
 
  RTCOcclusionRay::RTCOcclusionRay(const std::vector<double> &origin,
                                 const std::vector<double> &direction) {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    ignorePrimID  = -1;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = origin[0];
    ray.org_y = origin[1];
    ray.org_z = origin[2];
    ray.dir_x = direction[0];
    ray.dir_y = direction[1];
    ray.dir_z = direction[2];
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
  RTCOcclusionRay::RTCOcclusionRay(LinearAlgebra::Vector origin,
                                   LinearAlgebra::Vector direction) {
    ray.tnear   = 0.0;
    ray.tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    ray.mask    = 0xFFFFFFFF;
    lastHit = -1;
    hit.u       = 0.0;
    hit.v       = 0.0;
    hit.geomID  = RTC_INVALID_GEOMETRY_ID;
    ignorePrimID  = -1;
    hit.instID[0]  = RTC_INVALID_GEOMETRY_ID;
    ray.org_x = origin[0];
    ray.org_y = origin[1];
    ray.org_z = origin[2];
    ray.dir_x = direction[0];
    ray.dir_y = direction[1];
    ray.dir_z = direction[2];
    hit.Ng_x = 0.0;
    hit.Ng_y = 0.0;
    hit.Ng_z = 0.0;
  }
 
 
  RayHitInformation::RayHitInformation() {
    intersection = LinearAlgebra::vector(0, 0, 0);
    surfaceNormal = LinearAlgebra::vector(0, 0, 0);
    primID = 0;
  }
 
 
  RayHitInformation::RayHitInformation(LinearAlgebra::Vector &location,
                                       LinearAlgebra::Vector &normal,
                                       int prim)
      : intersection(location),
        surfaceNormal(normal) ,
        primID(prim) { }
 
 
  EmbreeTargetShape::EmbreeTargetShape()
      : m_name(),
        m_mesh(),
        m_cloud(),
        m_device(rtcNewDevice(NULL)),
        m_scene(rtcNewScene(m_device)) 
  {
    rtcSetSceneFlags(m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
    rtcSetSceneBuildQuality(m_scene, RTC_BUILD_QUALITY_HIGH);
  }
 
  EmbreeTargetShape::EmbreeTargetShape(pcl::PolygonMesh::Ptr mesh, const QString &name)
      : m_name(name),
        m_mesh(),
        m_cloud(),
        m_device(rtcNewDevice(NULL)),
        m_scene(rtcNewScene(m_device))
  {
    rtcSetSceneFlags(m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
    rtcSetSceneBuildQuality(m_scene, RTC_BUILD_QUALITY_HIGH);
    initMesh(mesh);
  }
 
  EmbreeTargetShape::EmbreeTargetShape(const QString &dem, const Pvl *conf)
      : m_name(),
        m_mesh(),
        m_cloud(),
        m_device(rtcNewDevice(NULL)),
        m_scene(rtcNewScene(m_device)) {
    rtcSetSceneFlags(m_scene, RTC_SCENE_FLAG_ROBUST | RTC_SCENE_FLAG_CONTEXT_FILTER_FUNCTION);
    rtcSetSceneBuildQuality(m_scene, RTC_BUILD_QUALITY_HIGH);
    FileName file(dem);
    pcl::PolygonMesh::Ptr mesh;
    m_name = file.baseName();
 
    try {
      // DEMs (ISIS cubes) TODO implement this
      if (file.extension() == "cub") {
        QString msg = "DEMs cannot be used to create an EmbreeTargetShape.";
        throw IException(IException::Io, msg, _FILEINFO_);
      }
      // DSKs
      else if (file.extension().toLower() == "bds") {
        mesh = readDSK(file);
      }
      // Let PCL try to handle other formats (obj, ply, etc.)
      else {
        mesh = readPC(file);
      }
    }
    catch (IException &e) {
      QString msg = "Failed creating an EmbreeTargetShape from ["
                    + file.expanded() + "].";
      throw IException(e, IException::Io, msg, _FILEINFO_);
    }
    initMesh(mesh);
  }
 
 
  pcl::PolygonMesh::Ptr EmbreeTargetShape::readDSK(FileName file) {
 
    SpiceInt      dskHandle;      
    SpiceDLADescr dlaDescriptor;  
    SpiceDSKDescr dskDescriptor;  
    SpiceInt      numPlates;      
    SpiceInt      numVertices;    
 
    // Sanity check
    if ( !file.fileExists() ) {
      QString mess = "NAIF DSK file [" + file.expanded() + "] does not exist.";
      throw IException(IException::User, mess, _FILEINFO_);
    }
  
    // Open the NAIF Digital Shape Kernel (DSK)
    dasopr_c( file.expanded().toLatin1().data(), &dskHandle );
    NaifStatus::CheckErrors();
  
    // Search to the first DLA segment
    SpiceBoolean found;
    dlabfs_c( dskHandle, &dlaDescriptor, &found );
    NaifStatus::CheckErrors();
    if ( !found ) {
      QString mess = "No segments found in DSK file [" + file.expanded() + "]"; 
      throw IException(IException::User, mess, _FILEINFO_);
    }
 
    dskgd_c( dskHandle, &dlaDescriptor, &dskDescriptor );
    NaifStatus::CheckErrors();
 
    // Get The number of polygons and vertices
    dskz02_c( dskHandle, &dlaDescriptor, &numVertices, &numPlates );
    NaifStatus::CheckErrors();
 
    // Allocate polygon and vertices arrays
    //   These arrays are actually numVertices x 3 and numPlates x 3,
    //     this allocation is easier and produces needed memory layout for dskv02_c
    //   These arrays are very large, so they need to be heap allocated.
    double *verticesArray = new double[numVertices * 3];
    int *polygonsArray = new int[numPlates * 3];
 
    // Read the vertices from the dsk file
    SpiceInt numRead = 0;
    dskv02_c(dskHandle, &dlaDescriptor, 1, numVertices,
             &numRead, ( SpiceDouble(*)[3] )(verticesArray) );
    NaifStatus::CheckErrors();
    if ( numRead != numVertices ) {
      QString msg = "Failed reading all vertices from the DSK file, ["
                    + toString(numRead) + "] out of ["
                    + toString(numVertices) + "] vertices read.";
      throw IException(IException::Io, msg, _FILEINFO_);
    }
 
    // Read the polygons from the DSK
    numRead = 0;
    dskp02_c(dskHandle, &dlaDescriptor, 1, numPlates,
             &numRead, ( SpiceInt(*)[3] )(polygonsArray) );
    NaifStatus::CheckErrors();
    if ( numRead != numPlates ) {
      QString msg = "Failed reading all polygons from the DSK file, ["
                    + toString(numRead) + "] out of ["
                    + toString(numPlates) + "] polygons read.";
      throw IException(IException::Io, msg, _FILEINFO_);
    }
 
    // Store the vertices in a point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex) {
      cloud->push_back(pcl::PointXYZ(verticesArray[vertexIndex * 3],
                                     verticesArray[vertexIndex * 3 + 1],
                                     verticesArray[vertexIndex * 3 + 2]));
    }
 
    // Store the polygons as a vector of vertices
    std::vector<pcl::Vertices> polygons;
    for (int plateIndex = 0; plateIndex < numPlates; ++plateIndex) {
      pcl::Vertices vertexIndices;
      // NAIF uses 1 based indexing for the vertices, so subtract 1
      vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3] - 1);
      vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 1] - 1);
      vertexIndices.vertices.push_back(polygonsArray[plateIndex * 3 + 2] - 1);
      polygons.push_back(vertexIndices);
    }
 
    // Create the mesh
    pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
    mesh->polygons = polygons;
    pcl::PCLPointCloud2::Ptr cloudBlob(new pcl::PCLPointCloud2);
    pcl::toPCLPointCloud2(*cloud, *cloudBlob);
    mesh->cloud = *cloudBlob;
 
    // Free the vectors used to read the vertices and polygons
    delete [] verticesArray;
    delete [] polygonsArray;
 
    return mesh;
  }
 
 
  pcl::PolygonMesh::Ptr EmbreeTargetShape::readPC(FileName file) {
    pcl::PolygonMesh::Ptr mesh( new pcl::PolygonMesh );
 
    int loadStatus = pcl::io::load(file.expanded().toStdString(), *mesh);
    if (loadStatus == -1) {
      QString msg = "Failed loading target shape file [" + file.expanded() + "]";
      throw IException(IException::Io, msg, _FILEINFO_);
    }
    return mesh;
  }
 
 
  void EmbreeTargetShape::initMesh(pcl::PolygonMesh::Ptr mesh) {
    m_mesh = mesh;
 
    // The points are stored in a pcl::PCLPointCloud2 object that we cannot used.
    // So, convert them into a pcl::PointCloud<pcl::PointXYZ> object that we can use.
    pcl::fromPCLPointCloud2(mesh->cloud, m_cloud);
 
    // Create a static geometry (the body) in our scene
    RTCGeometry rtcMesh = rtcNewGeometry(m_device, RTC_GEOMETRY_TYPE_TRIANGLE);
    // Add vertices and faces (indices)
    if (!isValid()) {
      return;
    }
    // Add the body's vertices to the Embree ray tracing device's vertex buffer
    Vertex *vertices = (Vertex *)rtcSetNewGeometryBuffer(rtcMesh, RTC_BUFFER_TYPE_VERTEX, 0, RTC_FORMAT_FLOAT3, sizeof(Vertex), numberOfVertices());
    for (int v = 0; v < numberOfVertices(); ++v) {
      vertices[v].x = m_cloud.points[v].x;
      vertices[v].y = m_cloud.points[v].y;
      vertices[v].z = m_cloud.points[v].z;
    }
 
    if (!isValid()) {
      return;
    }
    int tri = 0;
    Triangle *triangles = (Triangle *)rtcSetNewGeometryBuffer(rtcMesh, RTC_BUFFER_TYPE_INDEX, 0, RTC_FORMAT_UINT3, sizeof(Triangle), numberOfPolygons());
    // Add the body's face (vertex indices) to the Embree device's index buffer
    for (int t = 0; t < numberOfPolygons(); ++t) {
      triangles[t].v0 = m_mesh->polygons[t].vertices[0];
      triangles[t].v1 = m_mesh->polygons[t].vertices[1];
      triangles[t].v2 = m_mesh->polygons[t].vertices[2];
    }
 
    // Add the multi-hit filter
    rtcSetGeometryIntersectFilterFunction(rtcMesh, EmbreeTargetShape::multiHitFilter);
 
    // Add the occlusion filter
    rtcSetGeometryOccludedFilterFunction(rtcMesh, EmbreeTargetShape::occlusionFilter);
 
    rtcCommitGeometry(rtcMesh);
    unsigned int geomID = rtcAttachGeometry(m_scene, rtcMesh);
    rtcReleaseGeometry(rtcMesh);
 
    // Done, now we can perform some ray tracing
    rtcCommitScene(m_scene);
  }
 
 
  void EmbreeTargetShape::addVertices(int geomID) {
    // if (!isValid()) {
    //   return;
    // }
    // // Add the body's vertices to the Embree ray tracing device's vertex buffer
    // Vertex *vertices = (Vertex *) rtcMapBuffer(m_scene, geomID, RTC_VERTEX_BUFFER);
    // for (int v = 0; v < numberOfVertices(); ++v) {
    //   vertices[v].x = m_cloud.points[v].x;
    //   vertices[v].y = m_cloud.points[v].y;
    //   vertices[v].z = m_cloud.points[v].z;
    // }
    // // Flush buffer
    // rtcUnmapBuffer(m_scene, geomID, RTC_VERTEX_BUFFER);
  }
 
 
  void EmbreeTargetShape::addIndices(int geomID) {
    // if (!isValid()) {
    //   return;
    // }
    // // Add the body's face (vertex indices) to the Embree device's index buffer
    // Triangle *triangles = (Triangle *) rtcMapBuffer(m_scene, geomID, RTC_INDEX_BUFFER);
    // for (int t = 0; t < numberOfPolygons(); ++t) {
    //   triangles[t].v0 = m_mesh->polygons[t].vertices[0];
    //   triangles[t].v1 = m_mesh->polygons[t].vertices[1];
    //   triangles[t].v2 = m_mesh->polygons[t].vertices[2];
    // }
    // // Flush buffer
    // rtcUnmapBuffer(m_scene, geomID, RTC_INDEX_BUFFER);
  }
 
 
  EmbreeTargetShape::~EmbreeTargetShape() {
    rtcReleaseScene(m_scene);
    rtcReleaseDevice(m_device);
  }
 
 
  int EmbreeTargetShape::numberOfPolygons() const {
    if (isValid()) {
      return m_mesh->polygons.size();
    }
    return 0;
  }
 
 
  int EmbreeTargetShape::numberOfVertices() const {
    if (isValid()) {
      return m_mesh->cloud.height * m_mesh->cloud.width;
    }
    return 0;
  }
 
 
  RTCBounds EmbreeTargetShape::sceneBounds() const {
    RTCBounds sceneBounds;
    if (isValid()) {
      rtcGetSceneBounds(m_scene, &sceneBounds);
    }
    else {
      sceneBounds.lower_x = 0.0;
      sceneBounds.lower_y = 0.0;
      sceneBounds.lower_z = 0.0;
      sceneBounds.upper_x = 0.0;
      sceneBounds.upper_y = 0.0;
      sceneBounds.upper_z = 0.0;
    }
    return sceneBounds;
  }
 
 
  double EmbreeTargetShape::maximumSceneDistance() const {
    RTCBounds sceneBoundary = sceneBounds();
    LinearAlgebra::Vector diagonal(3);
    diagonal[0] = sceneBoundary.upper_x - sceneBoundary.lower_x;
    diagonal[1] = sceneBoundary.upper_y - sceneBoundary.lower_y;
    diagonal[2] = sceneBoundary.upper_z - sceneBoundary.lower_z;
    return LinearAlgebra::magnitude(diagonal);
  }
 
 
  void EmbreeTargetShape::intersectRay(RTCMultiHitRay &ray) {
    if (isValid()) {
      RTCIntersectContext context;
      rtcInitIntersectContext(&context);
 
      rtcIntersect1(m_scene, &context, (RTCRayHit *)&ray);
    }
  }
 
 
  bool EmbreeTargetShape::isOccluded(RTCOcclusionRay &ray) {
    RTCIntersectContext context;
    rtcInitIntersectContext(&context);
 
    rtcOccluded1(m_scene,  &context, (RTCRay*)&ray);
 
    // rtcOccluded sets the ray.tfar to -inf if the ray hits anything
    if (isinf(ray.ray.tfar) && ray.ray.tfar < 0) {
      return true;
    }
    return false;
  }
 
 
  RayHitInformation EmbreeTargetShape::getHitInformation(RTCMultiHitRay &ray, int hitIndex) {
    if (hitIndex > ray.lastHit || hitIndex < 0) {
      QString msg = "Hit index [" + toString(hitIndex) + "] is out of bounds.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    // Get the vertices of the triangle hit
    pcl::PointXYZ v0 = m_cloud.points[m_mesh->polygons[ray.hitPrimIDs[hitIndex]].vertices[0]];
    pcl::PointXYZ v1 = m_cloud.points[m_mesh->polygons[ray.hitPrimIDs[hitIndex]].vertices[1]];
    pcl::PointXYZ v2 = m_cloud.points[m_mesh->polygons[ray.hitPrimIDs[hitIndex]].vertices[2]];
 
    // The intersection location comes out in barycentric coordinates, (u, v, w).
    // Only u and v are returned because u + v + w = 1. If the coordinates of the
    // triangle vertices are v0, v1, and v2, then the cartesian coordinates are:
    //   w*v0 + u*v1 + v*v2
    float u = ray.hitUs[hitIndex];
    float v = ray.hitVs[hitIndex];
    float w = 1.0 - u - v;
 
    LinearAlgebra::Vector intersection(3);
    intersection[0] = w*v0.x + v*v1.x + u*v2.x;
    intersection[1] = w*v0.y + v*v1.y + u*v2.y;
    intersection[2] = w*v0.z + v*v1.z + u*v2.z;
 
    // Calculate the normal vector as (v1 - v0) x (v2 - v0) and normalize it
    // TODO This calculation assumes that the shape conforms to the NAIF dsk standard
    //      of the plate vertices being ordered counterclockwise about the normal.
    //      Check if this is true for other file types and/or make a more generic process
    LinearAlgebra::Vector surfaceNormal(3);
    surfaceNormal[0] = (v1.y - v0.y) * (v2.z - v0.z)
                       - (v1.z - v0.z) * (v2.y - v0.y);
    surfaceNormal[1] = (v1.z - v0.z) * (v2.x - v0.x)
                       - (v1.x - v0.x) * (v2.z - v0.z);
    surfaceNormal[2] = (v1.x - v0.x) * (v2.y - v0.y)
                       - (v1.y - v0.y) * (v2.x - v0.x);
 
    // The surface normal is not normalized so normalize it.
    surfaceNormal = LinearAlgebra::normalize(surfaceNormal);
    return RayHitInformation(intersection, surfaceNormal, ray.hitPrimIDs[hitIndex]);
  }
 
 
  QString EmbreeTargetShape::name() const {
    return ( m_name );
  }
 
 
  bool EmbreeTargetShape::isValid() const {
    return m_mesh.get();
  }
 
 
  void EmbreeTargetShape::multiHitFilter(const RTCFilterFunctionNArguments* args) {
    /* avoid crashing when debug visualizations are used */
    if (args->context == nullptr)
      return;
 
    assert(args->N == 1);
    int *valid = args->valid;
    if (valid[0] != -1) {
      return;
    }
    RTCMultiHitRay *ray = (RTCMultiHitRay *)args->ray;
    RTCHit *hit = (RTCHit *)args->hit;
 
    // Calculate the index to store the hit in
    ray->lastHit++;
 
    // Store the hits
    ray->hitGeomIDs[ray->lastHit] = hit->geomID;
    ray->hitPrimIDs[ray->lastHit] = hit->primID;
    ray->hitUs[ray->lastHit] = hit->u;
    ray->hitVs[ray->lastHit] = hit->v;
 
    // If there are less than 16 hits, continue ray tracing.
    if (ray->lastHit < 15) {
      valid[0] = 0;
      ray->hit.geomID = RTC_INVALID_GEOMETRY_ID;
      hit->geomID = RTC_INVALID_GEOMETRY_ID;
    }
  }
 
  void EmbreeTargetShape::occlusionFilter(const RTCFilterFunctionNArguments* args) {
    /* avoid crashing when debug visualizations are used */
    if (args->context == nullptr)
      return;
 
    assert(args->N == 1);
    int *valid = args->valid;
    if (valid[0] != -1) {
      return;
    }
    RTCOcclusionRay *ray = (RTCOcclusionRay *)args->ray;
    RTCHit *hit = (RTCHit *)args->hit;
    ray->hit.primID = hit->primID;
 
    // This is the case where we've re-intersected the occluded plate. If this happens, ignore 
    // and keep tracing
    if (ray->hit.primID == ray->ignorePrimID) {
      valid[0] = 0;
      // ray->hit.geomID = RTC_INVALID_GEOMETRY_ID;
      // hit->geomID = RTC_INVALID_GEOMETRY_ID;
    }
  }
 
}  // namespace Isis