double signedDistanceInsideConvexHull(
const Ref<const Matrix<double, 2, Dynamic>> &pts,
const Ref<const Vector2d> &q) {
std::vector<Point> hull_pts = convexHull(eigenToPoints(pts));
double d_star = std::numeric_limits<double>::infinity();
Matrix2d R;
R << 0, 1, -1, 0;
for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) {
Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x,
hull_pts[i + 1].y - hull_pts[i].y);
double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y);
double n = ai.norm();
if (std::isnormal(n)) {
ai = ai.array() / n;
b = b / n;
double d = b - ai.transpose() * q;
// std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y <<
// std::endl;
// std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y <<
// std::endl;
// std::cout << "ai: " << ai.transpose() << std::endl;
// std::cout << "b: " << b << std::endl;
// std::cout << "d: " << d << std::endl;
if (d < d_star) {
d_star = d;
}
}
}
return d_star;
}
Vector2d signed_distance_2d::closestPointOnLineSegment(Vector2d point, Vector2d p1, Vector2d p2) {
// We are working with line representation L = {u*z + d | z is a real number, u is normalized}
// u and d are in R^2 (2D vectors)
// Get Unit vector in direction of edge
Vector2d u = p1 - p2;
if ((u.cwiseAbs()).sum() != 0) {
u.normalize(); // Assuming u has nonzero length
} else {
u << 0, 0;
}
Vector2d d = p1; // d can be any point on the line. p1 or p2 will do
double t = (point - d).dot(u);
// Find if point is on line segment or if it needs to be projected to an end of segment
// Do this in projected 1D space. Find t for both p1 and p2, and compare to t from the point.
double p1t = (p1 - d).dot(u);
double p2t = (p2 - d).dot(u);
if (t <= min(p1t, p2t)) {
t = min(p1t, p2t);
} else if (t >= max(p1t, p2t)) {
t = max(p1t, p2t);
}
Vector2d proj = u.array() * t;
proj += d;
// Sanity check
Vector2d a = perpendicularAxis(u);
double c = a.dot(p1); // Constant, a'*x + d = 0
double eps = 1e-5;
if (abs(a.dot(proj) - c) > eps) {
std::cout << "Warning: Projection on line is not on line with tolerance " << eps << std::endl;
}
return proj;
}
