Gridmap3dLookup.hpp

Go to the documentation of this file.

/*
 * Gridmap3dLookup.h
 *
 *  Created on: Aug 13, 2020
 *      Author: Ruben Grandia
 *   Institute: ETH Zurich
 */

#pragma once

#include <grid_map_core/TypeDefs.hpp>

namespace grid_map {
namespace signed_distance_field {

struct Gridmap3dLookup {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  struct size_t_3d {
    size_t x{0};
    size_t y{0};
    size_t z{0};

    size_t_3d() = default;
    size_t_3d(size_t x, size_t y, size_t z) : x(x), y(y), z(z) {}
  };

  size_t_3d gridsize_{0, 0, 0};

  Position3 gridOrigin_{0.0, 0.0, 0.0};

  Position3 gridMaxIndexAsDouble_{0.0, 0.0, 0.0};

  double resolution_{1.0};

  Gridmap3dLookup() = default;

  Gridmap3dLookup(const size_t_3d& gridsize, const Position3& gridOrigin, double resolution)
      : gridsize_(gridsize),
        gridOrigin_(gridOrigin),
        gridMaxIndexAsDouble_(static_cast<double>(gridsize_.x - 1), static_cast<double>(gridsize_.y - 1),
                              static_cast<double>(gridsize_.z - 1)),
        resolution_(resolution) {
    assert(resolution_ > 0.0);
    assert(gridsize_.x > 0);
    assert(gridsize_.y > 0);
    assert(gridsize_.z > 0);
  };

  size_t_3d nearestNode(const Position3& position) const noexcept {
    const double resInv{1.0 / resolution_};
    Position3 subpixelVector{(gridOrigin_.x() - position.x()) * resInv, (gridOrigin_.y() - position.y()) * resInv,
                             (position.z() - gridOrigin_.z()) * resInv};
    return {getNearestPositiveInteger(subpixelVector.x(), gridMaxIndexAsDouble_.x()),
            getNearestPositiveInteger(subpixelVector.y(), gridMaxIndexAsDouble_.y()),
            getNearestPositiveInteger(subpixelVector.z(), gridMaxIndexAsDouble_.z())};
  }

  Position3 nodePosition(const size_t_3d& index) const noexcept {
    assert(index.x < gridsize_.x);
    assert(index.y < gridsize_.y);
    assert(index.z < gridsize_.z);
    return {gridOrigin_.x() - index.x * resolution_, gridOrigin_.y() - index.y * resolution_, gridOrigin_.z() + index.z * resolution_};
  }

  size_t linearIndex(const size_t_3d& index) const noexcept {
    assert(index.x < gridsize_.x);
    assert(index.y < gridsize_.y);
    assert(index.z < gridsize_.z);
    return (index.z * gridsize_.y + index.y) * gridsize_.x + index.x;
  }

  size_t linearSize() const noexcept { return gridsize_.x * gridsize_.y * gridsize_.z; }

  static size_t getNearestPositiveInteger(double val, double max) noexcept {
    // Comparing bounds as double prevents underflow/overflow of size_t
    return static_cast<size_t>(std::max(0.0, std::min(std::round(val), max)));
  }
};

}  // namespace signed_distance_field
}  // namespace grid_map