libigl_Martin/include/igl/embree/EmbreeIntersector.h

// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//               2014 Christian Schüller <schuellchr@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
// igl function interface for Embree2.2
//
// Necessary changes to switch from previous Embree versions:
// * Use igl:Hit instead of embree:Hit (where id0 -> id)
// * For Embree2.2
// * Uncomment #define __USE_RAY_MASK__ in platform.h to enable masking
//
// TODO: Check for NANs in the input

#ifndef IGL_EMBREE_INTERSECTOR_H
#define IGL_EMBREE_INTERSECTOR_H

#include <Eigen/Geometry>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <vector>
#include <embree2/rtcore.h>
#include <embree2/rtcore_ray.h>
#include <iostream>
#include "Hit.h"
#include <iostream>

namespace igl
{
  class EmbreeIntersector
  {
  public:
    // Initialize embree engine. This will be called on instance `init()`
    // calls. If already inited then this function does nothing: it is harmless
    // to call more than once.
    static inline void global_init();
  private:
    // Deinitialize the embree engine.
    static inline void global_deinit();
  public:
    typedef Eigen::Matrix<float,Eigen::Dynamic,3> PointMatrixType;
    typedef Eigen::Matrix<int,Eigen::Dynamic,3> FaceMatrixType;
  public:
    inline EmbreeIntersector();
  private:
    // Copying and assignment are not allowed.
    inline EmbreeIntersector(const EmbreeIntersector & that);
    inline EmbreeIntersector & operator=(const EmbreeIntersector &);
  public:
    virtual inline ~EmbreeIntersector();

    // Initialize with a given mesh.
    //
    // Inputs:
    //   V  #V by 3 list of vertex positions
    //   F  #F by 3 list of Oriented triangles
    // Side effects:
    //   The first time this is ever called the embree engine is initialized.
    inline void init(
      const PointMatrixType& V,
      const FaceMatrixType& F);

    // Initialize with a given mesh.
    //
    // Inputs:
    //   V  vector of #V by 3 list of vertex positions for each geometry
    //   F  vector of #F by 3 list of Oriented triangles for each geometry
    //   masks  a 32 bit mask to identify active geometries.
    // Side effects:
    //   The first time this is ever called the embree engine is initialized.
    inline void init(
      const std::vector<const PointMatrixType*>& V,
      const std::vector<const FaceMatrixType*>& F,
      const std::vector<int>& masks);

    // Deinitialize embree datasctructures for current mesh.  Also called on
    // destruction: no need to call if you just want to init() once and
    // destroy.
    inline void deinit();

    // Given a ray find the first hit
    //
    // Inputs:
    //   origin     3d origin point of ray
    //   direction  3d (not necessarily normalized) direction vector of ray
    //   tnear      start of ray segment
    //   tfar       end of ray segment
    //   masks      a 32 bit mask to identify active geometries.
    // Output:
    //   hit        information about hit
    // Returns true if and only if there was a hit
    inline bool intersectRay(
      const Eigen::RowVector3f& origin,
      const Eigen::RowVector3f& direction,
      Hit& hit,
      float tnear = 0,
      float tfar = -1,
      int mask = 0xFFFFFFFF) const;

    // Given a ray find the first hit
    // This is a conservative hit test where multiple rays within a small radius
    // will be tested and only the closesest hit is returned.
    //
    // Inputs:
    //   origin     3d origin point of ray
    //   direction  3d (not necessarily normalized) direction vector of ray
    //   tnear      start of ray segment
    //   tfar       end of ray segment
    //   masks      a 32 bit mask to identify active geometries.
    //   geoId      id of geometry mask (default -1 if no: no masking)
    //   closestHit true for gets closest hit, false for furthest hit
    // Output:
    //   hit        information about hit
    // Returns true if and only if there was a hit
    inline bool intersectBeam(
      const Eigen::RowVector3f& origin,
      const Eigen::RowVector3f& direction,
      Hit& hit,
      float tnear = 0,
      float tfar = -1,
      int mask = 0xFFFFFFFF,
      int geoId = -1,
      bool closestHit = true) const;

    // Given a ray find all hits in order
    //
    // Inputs:
    //   origin     3d origin point of ray
    //   direction  3d (not necessarily normalized) direction vector of ray
    //   tnear      start of ray segment
    //   tfar       end of ray segment
    //   masks      a 32 bit mask to identify active geometries.
    // Output:
    //   hit        information about hit
    //   num_rays   number of rays shot (at least one)
    // Returns true if and only if there was a hit
    inline bool intersectRay(
      const Eigen::RowVector3f& origin,
      const Eigen::RowVector3f& direction,
      std::vector<Hit > &hits,
      int& num_rays,
      float tnear = 0,
      float tfar = std::numeric_limits<float>::infinity(),
      int mask = 0xFFFFFFFF) const;

    // Given a ray find the first hit
    //
    // Inputs:
    //   a    3d first end point of segment
    //   ab   3d vector from a to other endpoint b
    // Output:
    //   hit  information about hit
    // Returns true if and only if there was a hit
    inline bool intersectSegment(
      const Eigen::RowVector3f& a,
      const Eigen::RowVector3f& ab,
      Hit &hit,
      int mask = 0xFFFFFFFF) const;

  private:

    struct Vertex   {float x,y,z,a;};
    struct Triangle {int v0, v1, v2;};

    RTCScene scene;
    unsigned geomID;
    Vertex* vertices;
    Triangle* triangles;
    bool initialized;

    inline void createRay(
      RTCRay& ray,
      const Eigen::RowVector3f& origin,
      const Eigen::RowVector3f& direction,
      float tnear,
      float tfar,
      int mask) const;
  };
}

// Implementation
#include <igl/EPS.h>
// This unfortunately cannot be a static field of EmbreeIntersector because it
// would depend on the template and then we might end up with initializing
// embree twice. If only there was a way to ask embree if it's already
// initialized...
namespace igl
{
  // Keeps track of whether the **Global** Embree intersector has been
  // initialized. This should never been done at the global scope.
  static bool EmbreeIntersector_inited = false;
}

inline void igl::EmbreeIntersector::global_init()
{
  if(!EmbreeIntersector_inited)
  {
    rtcInit();
    if(rtcGetError() != RTC_NO_ERROR)
      std::cerr << "Embree: An error occured while initialiting embree core!" << std::endl;
#ifdef IGL_VERBOSE
    else
      std::cerr << "Embree: core initialized." << std::endl;
#endif
    EmbreeIntersector_inited = true;
  }
}

inline void igl::EmbreeIntersector::global_deinit()
{
  EmbreeIntersector_inited = false;
  rtcExit();
}

inline igl::EmbreeIntersector::EmbreeIntersector()
  :
  //scene(NULL),
  geomID(0),
  triangles(NULL),
  vertices(NULL),
  initialized(false)
{
}

inline igl::EmbreeIntersector::EmbreeIntersector(
  const EmbreeIntersector &)
  :// To make -Weffc++ happy
  //scene(NULL),
  geomID(0),
  triangles(NULL),
  vertices(NULL),
  initialized(false)
{
  assert(false && "Embree: Copying EmbreeIntersector is not allowed");
}

inline igl::EmbreeIntersector & igl::EmbreeIntersector::operator=(
  const EmbreeIntersector &)
{
  assert(false && "Embree: Assigning an EmbreeIntersector is not allowed");
  return *this;
}


inline void igl::EmbreeIntersector::init(
  const PointMatrixType& V,
  const FaceMatrixType& F)
{
  std::vector<const PointMatrixType*> Vtemp;
  std::vector<const FaceMatrixType*> Ftemp;
  std::vector<int> masks;
  Vtemp.push_back(&V);
  Ftemp.push_back(&F);
  masks.push_back(0xFFFFFFFF);
  init(Vtemp,Ftemp,masks);
}

inline void igl::EmbreeIntersector::init(
  const std::vector<const PointMatrixType*>& V,
  const std::vector<const FaceMatrixType*>& F,
  const std::vector<int>& masks)
{

  if(initialized)
    deinit();

  using namespace std;
  global_init();

  if(V.size() == 0 || F.size() == 0)
  {
    std::cerr << "Embree: No geometry specified!";
    return;
  }

  // create a scene
  scene = rtcNewScene(RTC_SCENE_ROBUST | RTC_SCENE_HIGH_QUALITY,RTC_INTERSECT1);

  for(int g=0;g<V.size();g++)
  {
    // create triangle mesh geometry in that scene
    geomID = rtcNewTriangleMesh(scene,RTC_GEOMETRY_STATIC,F[g]->rows(),V[g]->rows(),1);

    // fill vertex buffer
    vertices = (Vertex*)rtcMapBuffer(scene,geomID,RTC_VERTEX_BUFFER);
    for(int i=0;i<(int)V[g]->rows();i++)
    {
      vertices[i].x = (float)V[g]->coeff(i,0);
      vertices[i].y = (float)V[g]->coeff(i,1);
      vertices[i].z = (float)V[g]->coeff(i,2);
    }
    rtcUnmapBuffer(scene,geomID,RTC_VERTEX_BUFFER);

    // fill triangle buffer
    triangles = (Triangle*) rtcMapBuffer(scene,geomID,RTC_INDEX_BUFFER);
    for(int i=0;i<(int)F[g]->rows();i++)
    {
      triangles[i].v0 = (int)F[g]->coeff(i,0);
      triangles[i].v1 = (int)F[g]->coeff(i,1);
      triangles[i].v2 = (int)F[g]->coeff(i,2);
    }
    rtcUnmapBuffer(scene,geomID,RTC_INDEX_BUFFER);

    rtcSetMask(scene,geomID,masks[g]);
  }

  rtcCommit(scene);

  if(rtcGetError() != RTC_NO_ERROR)
      std::cerr << "Embree: An error occured while initializing the provided geometry!" << endl;
#ifdef IGL_VERBOSE
  else
    std::cerr << "Embree: geometry added." << endl;
#endif

  initialized = true;
}

igl::EmbreeIntersector
::~EmbreeIntersector()
{
  if(initialized)
    deinit();
}

void igl::EmbreeIntersector::deinit()
{
  rtcDeleteScene(scene);

  if(rtcGetError() != RTC_NO_ERROR)
      std::cerr << "Embree: An error occured while resetting!" << std::endl;
#ifdef IGL_VERBOSE
  else
    std::cerr << "Embree: geometry removed." << std::endl;
#endif
}

inline bool igl::EmbreeIntersector::intersectRay(
  const Eigen::RowVector3f& origin,
  const Eigen::RowVector3f& direction,
  Hit& hit,
  float tnear,
  float tfar,
  int mask) const
{
  RTCRay ray;
  createRay(ray, origin,direction,tnear,std::numeric_limits<float>::infinity(),mask);

  // shot ray
  rtcIntersect(scene,ray);
#ifdef IGL_VERBOSE
  if(rtcGetError() != RTC_NO_ERROR)
      std::cerr << "Embree: An error occured while resetting!" << std::endl;
#endif

  if(ray.geomID != RTC_INVALID_GEOMETRY_ID)
  {
    hit.id = ray.primID;
    hit.gid = ray.geomID;
    hit.u = ray.u;
    hit.v = ray.v;
    hit.t = ray.tfar;
    return true;
  }

  return false;
}

inline bool igl::EmbreeIntersector::intersectBeam(
      const Eigen::RowVector3f& origin,
      const Eigen::RowVector3f& direction,
      Hit& hit,
      float tnear,
      float tfar,
      int mask,
      int geoId,
      bool closestHit) const
{
  bool hasHit = false;
  Hit bestHit;

  if(closestHit)
    bestHit.t = std::numeric_limits<float>::max();
  else
    bestHit.t = 0;

  hasHit = (intersectRay(origin,direction,hit,tnear,tfar,mask) && (hit.gid == geoId || geoId == -1));
  if(hasHit)
    bestHit = hit;

  // sample points around actual ray (conservative hitcheck)
  float eps= 1e-5;
  int density = 4;

  Eigen::RowVector3f up; up.setRandom(1,3);
  while (fabs(direction.cross(up).norm()) < 1e-5)
    up.setRandom(1,3);
  Eigen::RowVector3f offset = direction.cross(up).normalized();

  Eigen::Matrix3f rot = Eigen::AngleAxis<float>(2*3.14159265358979/density,direction).toRotationMatrix();

  for(int r=0;r<density;r++)
  {
    if(intersectRay(origin+offset*eps,direction,hit,tnear,tfar,mask) && ((closestHit && (hit.t < bestHit.t)) || (!closestHit && (hit.t > bestHit.t))) && (hit.gid == geoId || geoId == -1))
    {
      bestHit = hit;
      hasHit = true;
    }
    offset = rot*offset.transpose();
  }

  hit = bestHit;
  return hasHit;
}

inline bool
igl::EmbreeIntersector
::intersectRay(
  const Eigen::RowVector3f& origin,
  const Eigen::RowVector3f& direction,
  std::vector<Hit > &hits,
  int& num_rays,
  float tnear,
  float tfar,
  int mask) const
{
  using namespace std;
  num_rays = 0;
  hits.clear();
  int last_id0 = -1;
  double self_hits = 0;
  // This epsilon is directly correleated to the number of missed hits, smaller
  // means more accurate and slower
  //const double eps = DOUBLE_EPS;
  const double eps = FLOAT_EPS;
  double min_t = tnear;
  bool large_hits_warned = false;
  RTCRay ray;
  createRay(ray,origin,direction,tnear,std::numeric_limits<float>::infinity(),mask);

  while(true)
  {
    ray.tnear = min_t;
    ray.tfar = tfar;
    ray.geomID = RTC_INVALID_GEOMETRY_ID;
    ray.primID = RTC_INVALID_GEOMETRY_ID;
    ray.instID = RTC_INVALID_GEOMETRY_ID;
    num_rays++;
    rtcIntersect(scene,ray);
    if(ray.geomID != RTC_INVALID_GEOMETRY_ID)
    {
      // Hit self again, progressively advance
      if(ray.primID == last_id0 || ray.tfar <= min_t)
      {
        // push min_t a bit more
        //double t_push = pow(2.0,self_hits-4)*(hit.t<eps?eps:hit.t);
        double t_push = pow(2.0,self_hits)*eps;
        #ifdef IGL_VERBOSE
        std::cerr<<"  t_push: "<<t_push<<endl;
        #endif
        //o = o+t_push*d;
        min_t += t_push;
        self_hits++;
      }
      else
      {
        Hit hit;
        hit.id = ray.primID;
        hit.gid = ray.geomID;
        hit.u = ray.u;
        hit.v = ray.v;
        hit.t = ray.tfar;
        hits.push_back(hit);
#ifdef IGL_VERBOSE
        std::cerr<<"  t: "<<hit.t<<endl;
#endif
        // Instead of moving origin, just change min_t. That way calculations
        // all use exactly same origin values
        min_t = ray.tfar;

        // reset t_scale
        self_hits = 0;
      }
      last_id0 = ray.primID;
    }
    else
      break; // no more hits

    if(hits.size()>1000 && !large_hits_warned)
    {
      std::cout<<"Warning: Large number of hits..."<<endl;
      std::cout<<"[ ";
      for(vector<Hit>::iterator hit = hits.begin(); hit != hits.end();hit++)
      {
        std::cout<<(hit->id+1)<<" ";
      }

      std::cout.precision(std::numeric_limits< double >::digits10);
      std::cout<<"[ ";

      for(vector<Hit>::iterator hit = hits.begin(); hit != hits.end(); hit++)
      {
        std::cout<<(hit->t)<<endl;;
      }

      std::cout<<"]"<<endl;
      large_hits_warned = true;

      return hits.empty();
    }
  }

  return hits.empty();
}

inline bool
igl::EmbreeIntersector
::intersectSegment(const Eigen::RowVector3f& a, const Eigen::RowVector3f& ab, Hit &hit, int mask) const
{
  RTCRay ray;
  createRay(ray,a,ab,0,1.0,mask);

  rtcIntersect(scene,ray);

  if(ray.geomID != RTC_INVALID_GEOMETRY_ID)
  {
    hit.id = ray.primID;
    hit.gid = ray.geomID;
    hit.u = ray.u;
    hit.v = ray.v;
    hit.t = ray.tfar;
    return true;
  }

  return false;
}

inline void
igl::EmbreeIntersector
::createRay(RTCRay& ray, const Eigen::RowVector3f& origin, const Eigen::RowVector3f& direction, float tnear, float tfar, int mask) const
{
  ray.org[0] = origin[0];
  ray.org[1] = origin[1];
  ray.org[2] = origin[2];
  ray.dir[0] = direction[0];
  ray.dir[1] = direction[1];
  ray.dir[2] = direction[2];
  ray.tnear = tnear;
  ray.tfar = tfar;
  ray.geomID = RTC_INVALID_GEOMETRY_ID;
  ray.primID = RTC_INVALID_GEOMETRY_ID;
  ray.instID = RTC_INVALID_GEOMETRY_ID;
  ray.mask = mask;
  ray.time = 0.0f;
}

#endif //EMBREE_INTERSECTOR_H