| 104 | } |
| 105 | |
| 106 | double SignedDistanceField::getDistanceAt(const Position3& position) const |
| 107 | { |
| 108 | double xCenter = size_.x() / 2.0; |
| 109 | double yCenter = size_.y() / 2.0; |
| 110 | int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); |
| 111 | int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); |
| 112 | int k = std::round((position.z() - zIndexStartHeight_) / resolution_); |
| 113 | i = std::max(i, 0); |
| 114 | i = std::min(i, size_.x() - 1); |
| 115 | j = std::max(j, 0); |
| 116 | j = std::min(j, size_.y() - 1); |
| 117 | k = std::max(k, 0); |
| 118 | k = std::min(k, (int)data_.size() - 1); |
| 119 | return data_[k](i, j); |
| 120 | } |
| 121 | |
| 122 | double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const |
| 123 | { |