Acts/Utilities/Frustum.ipp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/Utilities/Frustum.hpp"
0012
0013 #include "Acts/Utilities/VectorHelpers.hpp"
0014
0015 #include <numbers>
0016
0017 template <typename value_t, std::size_t DIM, std::size_t SIDES>
0018 Acts::Frustum<value_t, DIM, SIDES>::Frustum(const VertexType& origin,
0019                                             const VertexType& dir,
0020                                             value_type opening_angle)
0021   requires(DIM == 2)
0022     : m_origin(origin) {
0023   using rotation_t = Eigen::Rotation2D<value_type>;
0024
0025   static_assert(SIDES == 2, "2D frustum can only have 2 sides");
0026   assert(opening_angle < std::numbers::pi_v<value_type>);
0027
0028   translation_t translation(origin);
0029   value_type angle = VectorHelpers::phi(dir);
0030   Eigen::Rotation2D<value_type> rot(angle);
0031
0032   value_type normal_angle = std::numbers::pi / 2. - opening_angle / 2.;
0033   VertexType normal1 = rotation_t(normal_angle) * VertexType::UnitX();
0034   VertexType normal2 = rotation_t(-normal_angle) * VertexType::UnitX();
0035
0036   m_normals = {rot * VertexType::UnitX(), rot * normal1, rot * normal2};
0037 }
0038
0039 template <typename value_t, std::size_t DIM, std::size_t SIDES>
0040 Acts::Frustum<value_t, DIM, SIDES>::Frustum(const VertexType& origin,
0041                                             const VertexType& dir,
0042                                             value_type opening_angle)
0043   requires(DIM == 3)
0044     : m_origin(origin) {
0045   static_assert(SIDES > 2, "3D frustum must have 3 or more sides");
0046   assert(opening_angle < std::numbers::pi_v<value_type>);
0047   using angle_axis_t = Eigen::AngleAxis<value_type>;
0048
0049   const VertexType ldir = VertexType::UnitZ();
0050   const VertexType lup = VertexType::UnitX();
0051
0052   transform_type transform;
0053   transform = (Eigen::Quaternion<value_type>().setFromTwoVectors(ldir, dir));
0054
0055   m_normals[0] = ldir;
0056
0057   const value_type phi_sep = 2. * std::numbers::pi_v<value_type> / sides;
0058   transform_type rot;
0059   rot = angle_axis_t(phi_sep, ldir);
0060
0061   value_type half_opening_angle = opening_angle / 2.;
0062   auto calculate_normal =
0063       [&ldir, &half_opening_angle](const VertexType& out) -> VertexType {
0064     const VertexType tilt_axis = -1 * out.cross(ldir);
0065     return (-1 * (angle_axis_t(half_opening_angle, tilt_axis) * out))
0066         .normalized();
0067   };
0068
0069   VertexType current_outward = lup;
0070   m_normals[1] = calculate_normal(current_outward);
0071
0072   for (std::size_t i = 1; i < sides; i++) {
0073     current_outward = rot * current_outward;
0074     m_normals[i + 1] = calculate_normal(current_outward);
0075   }
0076
0077   for (auto& normal : m_normals) {
0078     normal = transform * normal;
0079   }
0080 }
0081
0082 template <typename value_t, std::size_t DIM, std::size_t SIDES>
0083 void Acts::Frustum<value_t, DIM, SIDES>::draw(IVisualization3D& helper,
0084                                               value_type far_distance) const
0085   requires(DIM == 3)
0086 {
0087   static_assert(DIM == 3, "Drawing is only supported in 3D");
0088
0089   // Iterate around normals, calculate cross with "far" plane
0090   // to get intersection lines.
0091   // Work in local reference frame of the frustum, and only convert to global
0092   // right before drawing.
0093   VertexType far_normal = m_normals[0];  // far has same normal as pseudo-near
0094   VertexType far_center = m_normals[0] * far_distance;
0095   std::array<std::pair<VertexType, VertexType>, SIDES> planeFarIXs;
0096
0097   auto ixPlanePlane = [](const auto& n1, const auto& p1, const auto& n2,
0098                          const auto& p2) -> std::pair<VertexType, VertexType> {
0099     const VertexType m = n1.cross(n2).normalized();
0100     const double j = (n2.dot(p2 - p1)) / (n2.dot(n1.cross(m)));
0101     const VertexType q = p1 + j * n1.cross(m);
0102     return {m, q};
0103   };
0104
0105   auto ixLineLine = [](const auto& p1, const auto& d1, const auto& p2,
0106                        const auto& d2) -> VertexType {
0107     return p1 + (((p2 - p1).cross(d2)).norm() / (d1.cross(d2)).norm()) * d1;
0108   };
0109
0110   // skip i=0 <=> pseudo-near
0111   for (std::size_t i = 1; i < n_normals; i++) {
0112     const auto ixLine =
0113         ixPlanePlane(far_normal, far_center, m_normals[i], VertexType::Zero());
0114     planeFarIXs.at(i - 1) = ixLine;
0115   }
0116
0117   std::array<VertexType, SIDES> points{};
0118
0119   for (std::size_t i = 0; i < std::size(planeFarIXs); i++) {
0120     std::size_t j = (i + 1) % std::size(planeFarIXs);
0121     const auto& l1 = planeFarIXs.at(i);
0122     const auto& l2 = planeFarIXs.at(j);
0123     const VertexType ix =
0124         m_origin + ixLineLine(l1.second, l1.first, l2.second, l2.first);
0125     points.at(i) = ix;
0126   }
0127
0128   for (std::size_t i = 0; i < std::size(points); i++) {
0129     std::size_t j = (i + 1) % std::size(points);
0130     helper.face(
0131         std::vector<VertexType>({m_origin, points.at(i), points.at(j)}));
0132   }
0133 }
0134
0135 template <typename value_t, std::size_t DIM, std::size_t SIDES>
0136 std::ostream& Acts::Frustum<value_t, DIM, SIDES>::svg(std::ostream& os,
0137                                                       value_type w,
0138                                                       value_type h,
0139                                                       value_type far_distance,
0140                                                       value_type unit) const
0141   requires(DIM == 2)
0142 {
0143   static_assert(DIM == 2, "SVG is only supported in 2D");
0144
0145   VertexType mid(w / 2., h / 2.);
0146
0147   // set up transform for svg. +y is down, normally, and unit is pixels.
0148   // We flip the y axis, and scale up by `unit`.
0149   transform_type trf = transform_type::Identity();
0150   trf.translate(mid);
0151   trf = trf * Eigen::Scaling(VertexType(1, -1));
0152   trf.scale(unit);
0153
0154   std::array<std::string, 3> colors({"orange", "blue", "red"});
0155
0156   auto draw_line = [&](const VertexType& left_, const VertexType& right_,
0157                        const std::string& color, std::size_t width) {
0158     VertexType left = trf * left_;
0159     VertexType right = trf * right_;
0160     os << "<line ";
0161
0162     os << "x1=\"" << left.x() << "\" ";
0163     os << "y1=\"" << left.y() << "\" ";
0164     os << "x2=\"" << right.x() << "\" ";
0165     os << "y2=\"" << right.y() << "\" ";
0166
0167     os << " stroke=\"" << color << "\" stroke-width=\"" << width << "\"/>\n";
0168   };
0169
0170   auto draw_point = [&](const VertexType& p_, const std::string& color,
0171                         std::size_t r) {
0172     VertexType p = trf * p_;
0173     os << "<circle ";
0174     os << "cx=\"" << p.x() << "\" cy=\"" << p.y() << "\" r=\"" << r << "\"";
0175     os << " fill=\"" << color << "\"";
0176     os << "/>\n";
0177   };
0178
0179   using vec3 = Eigen::Matrix<value_type, 3, 1>;
0180   auto ixLineLine = [](const VertexType& p1_2, const VertexType& d1_2,
0181                        const VertexType& p2_2,
0182                        const VertexType& d2_2) -> VertexType {
0183     const vec3 p1(p1_2.x(), p1_2.y(), 0);
0184     const vec3 p2(p2_2.x(), p2_2.y(), 0);
0185     const vec3 d1(d1_2.x(), d1_2.y(), 0);
0186     const vec3 d2(d2_2.x(), d2_2.y(), 0);
0187
0188     vec3 num = (p2 - p1).cross(d2);
0189     vec3 den = d1.cross(d2);
0190
0191     value_type f = 1.;
0192     value_type dot = num.normalized().dot(den.normalized());
0193     if (std::abs(dot) - 1 < 1e-9 && dot < 0) {
0194       f = -1.;
0195     }
0196
0197     const vec3 p = p1 + f * (num.norm() / den.norm()) * d1;
0198     assert(std::abs(p.z()) < 1e-9);
0199     return {p.x(), p.y()};
0200   };
0201
0202   const VertexType far_dir = {m_normals[0].y(), -m_normals[0].x()};
0203   const VertexType far_point = m_normals[0] * far_distance;
0204
0205   std::array<VertexType, 2> points{};
0206
0207   for (std::size_t i = 1; i < n_normals; i++) {
0208     VertexType plane_dir(m_normals[i].y(), -m_normals[i].x());
0209
0210     const VertexType ix = ixLineLine(far_point, far_dir, {0, 0}, plane_dir);
0211     draw_point(m_origin + ix, "black", 3);
0212     draw_line(m_origin, m_origin + ix, "black", 2);
0213     points.at(i - 1) = ix;
0214   }
0215
0216   draw_line(m_origin + points.at(0), m_origin + points.at(1), "black", 2);
0217
0218   draw_line(m_origin, m_origin + m_normals[0] * 2, colors[0], 3);
0219
0220   draw_point({0, 0}, "yellow", 5);
0221   draw_point(m_origin, "green", 5);
0222
0223   return os;
0224 }
0225
0226 template <typename value_t, std::size_t DIM, std::size_t SIDES>
0227 Acts::Frustum<value_t, DIM, SIDES>
0228 Acts::Frustum<value_t, DIM, SIDES>::transformed(
0229     const transform_type& trf) const {
0230   const auto& rot = trf.rotation();
0231
0232   std::array<VertexType, n_normals> new_normals;
0233   for (std::size_t i = 0; i < n_normals; i++) {
0234     new_normals[i] = rot * m_normals[i];
0235   }
0236
0237   return Frustum<value_t, DIM, SIDES>(trf * m_origin, std::move(new_normals));
0238 }