EmbreeTargetShape.cpp

Go to the documentation of this file.

#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() {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    primID  = RTC_INVALID_GEOMETRY_ID;
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = 0.0;
    org[1] = 0.0;
    org[2] = 0.0;
    dir[0] = 0.0;
    dir[1] = 0.0;
    dir[2] = 0.0;
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 0.0;
  }


  RTCMultiHitRay::RTCMultiHitRay(const std::vector<double> &origin,
                                 const std::vector<double> &direction) {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    primID  = RTC_INVALID_GEOMETRY_ID;
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = origin[0];
    org[1] = origin[1];
    org[2] = origin[2];
    dir[0] = direction[0];
    dir[1] = direction[1];
    dir[2] = direction[2];
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 0.0;
  }


  RTCMultiHitRay::RTCMultiHitRay(LinearAlgebra::Vector origin,
                                 LinearAlgebra::Vector direction) {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    primID  = RTC_INVALID_GEOMETRY_ID;
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = origin[0];
    org[1] = origin[1];
    org[2] = origin[2];
    dir[0] = direction[0];
    dir[1] = direction[1];
    dir[2] = direction[2];
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 0.0;
  }


  RTCOcclusionRay::RTCOcclusionRay() {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    ignorePrimID = -1;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    primID  = RTC_INVALID_GEOMETRY_ID;
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = 0.0;
    org[1] = 0.0;
    org[2] = 0.0;
    dir[0] = 0.0;
    dir[1] = 0.0;
    dir[2] = 0.0;
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 0.0;
  }


  RTCOcclusionRay::RTCOcclusionRay(const std::vector<double> &origin,
                                 const std::vector<double> &direction) {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    ignorePrimID  = -1;
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = origin[0];
    org[1] = origin[1];
    org[2] = origin[2];
    dir[0] = direction[0];
    dir[1] = direction[1];
    dir[2] = direction[2];
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 0.0;
  }

  RTCOcclusionRay::RTCOcclusionRay(LinearAlgebra::Vector origin,
                                   LinearAlgebra::Vector direction) {
    tnear   = 0.0;
    tfar    = std::numeric_limits<float>::infinity(); // Should be INF
    mask    = 0xFFFFFFFF;
    lastHit = -1;
    u       = 0.0;
    v       = 0.0;
    geomID  = RTC_INVALID_GEOMETRY_ID;
    ignorePrimID  = -1; 
    instID  = RTC_INVALID_GEOMETRY_ID;
    org[0] = origin[0];
    org[1] = origin[1];
    org[2] = origin[2];
    dir[0] = direction[0];
    dir[1] = direction[1];
    dir[2] = direction[2];
    Ng[0] = 0.0;
    Ng[1] = 0.0;
    Ng[2] = 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(rtcDeviceNewScene(m_device,
                                  RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
                                  RTC_INTERSECT1)) { }


  EmbreeTargetShape::EmbreeTargetShape(pcl::PolygonMesh::Ptr mesh, const QString &name)
      : m_name(name),
        m_mesh(),
        m_cloud(),
        m_device(rtcNewDevice(NULL)),
        m_scene(rtcDeviceNewScene(m_device,
                                  RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
                                  RTC_INTERSECT1)) {
    initMesh(mesh);
  }


  EmbreeTargetShape::EmbreeTargetShape(const QString &dem, const Pvl *conf)
      : m_name(),
        m_mesh(),
        m_cloud(),
        m_device(rtcNewDevice(NULL)),
        m_scene(rtcDeviceNewScene(m_device,
                                  RTC_SCENE_STATIC | RTC_SCENE_HIGH_QUALITY | RTC_SCENE_ROBUST,
                                  RTC_INTERSECT1)) {
    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() == "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
    unsigned geomID = rtcNewTriangleMesh(m_scene,
                                         RTC_GEOMETRY_STATIC,
                                         numberOfPolygons(),
                                         numberOfVertices(),
                                         1);
    // Add vertices and faces (indices)
    addVertices(geomID); // add cloud of points
    addIndices(geomID);  // connect dots of cloud into triangles

    // Add the multi-hit filter
    rtcSetIntersectionFilterFunction(m_scene, geomID,
                                     (RTCFilterFunc)&EmbreeTargetShape::multiHitFilter);

    // Add the occlusion filter
    rtcSetOcclusionFilterFunction(m_scene, geomID,
                                    (RTCFilterFunc)&EmbreeTargetShape::occlusionFilter);

    // Done, now we can perform some ray tracing
    rtcCommit(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() {
    rtcDeleteScene(m_scene);
    rtcDeleteDevice(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()) {
      rtcGetBounds(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()) {
      rtcIntersect(m_scene, *((RTCRay*)&ray));
    }
  }


  bool EmbreeTargetShape::isOccluded(RTCOcclusionRay &ray) {
    rtcOccluded(m_scene, ray);

    // rtcOccluded sets the geomID to 0 if the ray hits anything
    if (ray.geomID == 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(void* userDataPtr, RTCMultiHitRay& ray) {
    // Calculate the index to store the hit in
    ray.lastHit ++;

    // Store the hits
    ray.hitGeomIDs[ray.lastHit] = ray.geomID;
    ray.hitPrimIDs[ray.lastHit] = ray.primID;
    ray.hitUs[ray.lastHit] = ray.u;
    ray.hitVs[ray.lastHit] = ray.v;

    // If there are less than 16 hits, continue ray tracing.
    if (ray.lastHit < 15) {
      ray.geomID = RTC_INVALID_GEOMETRY_ID;
    }
  }

  void EmbreeTargetShape::occlusionFilter(void* userDataPtr, RTCOcclusionRay& ray) {
        
    // This is the case where we've re-intersected the occluded plate. If this happens, ignore 
    // and keep tracing
    if ( ray.primID == ray.ignorePrimID) {
      ray.geomID = RTC_INVALID_GEOMETRY_ID;
    }
  }

}  // namespace Isis