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; }