File indexing completed on 2025-01-18 09:11:09
0001
0002
0003
0004
0005
0006
0007
0008
0009 template <typename entity_t, typename value_t, std::size_t DIM>
0010 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::AxisAlignedBoundingBox(
0011 const entity_t* entity, const VertexType& vmin, const VertexType& vmax)
0012 : m_entity(entity),
0013 m_vmin(vmin),
0014 m_vmax(vmax),
0015 m_center((vmin + vmax) / 2.),
0016 m_width(vmax - vmin),
0017 m_iwidth(1 / m_width) {}
0018
0019 template <typename entity_t, typename value_t, std::size_t DIM>
0020 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::AxisAlignedBoundingBox(
0021 const entity_t* entity, const VertexType& center, const Size& size)
0022 : m_entity(entity),
0023 m_vmin(center - size.get() * 0.5),
0024 m_vmax(center + size.get() * 0.5),
0025 m_center(center),
0026 m_width(size.get()),
0027 m_iwidth(1 / m_width) {}
0028
0029 template <typename entity_t, typename value_t, std::size_t DIM>
0030 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::AxisAlignedBoundingBox(
0031 const std::vector<self_t*>& boxes, vertex_array_type envelope)
0032 : m_entity(nullptr) {
0033 assert(boxes.size() > 1);
0034
0035 for (std::size_t i = 0; i < boxes.size(); i++) {
0036 if (i < boxes.size() - 1) {
0037
0038 boxes[i]->setSkip(boxes[i + 1]);
0039 } else {
0040
0041
0042 boxes[i]->setSkip(nullptr);
0043 }
0044 }
0045
0046 m_left_child = boxes.front();
0047 m_right_child = boxes.back();
0048 m_skip = nullptr;
0049
0050 std::tie(m_vmin, m_vmax) = wrap(boxes, envelope);
0051
0052 m_center = (m_vmin + m_vmax) / 2.;
0053 m_width = m_vmax - m_vmin;
0054 m_iwidth = 1 / m_width;
0055 }
0056
0057 template <typename entity_t, typename value_t, std::size_t DIM>
0058 std::pair<
0059 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType,
0060 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType>
0061 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::wrap(
0062 const std::vector<const self_t*>& boxes, vertex_array_type envelope) {
0063 assert(boxes.size() > 1);
0064
0065
0066 vertex_array_type vmax(
0067 vertex_array_type::Constant(std::numeric_limits<value_type>::lowest()));
0068 vertex_array_type vmin(
0069 vertex_array_type::Constant(std::numeric_limits<value_type>::max()));
0070
0071 for (std::size_t i = 0; i < boxes.size(); i++) {
0072 vmin = vmin.min(boxes[i]->min().array());
0073 vmax = vmax.max(boxes[i]->max().array());
0074 }
0075
0076 vmax += envelope;
0077 vmin -= envelope;
0078
0079 return {vmin, vmax};
0080 }
0081
0082 template <typename entity_t, typename value_t, std::size_t DIM>
0083 std::pair<
0084 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType,
0085 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType>
0086 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::wrap(
0087 const std::vector<self_t*>& boxes, vertex_array_type envelope) {
0088 assert(boxes.size() > 1);
0089 std::vector<const self_t*> box_ptrs;
0090 box_ptrs.reserve(boxes.size());
0091 std::transform(boxes.begin(), boxes.end(), std::back_inserter(box_ptrs),
0092 [](const auto* box) { return box; });
0093 return wrap(box_ptrs, envelope);
0094 }
0095
0096 template <typename entity_t, typename value_t, std::size_t DIM>
0097 std::pair<
0098 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType,
0099 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType>
0100 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::wrap(
0101 const std::vector<self_t>& boxes, vertex_array_type envelope) {
0102 assert(boxes.size() > 1);
0103 std::vector<const self_t*> box_ptrs;
0104 box_ptrs.reserve(boxes.size());
0105 std::transform(boxes.begin(), boxes.end(), std::back_inserter(box_ptrs),
0106 [](auto& box) { return &box; });
0107 return wrap(box_ptrs, envelope);
0108 }
0109
0110 template <typename entity_t, typename value_t, std::size_t DIM>
0111 bool Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::intersect(
0112 const VertexType& point) const {
0113 vertex_array_type t = (point - m_vmin).array() * m_iwidth;
0114 return t.minCoeff() >= 0 && t.maxCoeff() < 1;
0115 }
0116
0117 template <typename entity_t, typename value_t, std::size_t DIM>
0118 bool Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::intersect(
0119 const Ray<value_type, DIM>& ray) const {
0120 const VertexType& origin = ray.origin();
0121 const vertex_array_type& idir = ray.idir();
0122
0123
0124
0125
0126 vertex_array_type t0s = (m_vmin - origin).array() * idir;
0127 vertex_array_type t1s = (m_vmax - origin).array() * idir;
0128
0129
0130
0131
0132
0133
0134 vertex_array_type tsmaller = t0s.min(t1s);
0135 vertex_array_type tbigger = t0s.max(t1s);
0136
0137
0138 value_type tmin = tsmaller.maxCoeff();
0139 value_type tmax = tbigger.minCoeff();
0140
0141
0142
0143 return tmin < tmax && tmax > 0.0;
0144 }
0145
0146 template <typename entity_t, typename value_t, std::size_t DIM>
0147 template <std::size_t sides>
0148 bool Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::intersect(
0149 const Frustum<value_type, DIM, sides>& fr) const {
0150 const auto& normals = fr.normals();
0151
0152
0153 const vertex_array_type fr_vmin = m_vmin - fr.origin();
0154 const vertex_array_type fr_vmax = m_vmax - fr.origin();
0155
0156
0157
0158
0159 VertexType p_vtx;
0160
0161
0162
0163 for (std::size_t i = 0; i < sides + 1; i++) {
0164 const VertexType& normal = normals[i];
0165
0166
0167
0168 p_vtx = (normal.array() < 0).template cast<value_type>() * fr_vmin +
0169 (normal.array() >= 0).template cast<value_type>() * fr_vmax;
0170
0171
0172
0173
0174 if (p_vtx.dot(normal) < 0) {
0175
0176 return false;
0177 }
0178 }
0179
0180
0181
0182 return true;
0183 }
0184
0185 template <typename entity_t, typename value_t, std::size_t DIM>
0186 void Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::setSkip(
0187 self_t* skip) {
0188
0189 m_skip = skip;
0190
0191 if (m_right_child != nullptr) {
0192 m_right_child->setSkip(skip);
0193 }
0194 }
0195
0196 template <typename entity_t, typename value_t, std::size_t DIM>
0197 const Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>*
0198 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::getLeftChild() const {
0199 return m_left_child;
0200 }
0201
0202 template <typename entity_t, typename value_t, std::size_t DIM>
0203 const Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>*
0204 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::getSkip() const {
0205 return m_skip;
0206 }
0207
0208 template <typename entity_t, typename value_t, std::size_t DIM>
0209 bool Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::hasEntity() const {
0210 return m_entity != nullptr;
0211 }
0212
0213 template <typename entity_t, typename value_t, std::size_t DIM>
0214 const entity_t* Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::entity()
0215 const {
0216 return m_entity;
0217 }
0218
0219 template <typename entity_t, typename value_t, std::size_t DIM>
0220 void Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::setEntity(
0221 const entity_t* entity) {
0222 m_entity = entity;
0223 }
0224
0225 template <typename entity_t, typename value_t, std::size_t DIM>
0226 const typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType&
0227 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::center() const {
0228 return m_center;
0229 }
0230
0231 template <typename entity_t, typename value_t, std::size_t DIM>
0232 const typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType&
0233 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::min() const {
0234 return m_vmin;
0235 }
0236
0237 template <typename entity_t, typename value_t, std::size_t DIM>
0238 const typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType&
0239 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::max() const {
0240 return m_vmax;
0241 }
0242
0243 template <typename entity_t, typename value_t, std::size_t DIM>
0244 std::ostream& Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::toStream(
0245 std::ostream& os) const {
0246 os << "AABB(ctr=(";
0247
0248 for (std::size_t i = 0; i < DIM; i++) {
0249 if (i > 0) {
0250 os << ", ";
0251 }
0252 os << m_center[i];
0253 }
0254
0255 os << ") vmin=(";
0256 for (std::size_t i = 0; i < DIM; i++) {
0257 if (i > 0) {
0258 os << ", ";
0259 }
0260 os << m_vmin[i];
0261 }
0262
0263 os << ") vmax=(";
0264
0265 for (std::size_t i = 0; i < DIM; i++) {
0266 if (i > 0) {
0267 os << ", ";
0268 }
0269 os << m_vmax[i];
0270 }
0271
0272 os << "))";
0273
0274 return os;
0275 }
0276
0277 template <typename entity_t, typename value_t, std::size_t DIM>
0278 std::pair<
0279 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType,
0280 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType>
0281 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::transformVertices(
0282 const transform_type& trf) const
0283 requires(DIM == 3)
0284 {
0285
0286
0287
0288 std::array<VertexType, 8> vertices({{
0289 {m_vmin.x(), m_vmin.y(), m_vmin.z()},
0290 {m_vmin.x(), m_vmax.y(), m_vmin.z()},
0291 {m_vmax.x(), m_vmax.y(), m_vmin.z()},
0292 {m_vmax.x(), m_vmin.y(), m_vmin.z()},
0293 {m_vmin.x(), m_vmin.y(), m_vmax.z()},
0294 {m_vmin.x(), m_vmax.y(), m_vmax.z()},
0295 {m_vmax.x(), m_vmax.y(), m_vmax.z()},
0296 {m_vmax.x(), m_vmin.y(), m_vmax.z()},
0297 }});
0298
0299 VertexType vmin = trf * vertices[0];
0300 VertexType vmax = trf * vertices[0];
0301
0302 for (std::size_t i = 1; i < 8; i++) {
0303 const VertexType vtx = trf * vertices[i];
0304 vmin = vmin.cwiseMin(vtx);
0305 vmax = vmax.cwiseMax(vtx);
0306 }
0307
0308 return {vmin, vmax};
0309 }
0310
0311 template <typename entity_t, typename value_t, std::size_t DIM>
0312 std::pair<
0313 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType,
0314 typename Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::VertexType>
0315 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::transformVertices(
0316 const transform_type& trf) const
0317 requires(DIM == 2)
0318 {
0319
0320
0321
0322 std::array<VertexType, 4> vertices({{{m_vmin.x(), m_vmin.y()},
0323 {m_vmin.x(), m_vmax.y()},
0324 {m_vmax.x(), m_vmax.y()},
0325 {m_vmax.x(), m_vmin.y()}}});
0326
0327 VertexType vmin = trf * vertices[0];
0328 VertexType vmax = trf * vertices[0];
0329
0330 for (std::size_t i = 1; i < 4; i++) {
0331 const VertexType vtx = trf * vertices[i];
0332 vmin = vmin.cwiseMin(vtx);
0333 vmax = vmax.cwiseMax(vtx);
0334 }
0335
0336 return {vmin, vmax};
0337 }
0338
0339 template <typename entity_t, typename value_t, std::size_t DIM>
0340 void Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::transform(
0341 const transform_type& trf) {
0342 std::tie(m_vmin, m_vmax) = transformVertices(trf);
0343 }
0344
0345 template <typename entity_t, typename value_t, std::size_t DIM>
0346 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>
0347 Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::transformed(
0348 const transform_type& trf) const {
0349 const auto [vmin, vmax] = transformVertices(trf);
0350 return self_t(m_entity, vmin, vmax);
0351 }
0352
0353 template <typename entity_t, typename value_t, std::size_t DIM>
0354 void Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::draw(
0355 IVisualization3D& helper, Color color, const transform_type& trf) const
0356 requires(DIM == 3)
0357 {
0358 static_assert(DIM == 3, "PLY output only supported in 3D");
0359
0360 const VertexType& vmin = m_vmin;
0361 const VertexType& vmax = m_vmax;
0362
0363 auto write = [&](const VertexType& a, const VertexType& b,
0364 const VertexType& c, const VertexType& d) {
0365 helper.face(std::vector<VertexType>({trf * a, trf * b, trf * c, trf * d}),
0366 color);
0367 };
0368
0369 write({vmin.x(), vmin.y(), vmin.z()}, {vmin.x(), vmax.y(), vmin.z()},
0370 {vmin.x(), vmax.y(), vmax.z()}, {vmin.x(), vmin.y(), vmax.z()});
0371
0372 write({vmax.x(), vmin.y(), vmin.z()}, {vmax.x(), vmax.y(), vmin.z()},
0373 {vmax.x(), vmax.y(), vmax.z()}, {vmax.x(), vmin.y(), vmax.z()});
0374
0375 write({vmin.x(), vmin.y(), vmin.z()}, {vmax.x(), vmin.y(), vmin.z()},
0376 {vmax.x(), vmin.y(), vmax.z()}, {vmin.x(), vmin.y(), vmax.z()});
0377
0378 write({vmin.x(), vmax.y(), vmin.z()}, {vmax.x(), vmax.y(), vmin.z()},
0379 {vmax.x(), vmax.y(), vmax.z()}, {vmin.x(), vmax.y(), vmax.z()});
0380
0381 write({vmin.x(), vmin.y(), vmin.z()}, {vmax.x(), vmin.y(), vmin.z()},
0382 {vmax.x(), vmax.y(), vmin.z()}, {vmin.x(), vmax.y(), vmin.z()});
0383
0384 write({vmin.x(), vmin.y(), vmax.z()}, {vmax.x(), vmin.y(), vmax.z()},
0385 {vmax.x(), vmax.y(), vmax.z()}, {vmin.x(), vmax.y(), vmax.z()});
0386 }
0387
0388 template <typename entity_t, typename value_t, std::size_t DIM>
0389 std::ostream& Acts::AxisAlignedBoundingBox<entity_t, value_t, DIM>::svg(
0390 std::ostream& os, value_type w, value_type h, value_type unit,
0391 const std::string& label, const std::string& fillcolor) const
0392 requires(DIM == 2)
0393 {
0394 static_assert(DIM == 2, "SVG is only supported in 2D");
0395
0396 VertexType mid(w / 2., h / 2.);
0397
0398 using transform_t = Eigen::Transform<value_t, DIM, Eigen::Affine>;
0399
0400 transform_t trf = transform_t::Identity();
0401 trf.translate(mid);
0402 trf = trf * Eigen::Scaling(VertexType(1, -1));
0403 trf.scale(unit);
0404
0405 auto draw_point = [&](const VertexType& p_, const std::string& color,
0406 std::size_t r) {
0407 VertexType p = trf * p_;
0408 os << "<circle ";
0409 os << "cx=\"" << p.x() << "\" cy=\"" << p.y() << "\" r=\"" << r << "\"";
0410 os << " fill=\"" << color << "\"";
0411 os << "/>\n";
0412 };
0413
0414 auto draw_rect = [&](const VertexType& center_, const VertexType& size_,
0415 const std::string& color) {
0416 VertexType size = size_ * unit;
0417 VertexType center = trf * center_ - size * 0.5;
0418
0419 os << "<rect ";
0420 os << "x=\"" << center.x() << "\" y=\"" << center.y() << "\" ";
0421 os << "width=\"" << size.x() << "\" height=\"" << size.y() << "\"";
0422 os << " fill=\"" << color << "\"";
0423 os << "/>\n";
0424 };
0425
0426 auto draw_text = [&](const VertexType& center_, const std::string& text,
0427 const std::string& color, std::size_t size) {
0428 VertexType center = trf * center_;
0429 os << "<text dominant-baseline=\"middle\" text-anchor=\"middle\" ";
0430 os << "fill=\"" << color << "\" font-size=\"" << size << "\" ";
0431 os << "x=\"" << center.x() << "\" y=\"" << center.y() << "\">";
0432 os << text << "</text>\n";
0433 };
0434
0435 draw_rect(m_center, m_width, fillcolor);
0436 draw_point(m_vmin, "black", 2);
0437 draw_point(m_vmax, "black", 2);
0438 draw_text(m_center, label, "white", 10);
0439
0440 return os;
0441 }
0442
0443 template <typename box_t>
0444 box_t* octree_inner(std::vector<std::unique_ptr<box_t>>& store,
0445 std::size_t max_depth,
0446 typename box_t::vertex_array_type envelope,
0447 const std::vector<box_t*>& lprims, std::size_t depth) {
0448 using VertexType = typename box_t::VertexType;
0449
0450 assert(!lprims.empty());
0451 if (lprims.size() == 1) {
0452
0453 return lprims.front();
0454 }
0455
0456 if (depth >= max_depth) {
0457
0458 auto bb = std::make_unique<box_t>(lprims, envelope);
0459 store.push_back(std::move(bb));
0460 return store.back().get();
0461 }
0462
0463 std::array<std::vector<box_t*>, 8> octants;
0464
0465 const auto [vmin, vmax] = box_t::wrap(lprims);
0466 VertexType glob_ctr = (vmin + vmax) / 2.;
0467
0468 for (auto* box : lprims) {
0469 VertexType ctr = box->center() - glob_ctr;
0470 if (ctr.x() < 0 && ctr.y() < 0 && ctr.z() < 0) {
0471 octants[0].push_back(box);
0472 continue;
0473 }
0474 if (ctr.x() > 0 && ctr.y() < 0 && ctr.z() < 0) {
0475 octants[1].push_back(box);
0476 continue;
0477 }
0478 if (ctr.x() < 0 && ctr.y() > 0 && ctr.z() < 0) {
0479 octants[2].push_back(box);
0480 continue;
0481 }
0482 if (ctr.x() > 0 && ctr.y() > 0 && ctr.z() < 0) {
0483 octants[3].push_back(box);
0484 continue;
0485 }
0486
0487 if (ctr.x() < 0 && ctr.y() < 0 && ctr.z() > 0) {
0488 octants[4].push_back(box);
0489 continue;
0490 }
0491 if (ctr.x() > 0 && ctr.y() < 0 && ctr.z() > 0) {
0492 octants[5].push_back(box);
0493 continue;
0494 }
0495 if (ctr.x() < 0 && ctr.y() > 0 && ctr.z() > 0) {
0496 octants[6].push_back(box);
0497 continue;
0498 }
0499 if (ctr.x() > 0 && ctr.y() > 0 && ctr.z() > 0) {
0500 octants[7].push_back(box);
0501 continue;
0502 }
0503
0504
0505 octants[0].push_back(box);
0506 }
0507
0508 std::vector<box_t*> sub_octs;
0509 for (const auto& sub_prims : octants) {
0510 if (sub_prims.size() <= 8) {
0511 if (sub_prims.empty()) {
0512
0513 } else if (sub_prims.size() == 1) {
0514 sub_octs.push_back(sub_prims.front());
0515 } else {
0516 store.push_back(std::make_unique<box_t>(sub_prims, envelope));
0517 sub_octs.push_back(store.back().get());
0518 }
0519 } else {
0520
0521 sub_octs.push_back(
0522 octree_inner(store, max_depth, envelope, sub_prims, depth + 1));
0523 }
0524 }
0525
0526 if (sub_octs.size() == 1) {
0527 return sub_octs.front();
0528 }
0529
0530 auto bb = std::make_unique<box_t>(sub_octs, envelope);
0531 store.push_back(std::move(bb));
0532 return store.back().get();
0533 }
0534
0535 template <typename box_t>
0536 box_t* Acts::make_octree(std::vector<std::unique_ptr<box_t>>& store,
0537 const std::vector<box_t*>& prims,
0538 std::size_t max_depth,
0539 typename box_t::value_type envelope1) {
0540 static_assert(box_t::dim == 3, "Octree can only be created in 3D");
0541
0542 using vertex_array_type = typename box_t::vertex_array_type;
0543
0544 vertex_array_type envelope(vertex_array_type::Constant(envelope1));
0545
0546 box_t* top = octree_inner(store, max_depth, envelope, prims, 0);
0547 return top;
0548 }
0549
0550 template <typename T, typename U, std::size_t V>
0551 std::ostream& Acts::operator<<(
0552 std::ostream& os, const Acts::AxisAlignedBoundingBox<T, U, V>& box) {
0553 return box.toStream(os);
0554 }