I have a snippet of code that I don't manage to understand.
To give some context: I have a line (defined by its angular coefficient m
) and I search over a set of segments to find the intersecting one. A segment and a vector of segments are defined as:
typedef std::pair<Eigen::Vector2d, Eigen::Vector2d> Vector2dPair;
typedef std::vector<Vector2dPair> Vector2dPairVector;
The mentioned code is (to provide a minimal working example):
#include <iostream>
#include <Eigen/Dense>
typedef std::vector<Eigen::Vector2d> Vector2dVector;
typedef std::pair<Eigen::Vector2d, Eigen::Vector2d> Vector2dPair;
typedef std::vector<Vector2dPair> Vector2dPairVector;
int main()
{
Vector2dPair segment;
segment.first = Eigen::Vector2d(2, -2);
segment.second = Eigen::Vector2d(2, 2);
Vector2dPairVector segments;
segments.push_back(segment);
double angle_min = 0;
double angle_max = M_PI_4;
double angle_increment = M_PI / 16.0;
double range_min = 0.0;
double range_max = 10.0;
Vector2dVector points;
double field_of_view = angle_max - angle_min;
int num_ranges = static_cast<int>(std::nearbyint(std::abs(field_of_view) / angle_increment));
std::cerr << "fov: " << field_of_view << std::endl;
std::cerr << "num ranges: " << num_ranges << std::endl;
points.resize(num_ranges);
for (int i = 0; i < num_ranges; ++i)
{
const double angle = angle_min + i * angle_increment;
std::cerr << "angle: " << angle << std::endl;
const double m = std::tan(angle);
Eigen::Vector2d point = Eigen::Vector2d::Zero();
bool found = false;
for (const auto& segment : segments)
{
// compute segment coefficients (a*x + b*y = c)
double a = segment.second.y() - segment.first.y();
double b = segment.first.x() - segment.second.x();
double c = a * segment.first.x() + b * segment.first.y();
// build A matrix and w vector to setup linear system of equation
Eigen::Matrix2d A;
A << -m, 1, a, b;
Eigen::Vector2d w;
w << 0, c;
// solve linear system to find point of intersection
Eigen::Vector2d p = A.colPivHouseholderQr().solve(w);
// check that the point lies inside the segment
double x_min = std::min(segment.first.x(), segment.second.x());
double x_max = std::max(segment.first.x(), segment.second.x());
double y_min = std::min(segment.first.y(), segment.second.y());
double y_max = std::max(segment.first.y(), segment.second.y());
// point is outside the segment
if (p.x() < x_min || p.x() > x_max || p.y() < y_min || p.y() > y_max)
{
std::cerr << "p: " << p.transpose() << std::endl;
std::cerr << "min: " << x_min << ", " << y_min << std::endl;
std::cerr << "max: " << x_max << ", " << y_max << std::endl;
std::cerr << (p.x() < x_min ? "true" : "false") << std::endl;
std::cerr << (p.x() > x_max ? "true" : "false") << std::endl;
std::cerr << (p.y() < y_min ? "true" : "false") << std::endl;
std::cerr << (p.y() > y_max ? "true" : "false") << std::endl;
continue;
}
std::cerr << "found" << std::endl;
found = true;
point = p;
break;
}
if (!found)
{
throw std::runtime_error("wtf!!");
}
std::cerr << std::endl;
}
return 0;
}
The weird thing is that at some point the final check is not behaving as expected. In particular, I get:
p: 2 0.828427
min: 2, -2
max: 2, 2
true
false
false
false
That is the check p.x() < x_min
is true
when p.x() = 2
and x_min = 2
.
Can someone please tell me what is the problem?
Thanks.