uLib/src/Math/VoxRaytracer.cpp

/*//////////////////////////////////////////////////////////////////////////////
// CMT Cosmic Muon Tomography project //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

  Copyright (c) 2014, Universita' degli Studi di Padova, INFN sez. di Padova
  All rights reserved

  Authors: Andrea Rigoni Garola < andrea.rigoni@pd.infn.it >

  ------------------------------------------------------------------
  This library is free software;  you  can  redistribute  it  and/or
  modify it  under the  terms  of  the  GNU  Lesser  General  Public
  License as published  by  the  Free  Software  Foundation;  either
  version 3.0 of the License, or (at your option) any later version.

  This library is  distributed in  the hope that it will  be useful,
  but  WITHOUT ANY WARRANTY;  without  even  the implied warranty of
  MERCHANTABILITY  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of  the GNU Lesser General  Public
  License along with this library.

//////////////////////////////////////////////////////////////////////////////*/

#include <iostream>

#include "Utils.h"
#include "VoxRaytracer.h"

#define unlikely(expr) __builtin_expect(!!(expr), 0)

inline float fast_sign(float f) { return 1 - 2 * (f < 0); }

namespace uLib {

////////////////////////////////////////////////////////////////////////////////
///// RAY DATA /////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

void VoxRaytracer::RayData::AddElement(Id_t id, float L) {
  if (m_Count >= m_Data.size()) {
    size_t new_size = m_Data.size() == 0 ? 128 : m_Data.size() * 2;
    m_Data.resize(new_size);
  }
  Element el = {id, L};
  m_Data[m_Count] = el;
  m_Count++;
  m_TotalLength += L;
}

void VoxRaytracer::RayData::AppendRay(const VoxRaytracer::RayData &in) {
  if (unlikely(in.m_Count == 0)) {
    std::cout << "Warinig: PoCA on exit border!\n";
    return;
  } else if (unlikely(m_Count == 0)) {
    m_Data.resize(in.m_Count);
    for (size_t i = 0; i < in.m_Count; ++i) {
      m_Data[i] = in.m_Data[i];
    }
    m_Count = in.m_Count;
    m_TotalLength = in.m_TotalLength;
    std::cout << "Warinig: PoCA on entrance border!\n";
    return;
  } else {
    // Opzione 1) un voxel in piu' //
    if (in.m_Count > 0) {
      if (m_Count + in.m_Count > m_Data.size()) {
        m_Data.resize(m_Count + in.m_Count);
      }
      for (size_t i = 0; i < in.m_Count; ++i) {
        m_Data[m_Count + i] = in.m_Data[i];
      }
      m_Count += in.m_Count;
    }
    m_TotalLength += in.m_TotalLength;
  }
}

void VoxRaytracer::RayData::PrintSelf(std::ostream &o) {
  o << "Ray: total lenght " << m_TotalLength << "\n";
  for (size_t i = 0; i < m_Count; ++i)
    o << "[ " << m_Data[i].vox_id << ", " << m_Data[i].L << "] \n";
}

////////////////////////////////////////////////////////////////////////////////
//// RAY TRACER ////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

bool VoxRaytracer::GetEntryPoint(const HLine3f &line, HPoint3f &pt) {
  Vector4f s = m_Image->GetLocalPoint(line.direction);
  pt = m_Image->GetLocalPoint(line.origin);

  // Considers Structured grid dimensions //
  Vector4f dims = m_Image->GetDims().homogeneous().cast<float>();
  pt = pt.cwiseQuotient(dims);
  s = s.cwiseQuotient(dims);

  float l = s.head(3).norm();
  Vector3f L(l / s(0), l / s(1), l / s(2));

  Vector3f offset;
  for (int i = 0; i < 3; ++i)
    offset(i) = (s(i) > 0) - (pt(i) - floor(pt(i)));
  offset = offset.cwiseProduct(L).cwiseAbs();

  int id;
  float d;
  for (int loop = 0; loop < 8; loop++) {
    int check_border = 0;
    for (int i = 0; i < 3; ++i) {
      check_border += pt(i) > 1;
      check_border += pt(i) < 0;
    }
    if (check_border == 0) {
      for (int i = 0; i < 3; ++i)
        pt(i) *= (float)dims(i);
      pt = m_Image->GetWorldPoint(pt);
      return true;
    }

    d = offset.minCoeff(&id);
    for (int i = 0; i < 3; ++i)
      pt(i) += d / L(i);

    pt(id) = rintf(pt(id));

    offset.array() -= d;
    offset(id) = fabs(L(id));
  }
  for (int i = 0; i < 3; ++i)
    pt(i) *= (float)dims(i);
  pt = m_Image->GetWorldPoint(pt);
  return false;
}

bool VoxRaytracer::GetExitPoint(const HLine3f &line, HPoint3f &pt) {
  HLine3f out = line;
  out.direction *= -1;
  return GetEntryPoint(out, pt);
}

VoxRaytracer::RayData
VoxRaytracer::TraceBetweenPoints(const HPoint3f &in,
                                 const HPoint3f &out) const {
  RayData ray;

  // get the local points and the direction vector
  // local to image means in the normalized voxel space where the size
  // of the voxel is 1 in all dimensions
  Vector4f pt1 = m_Image->GetLocalPoint(in);
  Vector4f pt2 = m_Image->GetLocalPoint(out);
  Vector4f s = pt2 - pt1;

  // l is the total length of the ray in normalized voxel space
  float l = s.head(3).norm();

  // L is the length of the ray between two grid lines in grid
  Vector3f L(l / s(0), l / s(1), l / s(2));

  // Vector3f scale; // TODO: FIX Scaling
  // scale << (m_Image->GetWorldMatrix() * Vector4f(1,0,0,0)).norm(),
  //          (m_Image->GetWorldMatrix() * Vector4f(0,1,0,0)).norm(),
  //          (m_Image->GetWorldMatrix() * Vector4f(0,0,1,0)).norm();

  Vector3f offset;
  for (int i = 0; i < 3; ++i)
    offset(i) = (s(i) >= 0) - (pt1(i) - floor(pt1(i)));
  offset = offset.cwiseProduct(L).cwiseAbs();
  L = L.cwiseAbs();

  //---- Check if the ray only crosses one voxel
  Vector3i vid = m_Image->Find(in);
  if (vid == m_Image->Find(out)) {
    ray.AddElement(m_Image->Map(vid), s.norm());
    return ray;
  }

  //---- Otherwise, loop until ray is finished
  int id;
  float d;
  while (l > 0) {

    // find which is the minimum of the offsets to the next grid line
    // it will be also the actual normalized voxel ray length
    d = offset.minCoeff(&id);

    // see if the voxel is inside the grid (we are still inside image)
    if (m_Image->IsInsideGrid(vid)) {
      // add the voxel to the ray with mapping id and length scaled
      ray.AddElement(m_Image->Map(vid), d * m_scale(id));
    }

    // move to the next voxel
    vid(id) += (int)fast_sign(s(id));

    // update the remaining length
    l -= d;

    // update the offsets
    offset.array() -= d;
    offset(id) = fmin(L(id), l);
  }
  return ray;
}

// 20150528 SV for absorbed muons
VoxRaytracer::RayData VoxRaytracer::TraceLine(const HLine3f &line) const {
  RayData ray;

  Vector4f pt = m_Image->GetLocalPoint(line.origin);
  Vector4f s = m_Image->GetLocalPoint(line.direction);

  float l = s.head(3).norm();
  // intersection between track and grid when spacing is +1
  Vector3f L(l / s(0), l / s(1), l / s(2));

  // RayTracer works with a grid of interspace +1
  // Vector3f scale; // FIXXX
  // scale << (m_Image->GetWorldMatrix() * Vector4f(1,0,0,0)).norm(),
  //          (m_Image->GetWorldMatrix() * Vector4f(0,1,0,0)).norm(),
  //          (m_Image->GetWorldMatrix() * Vector4f(0,0,1,0)).norm();

  // offset is the fraction of the segment between grid lines when origin is
  // insiede voxel cwiseAbs for having positive distances
  Vector3f offset;
  for (int i = 0; i < 3; ++i)
    offset(i) = (s(i) >= 0) - (pt(i) - floor(pt(i)));
  offset = offset.cwiseProduct(L).cwiseAbs();
  L = L.cwiseAbs();

  int id;
  float d;
  Vector3i vid = m_Image->Find(line.origin);
  while (m_Image->IsInsideGrid(vid)) {
    // minimun coefficient of offset: id is the coordinate, d is the value
    // dependig on which grid line horizontal or vertical it is first intercept
    d = offset.minCoeff(&id);

    // add Lij to ray
    ray.AddElement(m_Image->Map(vid), d * m_scale(id));

    // move to the next voxel
    vid(id) += (int)fast_sign(s(id));

    offset.array() -= d;
    offset(id) = L(id);
  }
  return ray;
}

} // namespace uLib