CgPointCloud.cpp

#include "CgPointCloud.h"
#include "CgUtils/Timer.h"

// squared distance between a point and a ray
float sqrPointRayDist(glm::vec3 point, glm::vec3 origin, glm::vec3 direction) {
  glm::vec3 delta = point - origin;
  float t = std::max(0.0f, glm::dot(direction, delta) / glm::length2(direction));
  return glm::length2(direction * t - delta);
}

CgPointCloud::CgPointCloud(): CgPointCloud::CgPointCloud(51) {}

CgPointCloud::CgPointCloud(int id):
m_type(Cg::PointCloud),
m_id(id) {
    m_vertices.push_back(glm::vec3(0.0,0.0,0.0));
    m_vertex_normals.push_back(glm::vec3(0.0,0.0,1.0));
    m_vertex_colors.push_back(glm::vec3(0.0,0.0,1.0));
    calculateSplatOrientations();
}


CgPointCloud::~CgPointCloud() {
    m_vertices.clear();
    m_vertex_normals.clear();
    m_vertex_colors.clear();
    m_splat_indices.clear();
}


void CgPointCloud::calculateSplatOrientations() {
  // calculate local coordinate system for splats (arbitrary orientation of ellipse in plane)
  // replace this if you have the real coordinate system, use up vector = y-Axis of your local coordinate system instead of getPerpendicularVector(...)

  m_splat_orientations.clear();
  m_splat_scaling.clear();
  m_splat_indices.clear();

  for(unsigned int i=0;i<m_vertices.size();i++)
    {
      glm::mat4 lookAt_matrix(glm::lookAt(glm::vec3(m_vertices[i]),glm::vec3(m_vertices[i]-m_vertex_normals[i]),getPerpendicularVector(m_vertex_normals[i])));
      m_splat_orientations.push_back(lookAt_matrix);
      m_splat_scaling.push_back(glm::vec2(0.02,0.005));

      // use all points for splatting by default
      m_splat_indices.push_back(i);
    }


}

void CgPointCloud::init( std::string filename, bool cheat_normals) {
    m_vertices.clear();
    m_vertex_normals.clear();
    m_vertex_colors.clear();
    m_splat_orientations.clear();
    m_splat_scaling.clear();
    m_splat_indices.clear();

    // load obj File
    ObjLoader loader;
    loader.load(filename);
    loader.getPositionData(m_vertices);
    m_centroid = getCentroid(m_vertices);

    buildKdTree(&m_vertices.front(), &(*(m_vertices.end() - 1)), 0);
    calculateAABB();
    // run_timed(1, [&]() {estimateNormals();});
    estimateNormals();
    std::cout << "got " << m_vertices.size() << " points" << std::endl;

    // calculate local coordinate system for splats 
    // (arbitrary orientation of ellipse in plane)
    // replace this if you have the real coordinate 
    // system, use up vector = y-Axis of your local 
    // coordinate system instead of getPerpendicularVector(...)
    //calculateSplatOrientations();

    //add a standard color for each point if lighting turned off
    for(unsigned int i = 0;i < m_vertices.size(); i++) {
      m_vertex_colors.push_back(glm::vec3(0.0,1.0,0.0));
    }

    unsigned int k = 50;
    // "fast" if k is relatively small, if k is on the order 
    // of m_vertices.size(), this is slower than brute force.
    std::vector<unsigned int> neighbors = getNearestNeighborsFast(0, k);
    setColors(neighbors, glm::vec3(0.0, 0.0, 1.0));
}

void CgPointCloud::buildKdTree(glm::vec3* begin, glm::vec3* end, int depth) {
    auto half_size = (end - begin) / 2;

    if(begin == end || half_size == 0) {
        // zero or one element, nothing to do 
        return;
    } else { 
        // more than one element!

        // partially sort and ensure median element 
        // is in the middle (runs in O(n))
        // about 4x faster on tyra.obj than std::sort
        std::nth_element(begin, begin + half_size, end, [=](glm::vec3 a, glm::vec3 b) {
            return a[depth % 3] < b[depth % 3];
        });

        // split array in two (excluding median) and recurse
        buildKdTree(begin, begin + half_size, depth + 1);
        buildKdTree(begin + half_size + 1, end, depth + 1);
    }
}

void CgPointCloud::estimateNormals() {
  unsigned int neighbourhood_size = 25;

  // find point with highest y-value
  //std::vector<unsigned int> spanning_tree;
  //spanning_tree.resize(m_vertices.size());
  m_vertex_normals.clear();
  m_vertex_normals.resize(m_vertices.size());
  //float max_y =-std::numeric_limits<float>::infinity();
  //unsigned int max_y_index = 0;
  //for(int i = 0; i < m_vertices.size(); i++) {
  //  float curr = m_vertices[i].y;
  //  if(max_y < curr) {
  //    max_y = curr;
  //    max_y_index = i;
  //  }
  //}

  // the extremal point is selected as root (has no parent)
  //spanning_tree[max_y_index] = max_y_index;

  // build minimal spanning tree (for each index, record the index we came from)
  // traverse the tree, search neighborhoods
  for(unsigned int i = 0; i < m_vertices.size(); i++) {
      //std::cout << "alive, now " << i << std::endl;
      std::vector<unsigned int> nh_indices = getNearestNeighborsFast(i, neighbourhood_size);
      std::vector<glm::vec3> nh;
      nh.resize(neighbourhood_size);
      for(unsigned int j = 0; j < neighbourhood_size; j++) {
        nh[j] = m_vertices[nh_indices[j]];
      }
      glm::mat3 cov_mat = getCovarianceMatrix(nh);
      CgEigenDecomposition3x3 eigen(cov_mat);
      // The eigenvalues/vectors are sorted in increasing order, and we want the smallest one.
      glm::vec3 normal = eigen.getEigenvectors()[0];
      m_vertex_normals[i] = glm::dot(normal, m_vertices[i] - m_centroid) > 0.0 
        ? normal
        : -normal;
  }

  // select normal pointing roughly in the same direction as parent's normal
}

void CgPointCloud::traverseBFO(std::function<void(glm::vec3*, const unsigned int)> fn) {
    // queue of pairs of vec3 pointers
    // (range of points in the current node and its children)
    std::queue<std::pair<glm::vec3*, glm::vec3*>> q;
    if(m_vertices.empty()) return;
    unsigned int depth = 0;
    q.push({&m_vertices.front(), &(*(m_vertices.end() - 1))});
    while(!q.empty()) {
        int level_count = q.size();
        for(int i = 0; i < level_count; i++) {
            auto curr = q.front();
            glm::vec3* begin = curr.first;
            glm::vec3* end = curr.second;
            unsigned int half_size = (end - begin) / 2;
            glm::vec3* curr_pointer = begin + half_size;
            fn(curr_pointer, depth);
            // push left & right half of array to reconstruct splits
            if(half_size != 0) q.push({begin, begin + half_size});
            if(begin + half_size != end) q.push({begin + half_size + 1, end});
            q.pop();
        }
        depth += 1;
    }
}

std::vector<unsigned int> CgPointCloud::getNearestNeighborsSlow(unsigned int current_point, unsigned int k) {

  glm::vec3 q = m_vertices[current_point];

  std::vector<std::pair<double,int>> distances;

  for(unsigned int i = 0; i < m_vertices.size(); i++) {
    double dist = glm::distance(m_vertices[i],q);
    distances.push_back(std::make_pair(dist,i));
  }

    std::sort(distances.begin(), distances.end());

    std::vector<unsigned int> erg;

  for(unsigned int i = 0; i < k; i++) {
       erg.push_back(distances[i].second);
  }

  return erg;
}

std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int current_point, unsigned int k) {

  std::vector<unsigned int> erg(k);
  if(k == 0 || m_vertices.size() < k) return erg;
  // pair of distance + index into backing vec3 array 
  using P = std::pair<double, unsigned int>;

  glm::vec3 query = m_vertices[current_point];
  glm::vec3* first = &m_vertices.front();
  glm::vec3* last = &(*(m_vertices.end() - 1));
  auto cmp = [&](const P &left, const P &right) { return (left.first < right.first);};

  // using an ordered set for simplicitly, this may be slow
  // because we frequently erase elements, causing the rb-tree
  // to be rebuilt.
  std::vector<P> candidates;
  candidates.reserve(k + 1);

  // stack of traversed nodes (start index, end index, aabb, depth)
  std::vector<std::tuple<glm::vec3*, glm::vec3*, CgAABB, unsigned int>> nodes;
  nodes.push_back({first, last, m_aabb, 0});
  
  // traverse the tree as long as there's something to do.
  while(!nodes.empty()) {
    // unpack current node & pop it
    glm::vec3* begin;
    glm::vec3* end;
    CgAABB aabb;
    unsigned int depth;
    std::tie(begin, end, aabb, depth) = nodes.back();
    auto half_size = (end - begin) / 2;
    nodes.pop_back();

    glm::vec3 curr_point = *(begin + half_size);
    // insert current point into set
    candidates.push_back({glm::distance2(query, curr_point), begin + half_size - first});
    std::push_heap(candidates.begin(), candidates.end(), cmp);
    if(candidates.size() > k) {
      std::pop_heap(candidates.begin(), candidates.end(), cmp);
      candidates.pop_back();
    }

    // the aabb of the sub-nodes
    unsigned int axis = depth % 3;
    CgAABB lower_aabb;
    CgAABB higher_aabb;
    std::tie(lower_aabb, higher_aabb) = aabb.split(axis, curr_point[axis]);
    // only push lower child if its aabb intersects with the 
    // neighbourhood we found 
    double neighbourhood_radius = (*candidates.begin()).first;
    if(
      begin != begin + half_size
      && lower_aabb.position[axis] + lower_aabb.extent[axis] >= query[axis] - neighbourhood_radius
    ) {
      nodes.push_back({begin, begin + half_size, lower_aabb, depth + 1});
    }
    // only push higher if it intersects with
    // the neighbourhood we found.
    if(
      begin + half_size != end
      && higher_aabb.position[axis] - higher_aabb.extent[axis] <= query[axis] + neighbourhood_radius
    ) {
      nodes.push_back({begin + half_size + 1, end, higher_aabb, depth + 1});
    }
  }

  // copy all nearest neighbour indices we found into
  // a vector to return
  for(unsigned int i = 0; i < k; i++) erg[i] = candidates[i].second;

  return erg;
}

int CgPointCloud::getClosestPointToRay(glm::vec3 origin, glm::vec3 direction) {
  if(m_vertices.size() == 0) return -1;

  glm::vec3 inv_direction = glm::vec3(1.0 / direction.x, 1.0 / direction.y, 1.0 / direction.z);
  glm::vec3* first = &m_vertices.front();
  glm::vec3* last = &(*(m_vertices.end() - 1));
  // sensible starting point
  float erg_sqr_dist = std::numeric_limits<float>::infinity();
  int erg = -1;

  // stack of traversed nodes (start index, end index, aabb, depth)
  std::vector<std::tuple<glm::vec3*, glm::vec3*, CgAABB, unsigned int>> nodes;
  nodes.push_back({first, last, m_aabb, 0});
  
  // traverse the tree as long as there's something to do.
  while(!nodes.empty()) {
    // unpack current node & pop it
    glm::vec3* begin;
    glm::vec3* end;
    CgAABB aabb;
    unsigned int depth;
    std::tie(begin, end, aabb, depth) = nodes.back();
    auto half_size = (end - begin) / 2;
    nodes.pop_back();

    glm::vec3 curr_point = *(begin + half_size);
    // insert current point into set if it's closer to ray than last one
    float cand_dist = sqrPointRayDist(curr_point, origin, direction);
    if(cand_dist < erg_sqr_dist) {
      erg_sqr_dist = cand_dist;
      erg = begin + half_size - first;
    }

    // the aabb of the sub-nodes
    unsigned int axis = depth % 3;
    CgAABB lower_aabb;
    CgAABB higher_aabb;
    std::tie(lower_aabb, higher_aabb) = aabb.split(axis, curr_point[axis]);
    // only push children if their aabb is closer to ray than 
    // the closest point we found until now and there are
    // points in them
    if (
      begin != begin + half_size
      && lower_aabb.sqrDistanceToRay(origin, direction, inv_direction) < erg_sqr_dist
    ) {
      nodes.push_back({begin, begin + half_size, lower_aabb, depth + 1});
    }
    if (
      end != begin + half_size
      && higher_aabb.sqrDistanceToRay(origin, direction, inv_direction) < erg_sqr_dist
    ) {
      nodes.push_back({begin + half_size + 1, end, higher_aabb, depth + 1});
    }
  }

  return erg;
}

void CgPointCloud::setColors(std::vector<unsigned int> indices, glm::vec3 color) {
  for(unsigned int i = 0; i < indices.size(); i++) m_vertex_colors[indices[i]] = color;
}

void CgPointCloud::resetColors(glm::vec3 color) {
  for(unsigned int i = 0; i < m_vertices.size(); i++) m_vertex_colors[i] = color;
}

// calculates an arbitrary verctor perpendicular to the given one
glm::vec3 CgPointCloud::getPerpendicularVector(glm::vec3 arg) {
  if((arg.x == 0.0) && (arg.y == 0.0)) {
    if(arg.z == 0.0) return glm::vec3(0.);
    return glm::vec3(0.0, 1.0, 0.0);
  }
  return glm::normalize(glm::vec3(-arg.y, arg.x, 0.0));
}

const glm::vec3 CgPointCloud::getCenter() const {
  return m_centroid;
}

void CgPointCloud::calculateAABB() {
  double max_x = -std::numeric_limits<double>::infinity();
  double max_y = -std::numeric_limits<double>::infinity();
  double max_z = -std::numeric_limits<double>::infinity();
  double min_x = std::numeric_limits<double>::infinity();
  double min_y = std::numeric_limits<double>::infinity();
  double min_z = std::numeric_limits<double>::infinity();

  for(unsigned int i=0; i < m_vertices.size(); i++) {
    glm::vec3 curr = m_vertices[i];
    max_x = max_x < curr[0] ? curr[0] : max_x;
    min_x = min_x > curr[0] ? curr[0] : min_x;
    max_y = max_y < curr[1] ? curr[1] : max_y;
    min_y = min_y > curr[1] ? curr[1] : min_y;
    max_z = max_z < curr[2] ? curr[2] : max_z;
    min_z = min_z > curr[2] ? curr[2] : min_z;
  }

  m_aabb.position = glm::vec3((max_x + min_x) * 0.5, (max_y + min_y) * 0.5, (max_z + min_z) * 0.5);
  m_aabb.extent = glm::vec3((max_x - min_x) * 0.5, (max_y - min_y) * 0.5, (max_z - min_z) * 0.5);
}

// returns the point clouds AABB
const CgAABB CgPointCloud::getAABB() const {
  return m_aabb;
}

const std::vector<glm::vec2>& CgPointCloud::getSplatScalings() const {
  return m_splat_scaling;
}