Skip to content

Commit

Permalink
Merge branch 'main' into refactor-cylinder-bounds-inside-early-exit
Browse files Browse the repository at this point in the history
  • Loading branch information
@kodiakhq
kodiakhq[bot] committed May 23, 2024
2 parents 3e44ad1 + cb9af28 commit 2bdfbfd
Show file tree
Hide file tree
Showing 20 changed files with 632 additions and 278 deletions.
130 changes: 8 additions & 122 deletions Core/include/Acts/Surfaces/BoundaryCheck.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#pragma once

#include "Acts/Definitions/Algebra.hpp"
#include "Acts/Definitions/TrackParametrization.hpp"
#include "Acts/Surfaces/detail/VerticesHelper.hpp"
Expand Down Expand Up @@ -138,11 +139,6 @@ class BoundaryCheck {
Vector2 computeClosestPointOnPolygon(const Vector2& point,
const Vector2Container& vertices) const;

/// Calculate the closest point on the box
Vector2 computeEuclideanClosestPointOnRectangle(
const Vector2& point, const Vector2& lowerLeft,
const Vector2& upperRight) const;

/// metric weight matrix: identity for absolute mode or inverse covariance
SquareMatrix2 m_weight;

Expand Down Expand Up @@ -246,7 +242,8 @@ inline bool Acts::BoundaryCheck::isInside(const Vector2& point,
if (m_type == Type::eNone || m_type == Type::eAbsolute) {
// absolute, can calculate directly
closestPoint =
computeEuclideanClosestPointOnRectangle(point, lowerLeft, upperRight);
detail::VerticesHelper::computeEuclideanClosestPointOnRectangle(
point, lowerLeft, upperRight);

} else /* Type::eChi2 */ {
// need to calculate by projection and squarednorm
Expand Down Expand Up @@ -275,8 +272,9 @@ inline double Acts::BoundaryCheck::distance(const Acts::Vector2& point,
const Vector2& upperRight) const {
if (m_type == Type::eNone || m_type == Type::eAbsolute) {
// compute closest point on box
double d = (point - computeEuclideanClosestPointOnRectangle(
point, lowerLeft, upperRight))
double d = (point -
detail::VerticesHelper::computeEuclideanClosestPointOnRectangle(
point, lowerLeft, upperRight))
.norm();
return detail::VerticesHelper::isInsideRectangle(point, lowerLeft,
upperRight)
Expand Down Expand Up @@ -311,118 +309,6 @@ inline double Acts::BoundaryCheck::squaredNorm(const Vector2& x) const {
template <typename Vector2Container>
inline Acts::Vector2 Acts::BoundaryCheck::computeClosestPointOnPolygon(
const Acts::Vector2& point, const Vector2Container& vertices) const {
// calculate the closest position on the segment between `ll0` and `ll1` to
// the point as measured by the metric induced by the weight matrix
auto closestOnSegment = [&](auto&& ll0, auto&& ll1) {
// normal vector and position of the closest point along the normal
auto n = ll1 - ll0;
auto weighted_n = m_weight * n;
auto f = n.dot(weighted_n);
auto u = std::isnormal(f)
? (point - ll0).dot(weighted_n) / f
: 0.5; // ll0 and ll1 are so close it doesn't matter
// u must be in [0, 1] to still be on the polygon segment
return ll0 + std::clamp(u, 0.0, 1.0) * n;
};

auto iv = std::begin(vertices);
Vector2 l0 = *iv;
Vector2 l1 = *(++iv);
Vector2 closest = closestOnSegment(l0, l1);
auto closestDist = squaredNorm(closest - point);
// Calculate the closest point on other connecting lines and compare distances
for (++iv; iv != std::end(vertices); ++iv) {
l0 = l1;
l1 = *iv;
Vector2 current = closestOnSegment(l0, l1);
auto currentDist = squaredNorm(current - point);
if (currentDist < closestDist) {
closest = current;
closestDist = currentDist;
}
}
// final edge from last vertex back to the first vertex
Vector2 last = closestOnSegment(l1, *std::begin(vertices));
if (squaredNorm(last - point) < closestDist) {
closest = last;
}
return closest;
}

inline Acts::Vector2
Acts::BoundaryCheck::computeEuclideanClosestPointOnRectangle(
const Vector2& point, const Vector2& lowerLeft,
const Vector2& upperRight) const {
/*
*
* | |
* IV | V | I
* | |
* ------------------------------
* | |
* | |
* VIII | INSIDE | VI
* | |
* | |
* ------------------------------
* | |
* III | VII | II
* | |
*
*/

double l0 = point[0], l1 = point[1];
double loc0Min = lowerLeft[0], loc0Max = upperRight[0];
double loc1Min = lowerLeft[1], loc1Max = upperRight[1];

// check if inside
if (loc0Min <= l0 && l0 < loc0Max && loc1Min <= l1 && l1 < loc1Max) {
// INSIDE
double dist = std::abs(loc0Max - l0);
Vector2 cls(loc0Max, l1);

double test = std::abs(loc0Min - l0);
if (test <= dist) {
dist = test;
cls = {loc0Min, l1};
}

test = std::abs(loc1Max - l1);
if (test <= dist) {
dist = test;
cls = {l0, loc1Max};
}

test = std::abs(loc1Min - l1);
if (test <= dist) {
return {l0, loc1Min};
}
return cls;
} else {
// OUTSIDE, check sectors
if (l0 > loc0Max) {
if (l1 > loc1Max) { // I
return {loc0Max, loc1Max};
} else if (l1 <= loc1Min) { // II
return {loc0Max, loc1Min};
} else { // VI
return {loc0Max, l1};
}
} else if (l0 < loc0Min) {
if (l1 > loc1Max) { // IV
return {loc0Min, loc1Max};
} else if (l1 <= loc1Min) { // III
return {loc0Min, loc1Min};
} else { // VIII
return {loc0Min, l1};
}
} else {
if (l1 > loc1Max) { // V
return {l0, loc1Max};
} else { // l1 <= loc1Min # VII
return {l0, loc1Min};
}
// third case not necessary, see INSIDE above
}
}
return detail::VerticesHelper::computeClosestPointOnPolygon(point, vertices,
m_weight);
}
124 changes: 124 additions & 0 deletions Core/include/Acts/Surfaces/detail/VerticesHelper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,4 +168,128 @@ bool isInsideRectangle(const vertex_t& point, const vertex_t& lowerLeft,
bool onHyperPlane(const std::vector<Vector3>& vertices,
ActsScalar tolerance = s_onSurfaceTolerance);

/// Calculate the closest point on the polygon.
template <typename Vector2Container>
inline Vector2 computeClosestPointOnPolygon(const Vector2& point,
const Vector2Container& vertices,
const SquareMatrix2& metric) {
auto squaredNorm = [&](const Vector2& x) {
return (x.transpose() * metric * x).value();
};

// calculate the closest position on the segment between `ll0` and `ll1` to
// the point as measured by the metric induced by the metric matrix
auto closestOnSegment = [&](auto&& ll0, auto&& ll1) {
// normal vector and position of the closest point along the normal
auto n = ll1 - ll0;
auto n_transformed = metric * n;
auto f = n.dot(n_transformed);
auto u = std::isnormal(f)
? (point - ll0).dot(n_transformed) / f
: 0.5; // ll0 and ll1 are so close it doesn't matter
// u must be in [0, 1] to still be on the polygon segment
return ll0 + std::clamp(u, 0.0, 1.0) * n;
};

auto iv = std::begin(vertices);
Vector2 l0 = *iv;
Vector2 l1 = *(++iv);
Vector2 closest = closestOnSegment(l0, l1);
auto closestDist = squaredNorm(closest - point);
// Calculate the closest point on other connecting lines and compare distances
for (++iv; iv != std::end(vertices); ++iv) {
l0 = l1;
l1 = *iv;
Vector2 current = closestOnSegment(l0, l1);
auto currentDist = squaredNorm(current - point);
if (currentDist < closestDist) {
closest = current;
closestDist = currentDist;
}
}
// final edge from last vertex back to the first vertex
Vector2 last = closestOnSegment(l1, *std::begin(vertices));
if (squaredNorm(last - point) < closestDist) {
closest = last;
}
return closest;
}

/// Calculate the closest point on the box
inline Vector2 computeEuclideanClosestPointOnRectangle(
const Vector2& point, const Vector2& lowerLeft, const Vector2& upperRight) {
/*
*
* | |
* IV | V | I
* | |
* ------------------------------
* | |
* | |
* VIII | INSIDE | VI
* | |
* | |
* ------------------------------
* | |
* III | VII | II
* | |
*
*/

double l0 = point[0], l1 = point[1];
double loc0Min = lowerLeft[0], loc0Max = upperRight[0];
double loc1Min = lowerLeft[1], loc1Max = upperRight[1];

// check if inside
if (loc0Min <= l0 && l0 < loc0Max && loc1Min <= l1 && l1 < loc1Max) {
// INSIDE
double dist = std::abs(loc0Max - l0);
Vector2 cls(loc0Max, l1);

double test = std::abs(loc0Min - l0);
if (test <= dist) {
dist = test;
cls = {loc0Min, l1};
}

test = std::abs(loc1Max - l1);
if (test <= dist) {
dist = test;
cls = {l0, loc1Max};
}

test = std::abs(loc1Min - l1);
if (test <= dist) {
return {l0, loc1Min};
}
return cls;
} else {
// OUTSIDE, check sectors
if (l0 > loc0Max) {
if (l1 > loc1Max) { // I
return {loc0Max, loc1Max};
} else if (l1 <= loc1Min) { // II
return {loc0Max, loc1Min};
} else { // VI
return {loc0Max, l1};
}
} else if (l0 < loc0Min) {
if (l1 > loc1Max) { // IV
return {loc0Min, loc1Max};
} else if (l1 <= loc1Min) { // III
return {loc0Min, loc1Min};
} else { // VIII
return {loc0Min, l1};
}
} else {
if (l1 > loc1Max) { // V
return {l0, loc1Max};
} else { // l1 <= loc1Min # VII
return {l0, loc1Min};
}
// third case not necessary, see INSIDE above
}
}
}

} // namespace Acts::detail::VerticesHelper
Loading

0 comments on commit 2bdfbfd

Please sign in to comment.