diff --git a/include/convex_bodies/simplexintersectball.h b/include/convex_bodies/simplexintersectball.h new file mode 100644 index 000000000..865e2534c --- /dev/null +++ b/include/convex_bodies/simplexintersectball.h @@ -0,0 +1,835 @@ +#ifndef SIMPLEXINTERSECTBALL_H +#define SIMPLEXINTERSECTBALL_H + +#include +#include +#include +#include +#include +#include + +/// This class represents the intersection of a simplex with the unit ball. +/// The simplex is given in H-representation: +/// A x <= b +/// and the ball is: +/// ||x - x0|| <= 1 +/// +/// Return convention for is_in: +/// -1 : inside +/// 0 : outside +/// +/// \tparam Point Point type used by volesti +/// \tparam MT_type Matrix type for A +template < + typename Point, + typename MT_type = Eigen::Matrix> +class SimplexIntersectBall +{ +public: + typedef Point PointType; + typedef typename Point::FT NT; + typedef MT_type MT; + typedef Eigen::Matrix VT; + + static NT pi() + { + return std::acos(NT(-1)); + } + +private: + unsigned int _d; // dimension + MT A; // matrix A + VT b; // vector b, such that A x <= b + MT V; // simplex vertices, stored column-wise + VT x0; // center of the unit ball + + bool compute_facet_roots(NT const& Ar_i, + NT const& Av_i, + NT const& b_i, + NT& C1, + NT& C2) const + { + NT const denom = + Ar_i * Ar_i + Av_i * Av_i; + + NT const D = + denom - b_i * b_i; + + if (D <= NT(0)) + { + return false; + } + + NT const radius = + std::sqrt(denom); + + NT const phase = + std::atan2(Av_i, Ar_i); + + NT const cosine = + std::max( + NT(-1), + std::min(NT(1), b_i / radius)); + + NT const offset = + std::acos(cosine); + + C1 = phase + offset; + C2 = phase - offset; + + return true; + } + +public: + SimplexIntersectBall() {} + + SimplexIntersectBall(unsigned int d_, + MT const &A_, + VT const &b_, + MT const &V_, + VT const &x0_) + : _d{d_}, A{A_}, b{b_}, V{V_}, x0{x0_} + { + } + + // Return dimension + unsigned int dimension() const + { + return _d; + } + + // Return number of facets / hyperplanes + int num_of_hyperplanes() const + { + return A.rows(); + } + + // Return matrix A + MT get_mat() const + { + return A; + } + + // Return vector b + VT get_vec() const + { + return b; + } + + // Return simplex vertices + MT get_vertices() const + { + return V; + } + + // Return center of the unit ball + VT get_center() const + { + return x0; + } + + // Change matrix A + void set_mat(MT const &A2) + { + A = A2; + } + + // Change vector b + void set_vec(VT const &b2) + { + b = b2; + } + + // Change simplex vertices + void set_vertices(MT const &V2) + { + V = V2; + } + + // Change ball center + void set_center(VT const &x02) + { + x0 = x02; + } + + // Check if Point p lies in the simplex-ball intersection: + // A p <= b + // ||p - x0|| <= 1 + int is_in(Point const &p, NT tol = NT(0)) const + { + VT p_vec = p.getCoefficients(); + + // Check ball condition + VT diff = p_vec - x0; + NT radius_tol = NT(1) + tol; + if (diff.squaredNorm() > radius_tol * radius_tol) + return 0; + + // Check simplex inequalities A p <= b + VT temp = b - A * p_vec; + const NT *Ax_b_data = temp.data(); + + for (int i = 0; i < A.rows(); i++) + { + if ((*Ax_b_data) < NT(-tol)) + { + return 0; + } + Ax_b_data++; + } + + return -1; + } + + int is_in_optimized(Point const &p, + VT &Ar, + VT &Av, + NT const &lambda_prev, + NT tol = NT(0)) const + { + VT p_vec = p.getCoefficients(); + + // Ball membership check. + VT diff = p_vec - x0; + NT radius_tol = NT(1) + tol; + if (diff.squaredNorm() > radius_tol * radius_tol) + return 0; + + // Update cached A*x along the great-circle rotation: + // x(lambda) = cos(lambda) * r + sin(lambda) * v. + // + // Here Ar stores A*r and Av stores A*v from the previous step. + Ar.noalias() = std::cos(lambda_prev) * Ar + std::sin(lambda_prev) * Av; + + VT temp = b - Ar; + const NT *Ax_b_data = temp.data(); + + for (int i = 0; i < A.rows(); i++) + { + if ((*Ax_b_data) < NT(-tol)) + return 0; + Ax_b_data++; + } + + return -1; + } + + // Compute intersection parameters of the line r + lambda * v + // with the simplex A x <= b. + // + // Returns: + // first = smallest positive lambda + // second = largest negative lambda + std::pair line_intersect(Point const &r, Point const &v) const + { + NT lambda = 0; + NT min_plus = std::numeric_limits::max(); + NT max_minus = std::numeric_limits::lowest(); + + VT sum_nom; + VT sum_denom; + + int m = num_of_hyperplanes(); + + sum_nom.noalias() = b - A * r.getCoefficients(); + sum_denom.noalias() = A * v.getCoefficients(); + + NT *sum_nom_data = sum_nom.data(); + NT *sum_denom_data = sum_denom.data(); + + for (int i = 0; i < m; i++) + { + if (*sum_denom_data != NT(0)) + { + lambda = *sum_nom_data / *sum_denom_data; + + if (lambda < min_plus && lambda > NT(0)) + { + min_plus = lambda; + } + + if (lambda > max_minus && lambda < NT(0)) + { + max_minus = lambda; + } + } + + sum_nom_data++; + sum_denom_data++; + } + + return std::make_pair(min_plus, max_minus); + } + + // Optimized version of line_intersect. + // It also computes and returns: + // Ar = A * r + // Av = A * v + // + // If pos = false: + // returns (min positive lambda, max negative lambda) + // + // If pos = true: + // returns (min positive lambda, facet index) + std::pair line_intersect(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + bool pos = false) const + { + NT lambda = 0; + NT min_plus = std::numeric_limits::max(); + NT max_minus = std::numeric_limits::lowest(); + + VT sum_nom; + int m = num_of_hyperplanes(); + int facet = -1; + + Ar.noalias() = A * r.getCoefficients(); + sum_nom.noalias() = b - Ar; + Av.noalias() = A * v.getCoefficients(); + + NT *Av_data = Av.data(); + NT *sum_nom_data = sum_nom.data(); + + for (int i = 0; i < m; i++) + { + if (*Av_data != NT(0)) + { + lambda = *sum_nom_data / *Av_data; + + if (lambda < min_plus && lambda > NT(0)) + { + min_plus = lambda; + if (pos) + { + facet = i; + } + } + else if (lambda > max_minus && lambda < NT(0)) + { + max_minus = lambda; + } + } + + Av_data++; + sum_nom_data++; + } + + if (pos) + { + return std::make_pair(min_plus, facet); + } + + return std::make_pair(min_plus, max_minus); + } + + // Optimized line_intersect version using the previous lambda. + // Ar is updated as: + // Ar <- Ar + lambda_prev * Av + // + // Then Av is recomputed as: + // Av <- A * v + std::pair line_intersect(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + NT const &lambda_prev, + bool pos = false) const + { + (void)r; + NT lambda = 0; + NT min_plus = std::numeric_limits::max(); + NT max_minus = std::numeric_limits::lowest(); + + VT sum_nom; + int m = num_of_hyperplanes(); + int facet = -1; + + Ar.noalias() += lambda_prev * Av; + sum_nom.noalias() = b - Ar; + Av.noalias() = A * v.getCoefficients(); + + NT *sum_nom_data = sum_nom.data(); + NT *Av_data = Av.data(); + + for (int i = 0; i < m; i++) + { + if (*Av_data != NT(0)) + { + lambda = *sum_nom_data / *Av_data; + + if (lambda < min_plus && lambda > NT(0)) + { + min_plus = lambda; + if (pos) + { + facet = i; + } + } + else if (lambda > max_minus && lambda < NT(0)) + { + max_minus = lambda; + } + } + + Av_data++; + sum_nom_data++; + } + + if (pos) + { + return std::make_pair(min_plus, facet); + } + + return std::make_pair(min_plus, max_minus); + } + + // Compute intersection angles of the great circle + // x(lambda) = cos(lambda) * r + sin(lambda) * v + // with the simplex boundary A x <= b. + // + // Input: + // Ar = A * r + // Av = A * v + // + // Returns: + // first = smallest positive angle lambda + // second = largest negative angle lambda + std::pair compute_intersections(VT &Ar, VT &Av) const + { + NT C1; + NT C2; + + NT max_root = std::numeric_limits::lowest(); + NT min_root = std::numeric_limits::max(); + + NT min_plus = std::numeric_limits::max(); + NT max_minus = std::numeric_limits::lowest(); + + int m = num_of_hyperplanes(); + + bool set_negative_root = false; + bool set_positive_root = false; + bool pos_D = false; + + NT *Av_data = Av.data(); + NT *Ar_data = Ar.data(); + const NT *b_data = b.data(); + + for (int i = 0; i < m; i++) + { + if (compute_facet_roots(*Ar_data, *Av_data, *b_data, C1, C2)) + { + pos_D = true; + + if (C1 < min_plus && C1 > NT(0)) + { + min_plus = C1; + set_positive_root = true; + } + else if (C1 > max_minus && C1 < NT(0)) + { + max_minus = C1; + set_negative_root = true; + } + + if (C1 > max_root && C1 < NT(2) * pi()) + { + max_root = C1; + } + + if ((C1 < min_root) && (C1 > (-NT(2) * pi()))) + { + min_root = C1; + } + + if (C2 < min_plus && C2 > NT(0)) + { + min_plus = C2; + set_positive_root = true; + } + else if (C2 > max_minus && C2 < NT(0)) + { + max_minus = C2; + set_negative_root = true; + } + + if (C2 > max_root && C2 < NT(2) * pi()) + { + max_root = C2; + } + + if (C2 < min_root && C2 > (-NT(2) * pi())) + { + min_root = C2; + } + } + + Av_data++; + Ar_data++; + b_data++; + } + + if (!set_negative_root) + { + if (pos_D) + { + max_minus = max_root - NT(2) * pi(); + } + else + { + max_minus = NT(0); + } + } + + if (!set_positive_root) + { + if (pos_D) + { + min_plus = min_root + NT(2) * pi(); + } + else + { + min_plus = NT(2) * pi(); + } + } + + return std::make_pair(min_plus, max_minus); + } + + // Great-circle intersection. + // Computes Ar = A*r and Av = A*v, then calls compute_intersections. + std::pair gc_intersect(Point const &r, + Point const &v, + VT &Ar, + VT &Av) const + { + Ar.noalias() = A * r.getCoefficients(); + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections(Ar, Av); + } + + // Optimized great-circle intersection using previous lambda. + std::pair gc_intersect(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + NT const &lambda_prev) const + { + (void)r; + + Ar.noalias() = std::cos(lambda_prev) * Ar + std::sin(lambda_prev) * Av; + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections(Ar, Av); + } + + // Great-circle intersection assuming Ar is already up to date. + std::pair gc_intersect_optimized(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + NT const &lambda_prev) const + { + (void)r; + (void)lambda_prev; + + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections(Ar, Av); + } + + // Compute the first positive intersection angle of the great circle + // x(lambda) = cos(lambda) * r + sin(lambda) * v + // with the simplex boundary A x <= b. + // + // Input: + // Ar = A * r + // Av = A * v + // + // Returns: + // first = smallest positive angle lambda + // second = index of the facet hit + std::pair compute_intersections_positive(VT const &Ar, VT const &Av) const + { + NT C1; + NT C2; + NT min_root = std::numeric_limits::max(); + NT min_plus = std::numeric_limits::max(); + + int m = num_of_hyperplanes(); + int facet = -1; + int facet_min = -1; + + bool set_positive_root = false; + bool pos_D = false; + + NT const root_tolerance = + NT(64) * std::numeric_limits::epsilon(); + + const NT *Av_data = Av.data(); + const NT *Ar_data = Ar.data(); + const NT *b_data = b.data(); + + for (int i = 0; i < m; i++) + { + NT const scale = + std::max( + NT(1), + std::max( + std::abs(*Ar_data), + std::max( + std::abs(*Av_data), + std::abs(*b_data)))); + + NT const equation_tolerance = + NT(128) * + std::numeric_limits::epsilon() * + scale; + + // If the current point lies on this facet and the tangent + // direction points outside, the collision is immediate. + if (std::abs(*Ar_data - *b_data) <= + equation_tolerance && + *Av_data > equation_tolerance) + { + return std::make_pair(NT(0), i); + } + + if (compute_facet_roots( + *Ar_data, + *Av_data, + *b_data, + C1, + C2)) + { + pos_D = true; + + if (C1 < min_plus && + C1 > root_tolerance) + { + min_plus = C1; + set_positive_root = true; + facet = i; + } + + if ((C1 < min_root) && + (C1 > (-NT(2) * pi()))) + { + min_root = C1; + facet_min = i; + } + + if (C2 < min_plus && + C2 > root_tolerance) + { + min_plus = C2; + set_positive_root = true; + facet = i; + } + + if (C2 < min_root && + C2 > (-NT(2) * pi())) + { + min_root = C2; + facet_min = i; + } + } + + Av_data++; + Ar_data++; + b_data++; + } + + if (!set_positive_root) + { + if (pos_D) + { + min_plus = min_root + NT(2) * pi(); + facet = facet_min; + } + else + { + min_plus = NT(2) * pi(); + } + } + + return std::make_pair(min_plus, facet); + } + + // Great-circle first positive intersection. + // Computes Ar = A*r and Av = A*v, then calls compute_intersections_positive. + std::pair gc_intersect_positive(Point const &r, + Point const &v, + VT &Ar, + VT &Av) const + { + Ar.noalias() = A * r.getCoefficients(); + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections_positive(Ar, Av); + } + + // Optimized great-circle first positive intersection using previous lambda. + std::pair gc_intersect_positive(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + NT const &lambda_prev) const + { + (void)r; + + Ar.noalias() = std::cos(lambda_prev) * Ar + std::sin(lambda_prev) * Av; + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections_positive(Ar, Av); + } + + // Compute all intersection roots of the great circle + // x(lambda) = cos(lambda) * r + sin(lambda) * v + // with the simplex boundary A x <= b. + // + // Returns: + // first = negative roots in (-pi, 0) + // second = positive roots in (0, pi) + std::pair compute_intersections_all_roots(VT &Ar, VT &Av) const + { + std::vector neg_roots; + std::vector pos_roots; + + int m = num_of_hyperplanes(); + + NT *Av_data = Av.data(); + NT *Ar_data = Ar.data(); + const NT *b_data = b.data(); + + for (int i = 0; i < m; i++) + { + NT C1; + NT C2; + + if (compute_facet_roots(*Ar_data, *Av_data, *b_data, C1, C2)) + { + if (C1 > pi()) + { + C1 -= NT(2) * pi(); + } + else if (C1 < -pi()) + { + C1 += NT(2) * pi(); + } + + if ((C1 > -pi()) && (C1 < NT(0))) + { + neg_roots.push_back(C1); + } + else if ((C1 < pi()) && (C1 > NT(0))) + { + pos_roots.push_back(C1); + } + + if (C2 > pi()) + { + C2 -= NT(2) * pi(); + } + else if (C2 < -pi()) + { + C2 += NT(2) * pi(); + } + + if ((C2 > -pi()) && (C2 < NT(0))) + { + neg_roots.push_back(C2); + } + else if ((C2 < pi()) && (C2 > NT(0))) + { + pos_roots.push_back(C2); + } + } + Av_data++; + Ar_data++; + b_data++; + } + + std::sort(neg_roots.begin(), neg_roots.end()); + std::sort(pos_roots.begin(), pos_roots.end()); + + VT neg_roots_vec(neg_roots.size()); + for (unsigned int i = 0; i < neg_roots.size(); i++) + { + neg_roots_vec(i) = neg_roots[i]; + } + + VT pos_roots_vec(pos_roots.size()); + for (unsigned int i = 0; i < pos_roots.size(); i++) + { + pos_roots_vec(i) = pos_roots[i]; + } + + return std::make_pair(neg_roots_vec, pos_roots_vec); + } + + // Great-circle all-roots intersection. + std::pair gc_intersect_all_roots(Point const &r, + Point const &v, + VT &Ar, + VT &Av) const + { + Ar.noalias() = A * r.getCoefficients(); + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections_all_roots(Ar, Av); + } + + // Optimized great-circle all-roots intersection using previous lambda. + std::pair gc_intersect_all_roots(Point const &r, + Point const &v, + VT &Ar, + VT &Av, + NT const &lambda_prev) const + { + (void)r; + + Ar.noalias() = std::cos(lambda_prev) * Ar + std::sin(lambda_prev) * Av; + Av.noalias() = A * v.getCoefficients(); + + return compute_intersections_all_roots(Ar, Av); + } + + // Apply linear transformation T to the simplex inequalities. + // If the point transformation is x <- T^{-1} x, + // then the H-representation matrix changes as A <- A * T. + void linear_transformIt(MT const &T) + { + A = A * T; + } + + // Shift the simplex by a vector c. + // For A x <= b, after shifting x <- x + c, + // the right-hand side changes as b <- b - A*c. + void shift(VT const &c) + { + b -= A * c; + } + + // Reflect tangent direction v at point p on the sphere + // against the facet indexed by facet. + // + // p_p is the tangent-space projector: + // p_p = I - p p^T + void compute_reflection(VT &v, + VT const &p, + MT const &p_p, + int const &facet) const + { + (void)p; + + VT u = p_p * A.row(facet).transpose(); + u *= (NT(1) / u.norm()); + + v += -NT(2) * v.dot(u) * u; + } +}; +#endif \ No newline at end of file diff --git a/include/convex_bodies/simplexintersectball_components.h b/include/convex_bodies/simplexintersectball_components.h new file mode 100644 index 000000000..b064cbaa1 --- /dev/null +++ b/include/convex_bodies/simplexintersectball_components.h @@ -0,0 +1,571 @@ +#ifndef SIMPLEXINTERSECTBALL_COMPONENTS_H +#define SIMPLEXINTERSECTBALL_COMPONENTS_H + +#include +#include +#include +#include +#include + +#include + +#undef Realloc +#undef Free +#include "lp_lib.h" + + +/// Solves ||point + t * direction - center||^2 = radius^2. +/// Returns false if the line does not intersect the sphere. +/// Precondition: direction.dot(direction) > tol. +template +bool solve_ball_line_roots( + Eigen::Matrix const& point, + Eigen::Matrix const& direction, + Eigen::Matrix const& center, + NT radius, + NT tol, + NT& tmin, + NT& tmax) +{ + Eigen::Matrix shifted = point - center; + + NT a = direction.dot(direction); + NT b = NT(2) * shifted.dot(direction); + NT c = shifted.dot(shifted) - radius * radius; + + NT discriminant = b * b - NT(4) * a * c; + + if (discriminant < -tol) + { + return false; + } + + if (discriminant < NT(0)) + { + discriminant = NT(0); + } + + NT sqrt_discriminant = std::sqrt(discriminant); + + tmin = (-b - sqrt_discriminant) / (NT(2) * a); + tmax = (-b + sqrt_discriminant) / (NT(2) * a); + + return true; +} + +/// Tests whether a segment intersects a Euclidean ball. +template +bool segment_intersects_ball( + Eigen::Matrix const& u, + Eigen::Matrix const& v, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + Eigen::Matrix direction = v - u; + + if (direction.dot(direction) <= tol) + { + return (u - center).squaredNorm() <= radius * radius + tol; + } + + NT tmin; + NT tmax; + + if (!solve_ball_line_roots(u, direction, center, radius, tol, tmin, tmax)) + { + return false; + } + + return (tmin >= -tol && tmin <= NT(1) + tol) || + (tmax >= -tol && tmax <= NT(1) + tol); +} + +/// Returns true if p lies strictly inside the ball B(center, radius). +template +bool point_is_inside_ball( + Eigen::Matrix const& p, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + return (p - center).squaredNorm() < radius * radius - tol; +} + +/// Returns a mask for simplex vertices that are outside the ball. +template +std::vector active_vertices_outside_ball( + Eigen::Matrix const& vertices, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + typedef Eigen::Matrix VT; + + int n = vertices.cols(); + std::vector active(n, 0); + + for (int i = 0; i < n; ++i) + { + VT vertex = vertices.col(i); + + if (!point_is_inside_ball(vertex, center, radius, tol)) + { + active[i] = 1; + } + } + + return active; +} + +/// Builds the graph used to identify connected components of the +/// simplex-sphere intersection. +template +Eigen::Matrix build_simplex_ball_graph( + Eigen::Matrix const& vertices, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + typedef Eigen::Matrix VT; + + int n = vertices.cols(); + + Eigen::Matrix adjacency = + Eigen::Matrix::Zero(n, n); + + std::vector active = + active_vertices_outside_ball(vertices, center, radius, tol); + + for (int i = 0; i < n; ++i) + { + if (!active[i]) + { + continue; + } + + VT vi = vertices.col(i); + + for (int j = i + 1; j < n; ++j) + { + if (!active[j]) + { + continue; + } + + VT vj = vertices.col(j); + + if (!segment_intersects_ball(vi, vj, center, radius, tol)) + { + adjacency(i, j) = 1; + adjacency(j, i) = 1; + } + } + } + + return adjacency; +} + +/// Finds connected components of adjacency restricted to active vertices. +inline std::vector> connected_components_from_graph( + Eigen::Matrix const& adjacency, + std::vector const& active) +{ + int n = adjacency.rows(); + + std::vector visited(n, 0); + std::vector> components; + + for (int start = 0; start < n; ++start) + { + if (visited[start] || !active[start]) + { + continue; + } + + std::vector component; + std::queue queue; + + visited[start] = 1; + queue.push(start); + + while (!queue.empty()) + { + int current = queue.front(); + queue.pop(); + + component.push_back(current); + + for (int next = 0; next < n; ++next) + { + if (!visited[next] && active[next] && adjacency(current, next) != 0) + { + visited[next] = 1; + queue.push(next); + } + } + } + + components.push_back(component); + } + + return components; +} + + +/// Finds connected components of the simplex-sphere intersection. +template +std::vector> find_simplex_ball_components( + Eigen::Matrix const& vertices, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + Eigen::Matrix adjacency = + build_simplex_ball_graph(vertices, center, radius, tol); + + std::vector active = + active_vertices_outside_ball(vertices, center, radius, tol); + + return connected_components_from_graph(adjacency, active); +} + +/// Intersects the ray from an interior point to a vertex with the sphere boundary. +template +std::pair> +ray_sphere_intersection_from_interior( + Eigen::Matrix const& interior_point, + Eigen::Matrix const& vertex, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + typedef Eigen::Matrix VT; + + VT empty = VT::Zero(center.rows()); + VT direction = vertex - interior_point; + + if (direction.dot(direction) <= tol) + { + return std::make_pair(false, empty); + } + + NT tmin; + NT tmax; + + if (!solve_ball_line_roots( + interior_point, direction, center, radius, tol, tmin, tmax)) + { + return std::make_pair(false, empty); + } + + NT t = tmin; + + if (t < -tol || t > NT(1) + tol) + { + t = tmax; + } + + if (t < -tol || t > NT(1) + tol) + { + return std::make_pair(false, empty); + } + + VT candidate = interior_point + t * direction; + + return std::make_pair(true, candidate); +} + +/// Computes an approximate Chebyshev center of {x : A x <= b} intersected +/// with B(center, radius), using cutting-plane linearization of the ball. +template +std::pair> +chebyshev_center_intersect_ball( + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& center, + NT radius = NT(1), + unsigned int max_iterations = 50, + NT tol = NT(1e-8)) +{ + typedef Eigen::Matrix VT; + + int m = A.rows(); + int d = A.cols(); + int ncols = d + 1; + + std::vector ball_cuts; + + for (int i = 0; i < d; ++i) + { + VT positive = VT::Zero(d); + positive(i) = NT(1); + ball_cuts.push_back(positive); + + VT negative = VT::Zero(d); + negative(i) = NT(-1); + ball_cuts.push_back(negative); + } + + VT solution = center; + + for (unsigned int iteration = 0; iteration < max_iterations; ++iteration) + { + lprec* lp = make_lp(0, ncols); + + if (lp == NULL) + { + return std::make_pair(false, solution); + } + + REAL infinite = get_infinite(lp); + + for (int j = 0; j < d; ++j) + { + set_bounds(lp, j + 1, -infinite, infinite); + } + + set_bounds(lp, d + 1, 0.0, infinite); + set_add_rowmode(lp, TRUE); + + std::vector colno(ncols); + std::vector row(ncols); + + for (int j = 0; j < ncols; ++j) + { + colno[j] = j + 1; + } + + for (int i = 0; i < m; ++i) + { + NT normal_norm = A.row(i).norm(); + + for (int j = 0; j < d; ++j) + { + row[j] = A(i, j); + } + + row[d] = normal_norm; + + if (!add_constraintex(lp, ncols, row.data(), colno.data(), LE, b(i))) + { + delete_lp(lp); + return std::make_pair(false, solution); + } + } + + for (VT const& cut : ball_cuts) + { + for (int j = 0; j < d; ++j) + { + row[j] = cut(j); + } + + row[d] = NT(1); + + NT rhs = radius + cut.dot(center); + + if (!add_constraintex(lp, ncols, row.data(), colno.data(), LE, rhs)) + { + delete_lp(lp); + return std::make_pair(false, solution); + } + } + + set_add_rowmode(lp, FALSE); + + for (int j = 0; j < d; ++j) + { + row[j] = NT(0); + } + + row[d] = NT(1); + + if (!set_obj_fnex(lp, ncols, row.data(), colno.data())) + { + delete_lp(lp); + return std::make_pair(false, solution); + } + + set_maxim(lp); + set_verbose(lp, NEUTRAL); + + if (solve(lp) != OPTIMAL) + { + delete_lp(lp); + return std::make_pair(false, solution); + } + + get_variables(lp, row.data()); + + for (int j = 0; j < d; ++j) + { + solution(j) = row[j]; + } + + NT inner_radius = row[d]; + + delete_lp(lp); + + VT shifted = solution - center; + NT distance = shifted.norm(); + + if (distance + inner_radius <= radius + tol) + { + return std::make_pair(true, solution); + } + + if (distance <= tol) + { + return std::make_pair(false, solution); + } + + ball_cuts.push_back(shifted / distance); + } + + return std::make_pair(false, solution); +} + +/// Returns true if p satisfies A * p <= b. +template +bool point_satisfies_halfspaces( + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& p, + NT tol = NT(1e-10)) +{ + for (int i = 0; i < A.rows(); ++i) + { + if (A.row(i).dot(p) > b(i) + tol) + { + return false; + } + } + + return true; +} + +/// Finds a starting point for one component by intersecting rays from an +/// interior point to the component vertices with the sphere boundary. +template +std::pair> +find_starting_point_for_component( + Eigen::Matrix const& vertices, + std::vector const& component, + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& interior_point, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + typedef Eigen::Matrix VT; + + for (int vertex_index : component) + { + VT vertex = vertices.col(vertex_index); + + std::pair intersection = + ray_sphere_intersection_from_interior( + interior_point, vertex, center, radius, tol); + + if (!intersection.first) + { + continue; + } + + VT candidate = intersection.second; + + if (point_satisfies_halfspaces(A, b, candidate, tol)) + { + return std::make_pair(true, candidate); + } + } + + VT empty(center.rows()); + empty.setZero(); + + return std::make_pair(false, empty); +} + +/// Finds one starting point for each connected component. +template +std::vector> +find_starting_points_for_components( + Eigen::Matrix const& vertices, + std::vector> const& components, + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& interior_point, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + typedef Eigen::Matrix VT; + + std::vector starting_points; + + for (std::vector const& component : components) + { + std::pair result = + find_starting_point_for_component( + vertices, component, A, b, interior_point, center, radius, tol); + + if (result.first) + { + starting_points.push_back(result.second); + } + } + + return starting_points; +} + +/// Finds connected components and corresponding starting points. +template +std::pair< + std::vector>, + std::vector>> +find_simplex_ball_components_and_starting_points( + Eigen::Matrix const& vertices, + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& interior_point, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + std::vector> components = + find_simplex_ball_components(vertices, center, radius, tol); + + std::vector> starting_points = + find_starting_points_for_components( + vertices, components, A, b, interior_point, center, radius, tol); + + return std::make_pair(components, starting_points); +} + +/// Finds connected components and starting points, computing an approximate +/// Chebyshev center of the simplex-ball intersection as the interior point. +template +std::pair< + std::vector>, + std::vector>> +find_simplex_ball_components_and_starting_points( + Eigen::Matrix const& vertices, + Eigen::Matrix const& A, + Eigen::Matrix const& b, + Eigen::Matrix const& center, + NT radius = NT(1), + NT tol = NT(1e-10)) +{ + std::pair> chebyshev_result = + chebyshev_center_intersect_ball(A, b, center, radius, 50, tol); + + Eigen::Matrix interior_point = + chebyshev_result.first ? chebyshev_result.second : center; + + return find_simplex_ball_components_and_starting_points( + vertices, A, b, interior_point, center, radius, tol); +} + +#endif \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1ac19b49f..0889c06cd 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -408,13 +408,41 @@ add_test(NAME full_dimensional_polytope_hypercube_intersection COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_hypercube_intersection) add_test(NAME full_dimensional_polytope_sparse_constraint COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_sparse_constraint) +add_executable (simplexintersectball_test simplexintersectball_test.cpp $) +add_test(NAME simplexintersectball_basic_membership + COMMAND simplexintersectball_test -tc=simplexintersectball_basic_membership) +add_test(NAME simplexintersectball_line_intersection + COMMAND simplexintersectball_test -tc=simplexintersectball_line_intersection) +add_test(NAME simplexintersectball_3d_tetrahedron_membership + COMMAND simplexintersectball_test -tc=simplexintersectball_3d_tetrahedron_membership) +add_test(NAME simplexintersectball_3d_tetrahedron_gc_intersection + COMMAND simplexintersectball_test -tc=simplexintersectball_3d_tetrahedron_gc_intersection) +add_test(NAME simplexintersectball_3d_tetrahedron_gc_intersection_positive + COMMAND simplexintersectball_test -tc=simplexintersectball_3d_tetrahedron_gc_intersection_positive) +add_test(NAME simplexintersectball_incremental_rotation_matches_fresh + COMMAND simplexintersectball_test -tc=simplexintersectball_incremental_rotation_matches_fresh) +add_test(NAME simplexintersectball_3d_tetrahedron_gc_all_roots + COMMAND simplexintersectball_test -tc=simplexintersectball_3d_tetrahedron_gc_all_roots) +add_test(NAME simplexintersectball_3d_tetrahedron_reflection + COMMAND simplexintersectball_test -tc=simplexintersectball_3d_tetrahedron_reflection) add_test(NAME full_dimensional_polytope_orthogonality COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_orthogonality) add_test(NAME full_dimensional_polytope_infeasible COMMAND full_dimensional_polytope_test -tc=full_dimensional_polytope_infeasible) - - - +add_executable (simplexintersectball_components_test simplexintersectball_components_test.cpp $) +add_test(NAME simplexball_components_segment_intersects_ball + COMMAND simplexintersectball_components_test -tc=simplexball_components_segment_intersects_ball) +add_test(NAME simplexball_components_tetrahedron_finds_two_components + COMMAND simplexintersectball_components_test -tc=simplexball_components_tetrahedron_finds_two_components) +add_test(NAME simplexball_components_tetrahedron_starting_points + COMMAND simplexintersectball_components_test -tc=simplexball_components_tetrahedron_starting_points) +add_test(NAME simplexball_components_tetrahedron_starting_points_with_chebyshev_center + COMMAND simplexintersectball_components_test -tc=simplexball_components_tetrahedron_starting_points_with_chebyshev_center) +add_test(NAME simplexball_components_tetrahedron_filters_interior_vertex + COMMAND simplexintersectball_components_test -tc=simplexball_components_tetrahedron_filters_interior_vertex) +add_test(NAME simplexball_components_chebyshev_center_intersect_ball + COMMAND simplexintersectball_components_test -tc=simplexball_components_chebyshev_center_intersect_ball) +TARGET_LINK_LIBRARIES(simplexintersectball_components_test lp_solve coverage_config) set(ADDITIONAL_FLAGS "-march=native -DSIMD_LEN=0 -DTIME_KEEPING") #set_target_properties(benchmarks_crhmc diff --git a/test/simplexintersectball_components_test.cpp b/test/simplexintersectball_components_test.cpp new file mode 100644 index 000000000..b081c64ca --- /dev/null +++ b/test/simplexintersectball_components_test.cpp @@ -0,0 +1,189 @@ +#include "doctest.h" + +#include + +#include "convex_bodies/simplexintersectball_components.h" + +typedef double NT; +typedef Eigen::Matrix VT; + +TEST_CASE("simplexball_components_segment_intersects_ball") +{ + VT center(3); + center << 0, 0, 0; + + VT u(3); + VT v(3); + + // Segment crosses the unit ball. + u << -2, 0, 0; + v << 2, 0, 0; + CHECK(segment_intersects_ball(u, v, center, NT(1))); + + // Segment stays outside the unit ball. + u << 2, 2, 0; + v << 3, 2, 0; + CHECK_FALSE(segment_intersects_ball(u, v, center, NT(1))); + + // Segment is tangent to the unit ball. + u << -1, 1, 0; + v << 1, 1, 0; + CHECK(segment_intersects_ball(u, v, center, NT(1))); +} + +TEST_CASE("simplexball_components_chebyshev_center_intersect_ball") +{ + VT center(2); + center << 0, 0; + + // Feasible set: {x <= 0.4} intersected with the unit ball. + // The largest inscribed ball is centered at (-0.3, 0) with radius 0.7. + Eigen::Matrix A(4, 2); + A << 1, 0, + -1, 0, + 0, 1, + 0, -1; + + VT b(4); + b << 0.4, 2.0, 2.0, 2.0; + + std::pair result = + chebyshev_center_intersect_ball(A, b, center, NT(1), 100, NT(1e-10)); + + REQUIRE(result.first); + + VT xc = result.second; + + CHECK(xc(0) == doctest::Approx(-0.3).epsilon(1e-5)); + CHECK(xc(1) == doctest::Approx(0.0).epsilon(1e-5)); + CHECK((A * xc - b).maxCoeff() <= doctest::Approx(0.0).epsilon(1e-8)); + CHECK((xc - center).norm() < 1.0); +} + +TEST_CASE("simplexball_components_tetrahedron_finds_two_components") +{ + VT center(3); + center << 0, 0, 0; + + // Non-degenerate tetrahedron in R^3. + // Vertices 0 and 1 form one component. + // Vertices 2 and 3 form the other component. + // Every edge between the two pairs intersects the unit ball. + Eigen::Matrix vertices(3, 4); + vertices << -2, -2, 2, 2, + -0.2, 0.2, -0.2, 0.2, + -0.2, 0.2, 0.2, -0.2; + + std::vector> components = + find_simplex_ball_components(vertices, center, NT(1)); + + REQUIRE(components.size() == 2); + + CHECK(components[0].size() == 2); + CHECK(components[0][0] == 0); + CHECK(components[0][1] == 1); + + CHECK(components[1].size() == 2); + CHECK(components[1][0] == 2); + CHECK(components[1][1] == 3); +} + +TEST_CASE("simplexball_components_tetrahedron_starting_points") +{ + VT center(3); + center << 0, 0, 0; + + Eigen::Matrix vertices(3, 4); + vertices << -2, -2, 2, 2, + -0.2, 0.2, -0.2, 0.2, + -0.2, 0.2, 0.2, -0.2; + + // H-representation of the tetrahedron above. + Eigen::Matrix A(4, 3); + A << 1, 10, 10, + 1, -10, -10, + -1, 10, -10, + -1, -10, 10; + + VT b(4); + b << 2, 2, 2, 2; + + VT interior_point(3); + interior_point << 0, 0, 0; + + std::pair>, std::vector> result = + find_simplex_ball_components_and_starting_points( + vertices, A, b, interior_point, center, NT(1)); + + REQUIRE(result.first.size() == 2); + REQUIRE(result.second.size() == 2); + + for (VT const& p : result.second) + { + CHECK(p.rows() == 3); + CHECK(p.norm() == doctest::Approx(1.0)); + CHECK(point_satisfies_halfspaces(A, b, p)); + } +} + +TEST_CASE("simplexball_components_tetrahedron_starting_points_with_chebyshev_center") +{ + VT center(3); + center << 0, 0, 0; + + Eigen::Matrix vertices(3, 4); + vertices << -2, -2, 2, 2, + -0.2, 0.2, -0.2, 0.2, + -0.2, 0.2, 0.2, -0.2; + + // H-representation of the tetrahedron above. + Eigen::Matrix A(4, 3); + A << 1, 10, 10, + 1, -10, -10, + -1, 10, -10, + -1, -10, 10; + + VT b(4); + b << 2, 2, 2, 2; + + std::pair>, std::vector> result = + find_simplex_ball_components_and_starting_points( + vertices, A, b, center, NT(1)); + + REQUIRE(result.first.size() == 2); + REQUIRE(result.second.size() == 2); + + for (VT const& p : result.second) + { + CHECK(p.rows() == 3); + CHECK(p.norm() == doctest::Approx(1.0)); + CHECK(point_satisfies_halfspaces(A, b, p)); + } +} + +TEST_CASE("simplexball_components_tetrahedron_filters_interior_vertex") +{ + VT center(3); + center << 0, 0, 0; + + // Non-degenerate tetrahedron with one vertex inside the unit ball. + // Vertex 3 is inside and should be ignored. + // Vertex 0 remains an isolated active component. + // Vertices 1 and 2 remain connected. + Eigen::Matrix vertices(3, 4); + vertices << -2, 2, 2, 0, + 0, 0, 0.5, 0.1, + 0, 0, 0.5, 0; + + std::vector> components = + find_simplex_ball_components(vertices, center, NT(1)); + + REQUIRE(components.size() == 2); + + CHECK(components[0].size() == 1); + CHECK(components[0][0] == 0); + + CHECK(components[1].size() == 2); + CHECK(components[1][0] == 1); + CHECK(components[1][1] == 2); +} \ No newline at end of file diff --git a/test/simplexintersectball_test.cpp b/test/simplexintersectball_test.cpp new file mode 100644 index 000000000..239ec7da5 --- /dev/null +++ b/test/simplexintersectball_test.cpp @@ -0,0 +1,295 @@ +#include "doctest.h" + +#include + +#include "convex_bodies/simplexintersectball.h" +#include "cartesian_geom/cartesian_kernel.h" + +typedef double NT; +typedef Eigen::Matrix MT; +typedef Eigen::Matrix VT; +typedef Cartesian Kernel; +typedef typename Kernel::Point Point; +typedef SimplexIntersectBall SimplexBall; + +SimplexBall make_3d_tetrahedron_unit_ball() +{ + unsigned int d = 3; + + // Tetrahedron in R^3: + // x >= 0, y >= 0, z >= 0, x + y + z <= 2 + MT A(4, 3); + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 1, 1; + + VT b(4); + b << 0, 0, 0, 2; + + // Vertices stored column-wise: + // (0,0,0), (2,0,0), (0,2,0), (0,0,2) + MT V(3, 4); + V << 0, 2, 0, 0, + 0, 0, 2, 0, + 0, 0, 0, 2; + + VT x0(3); + x0 << 0, 0, 0; + + return SimplexBall(d, A, b, V, x0); +} + +TEST_CASE("simplexintersectball_basic_membership") +{ + unsigned int d = 2; + + // Simplex in R^2: + // x >= 0, y >= 0, x + y <= 1 + MT A(3, 2); + A << -1, 0, + 0, -1, + 1, 1; + + VT b(3); + b << 0, 0, 1; + + // Vertices stored column-wise: (0,0), (1,0), (0,1) + MT V(2, 3); + V << 0, 1, 0, + 0, 0, 1; + + VT x0(2); + x0 << 0, 0; + + SimplexBall K(d, A, b, V, x0); + + CHECK(K.dimension() == 2); + CHECK(K.num_of_hyperplanes() == 3); + + VT inside_vec(2); + inside_vec << 0.2, 0.2; + Point inside(inside_vec); + + VT outside_simplex_vec(2); + outside_simplex_vec << 0.8, 0.8; + Point outside_simplex(outside_simplex_vec); + + VT outside_ball_vec(2); + outside_ball_vec << 1.2, 0.0; + Point outside_ball(outside_ball_vec); + + CHECK(K.is_in(inside) == -1); + CHECK(K.is_in(outside_simplex) == 0); + CHECK(K.is_in(outside_ball) == 0); +} + +TEST_CASE("simplexintersectball_line_intersection") +{ + unsigned int d = 2; + + MT A(3, 2); + A << -1, 0, + 0, -1, + 1, 1; + + VT b(3); + b << 0, 0, 1; + + MT V(2, 3); + V << 0, 1, 0, + 0, 0, 1; + + VT x0(2); + x0 << 0, 0; + + SimplexBall K(d, A, b, V, x0); + + VT r_vec(2); + r_vec << 0.2, 0.2; + Point r(r_vec); + + VT v_vec(2); + v_vec << 1.0, 0.0; + Point v(v_vec); + + std::pair interval = K.line_intersect(r, v); + + CHECK(interval.first == doctest::Approx(0.6)); + CHECK(interval.second == doctest::Approx(-0.2)); +} + +TEST_CASE("simplexintersectball_3d_tetrahedron_membership") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + CHECK(K.dimension() == 3); + CHECK(K.num_of_hyperplanes() == 4); + + VT inside_vec(3); + inside_vec << 0.3, 0.3, 0.3; + Point inside(inside_vec); + + VT outside_simplex_vec(3); + outside_simplex_vec << -0.1, 0.2, 0.2; + Point outside_simplex(outside_simplex_vec); + + VT outside_ball_vec(3); + outside_ball_vec << 1.2, 0.0, 0.0; + Point outside_ball(outside_ball_vec); + + CHECK(K.is_in(inside) == -1); + CHECK(K.is_in(outside_simplex) == 0); + CHECK(K.is_in(outside_ball) == 0); +} + +TEST_CASE("simplexintersectball_3d_tetrahedron_gc_intersection") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + NT inv_sqrt3 = NT(1) / std::sqrt(NT(3)); + NT inv_sqrt2 = NT(1) / std::sqrt(NT(2)); + + VT r_vec(3); + r_vec << inv_sqrt3, inv_sqrt3, inv_sqrt3; + Point r(r_vec); + + VT v_vec(3); + v_vec << inv_sqrt2, -inv_sqrt2, 0; + Point v(v_vec); + + VT Ar; + VT Av; + + std::pair interval = K.gc_intersect(r, v, Ar, Av); + + NT alpha = std::atan(std::sqrt(NT(2) / NT(3))); + + CHECK(interval.first == doctest::Approx(alpha)); + CHECK(interval.second == doctest::Approx(-alpha)); +} + +TEST_CASE("simplexintersectball_3d_tetrahedron_gc_intersection_positive") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + NT inv_sqrt3 = NT(1) / std::sqrt(NT(3)); + NT inv_sqrt2 = NT(1) / std::sqrt(NT(2)); + + VT r_vec(3); + r_vec << inv_sqrt3, inv_sqrt3, inv_sqrt3; + Point r(r_vec); + + VT v_vec(3); + v_vec << inv_sqrt2, -inv_sqrt2, 0; + Point v(v_vec); + + VT Ar; + VT Av; + + std::pair hit = K.gc_intersect_positive(r, v, Ar, Av); + + NT alpha = std::atan(std::sqrt(NT(2) / NT(3))); + + CHECK(hit.first == doctest::Approx(alpha)); + CHECK(hit.second == 1); +} + +TEST_CASE("simplexintersectball_incremental_rotation_matches_fresh") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + NT inv_sqrt3 = NT(1) / std::sqrt(NT(3)); + NT inv_sqrt2 = NT(1) / std::sqrt(NT(2)); + + VT r0(3); + r0 << inv_sqrt3, inv_sqrt3, inv_sqrt3; + VT v0(3); + v0 << inv_sqrt2, -inv_sqrt2, 0; + Point pr0(r0), pv0(v0); + + VT Ar, Av; + K.gc_intersect(pr0, pv0, Ar, Av); + + NT lambda = 0.3; + std::pair inc = K.gc_intersect(pr0, pv0, Ar, Av, lambda); + + VT r1 = std::cos(lambda) * r0 + std::sin(lambda) * v0; + Point pr1(r1); + VT Ar2, Av2; + std::pair fresh = K.gc_intersect(pr1, pv0, Ar2, Av2); + + CHECK(inc.first == doctest::Approx(fresh.first)); + CHECK(inc.second == doctest::Approx(fresh.second)); +} + +TEST_CASE("simplexintersectball_3d_tetrahedron_reflection") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + NT inv_sqrt2 = NT(1) / std::sqrt(NT(2)); + // Point on the unit sphere and on the facet y = 0 + VT p(3); + p << inv_sqrt2, 0, inv_sqrt2; + + // Tangent direction pointing outside through y < 0 + VT v(3); + v << 0, -1, 0; + + MT projector = MT::Identity(3, 3) - p * p.transpose(); + + int facet = 1; // y = 0 facet, represented by -y <= 0 + + K.compute_reflection(v, p, projector, facet); + + CHECK(v(0) == doctest::Approx(0.0)); + CHECK(v(1) == doctest::Approx(1.0)); + CHECK(v(2) == doctest::Approx(0.0)); +} + +TEST_CASE("simplexintersectball_3d_tetrahedron_gc_all_roots") +{ + SimplexBall K = make_3d_tetrahedron_unit_ball(); + + NT inv_sqrt3 = NT(1) / std::sqrt(NT(3)); + NT inv_sqrt2 = NT(1) / std::sqrt(NT(2)); + + VT r_vec(3); + r_vec << inv_sqrt3, inv_sqrt3, inv_sqrt3; + Point r(r_vec); + + VT v_vec(3); + v_vec << inv_sqrt2, -inv_sqrt2, 0; + Point v(v_vec); + + VT Ar; + VT Av; + + std::pair roots = K.gc_intersect_all_roots(r, v, Ar, Av); + + NT alpha = std::atan(std::sqrt(NT(2) / NT(3))); + + CHECK(roots.first.rows() >= 1); + CHECK(roots.second.rows() >= 1); + + bool found_negative_alpha = false; + for (int i = 0; i < roots.first.rows(); ++i) + { + if (std::abs(roots.first(i) + alpha) < NT(1e-08)) + { + found_negative_alpha = true; + } + } + + bool found_positive_alpha = false; + for (int i = 0; i < roots.second.rows(); ++i) + { + if (std::abs(roots.second(i) - alpha) < NT(1e-08)) + { + found_positive_alpha = true; + } + } + + CHECK(found_negative_alpha); + CHECK(found_positive_alpha); +} \ No newline at end of file