| 120 | } |
| 121 | |
| 122 | double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const |
| 123 | { |
| 124 | double xCenter = size_.x() / 2.0; |
| 125 | double yCenter = size_.y() / 2.0; |
| 126 | int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); |
| 127 | int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); |
| 128 | int k = std::round((position.z() - zIndexStartHeight_) / resolution_); |
| 129 | i = std::max(i, 0); |
| 130 | i = std::min(i, size_.x() - 1); |
| 131 | j = std::max(j, 0); |
| 132 | j = std::min(j, size_.y() - 1); |
| 133 | k = std::max(k, 0); |
| 134 | k = std::min(k, (int)data_.size() - 1); |
| 135 | Vector3 gradient = getDistanceGradientAt(position); |
| 136 | double xp = position_.x() + ((size_.x() - i) - xCenter) * resolution_; |
| 137 | double yp = position_.y() + ((size_.y() - j) - yCenter) * resolution_; |
| 138 | double zp = zIndexStartHeight_ + k * resolution_; |
| 139 | Vector3 error = position - Vector3(xp, yp, zp); |
| 140 | return data_[k](i, j) + gradient.dot(error); |
| 141 | } |
| 142 | |
| 143 | Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const |
| 144 | { |