diff --git a/application/main.cpp b/application/main.cpp index f0121b8..f2a691f 100644 --- a/application/main.cpp +++ b/application/main.cpp @@ -27,8 +27,8 @@ void testExtrudeSDFXoY() true }; auto axisPoints = std::array{ - raw_vector3d_t{0., 0., 0.}, - raw_vector3d_t{0., 0., 2.} + raw_vector3d_t{1., 0., 0.}, + raw_vector3d_t{1., 0., 2.} }; auto axisBulges = std::array{0.0}; polyline_descriptor_t axis{ @@ -46,7 +46,7 @@ void testExtrudeSDFXoY() internal::extrude_polyline extrude_polyline{extrude, aabb}; // Eigen::Vector3d p{1., 1., -0.1}; // Eigen::Vector3d p{0.3, 0.5, 1.9}; - Eigen::Vector3d p{1., 1., 0.5}; + Eigen::Vector3d p{.7, .8, 0.6}; double res = extrude_polyline.evaluate_sdf(p); Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p); printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n", @@ -65,20 +65,20 @@ void testExtrudeSDFZoX() auto profilePoints = std::array{ raw_vector3d_t{3., 1., 2.}, raw_vector3d_t{1., 1., 2.}, - raw_vector3d_t{1., 1., 4.} + // raw_vector3d_t{1., 1., 4.} }; - auto profileBulges = std::array{1.0, 1.0, 1.0}; + auto profileBulges = std::array{1.0, 1.0}; polyline_descriptor_t profile{ - 3, + 2, profilePoints.data(), - 3, + 2, profileBulges.data(), raw_vector3d_t{0., 1., 0.}, true }; auto axisPoints = std::array{ - raw_vector3d_t{3., 1., 2}, - raw_vector3d_t{3., 3., 2} + raw_vector3d_t{3., 1., 1.5}, + raw_vector3d_t{3., 3., 1.5} }; auto axisBulges = std::array{0.0}; polyline_descriptor_t axis{ @@ -95,7 +95,7 @@ void testExtrudeSDFZoX() aabb_t<> aabb{}; internal::extrude_polyline extrude_polyline{extrude, aabb}; // Eigen::Vector3d p{1., 1., -0.1}; - Eigen::Vector3d p{2.3, 2.9, 1.5}; + Eigen::Vector3d p{2.6, 2.9, 1.2}; double res = extrude_polyline.evaluate_sdf(p); Eigen::Vector3d closest_point = extrude_polyline.evaluate_cpm(p); printf("SDF at (%f, %f, %f) is %f, closest point: (%f, %f, %f)\n", diff --git a/primitive_process/interface/internal_primitive_desc.hpp b/primitive_process/interface/internal_primitive_desc.hpp index aafe61e..ab75cea 100644 --- a/primitive_process/interface/internal_primitive_desc.hpp +++ b/primitive_process/interface/internal_primitive_desc.hpp @@ -175,6 +175,7 @@ struct PE_API polyline { // construct local XY coordinate system std::vector thetas{}; // for a straight line, this should be 0 std::vector start_indices{}; // has the same size as thetas, indicating the starting index of each segment + double bugle; void build_as_axis(const polyline_descriptor_t&, const raw_vector3d_t&, @@ -220,6 +221,7 @@ struct helixline { [[nodiscard]] Eigen::Vector3d calculate_tangent(double) const; [[nodiscard]] Eigen::Vector3d calculate_normal(double) const; + [[nodiscard]] Eigen::Vector3d evaluate(double) const; // CAUTION: make sure the input point is in the local space of the helixline [[nodiscard]] line_closest_param_t calculate_closest_param(const Eigen::Ref&) const; [[nodiscard]] bool isEnd(const double t) const; diff --git a/primitive_process/src/extrude_helixline.cpp b/primitive_process/src/extrude_helixline.cpp index c9dde04..1e6df33 100644 --- a/primitive_process/src/extrude_helixline.cpp +++ b/primitive_process/src/extrude_helixline.cpp @@ -45,8 +45,8 @@ static inline auto get_local_TBN(const helixline &line, const Eigen::Ref() = std::move(B); - handle.col(1).topRows<3>() = std::move(N); + handle.col(0).topRows<3>() = std::move(N); + handle.col(1).topRows<3>() = std::move(B); handle.col(2).topRows<3>() = std::move(T); handle.col(3).topRows<3>() = std::move(q); @@ -66,11 +66,55 @@ static inline process_return_type_t evaluate(const extrude_helixline // TODO: add support for non-parallel (ray vs. profile plane) case // for now just assume that profile plane is parallel to axis auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>()); - if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { - // if !is_peak_value, the point is two sides away, and pmc is always true - dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param) * 2 - 1; + + bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param); + profile_closest_param.point = TBN * profile_closest_param.point; + if (!isOut) { + // inside or projected point inside + const auto local_p_3d = local_p.topRows<3>() / local_p[3]; + if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { + // inside + // max() with two planes + dist = -dist; + Eigen::Vector3d start_tangent = desc.axis.calculate_tangent(0); + auto sdf_start_plane = -start_tangent.dot(local_p_3d); + if (sdf_start_plane > dist) { + dist = sdf_start_plane; + profile_closest_param.point = local_p_3d + start_tangent * dist; + } else { + Eigen::Vector3d final_v = desc.axis.evaluate(1); + Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1); + auto sdf_end_plane = final_tangent.dot(local_p_3d - final_v); + if (sdf_end_plane > dist) { + dist = sdf_end_plane; + profile_closest_param.point = local_p_3d - final_tangent * dist; + } + } + } else { + // outside, must closer to the plane + if (axis_closest_param.t < EPSILON) { + Eigen::Vector3d start_tangent = desc.axis.calculate_tangent(0); + auto sdf_start_plane = -start_tangent.dot(local_p_3d); + dist = sdf_start_plane; + profile_closest_param.point = local_p_3d + start_tangent * dist; + } else { + Eigen::Vector3d final_v = desc.axis.evaluate(1); + Eigen::Vector3d final_tangent = desc.axis.calculate_tangent(1); + auto sdf_end_plane = final_tangent.dot(local_p_3d - final_v); + dist = sdf_end_plane; + profile_closest_param.point = local_p_3d - final_tangent * dist; + } + } + } else { + // outside, nothing to do } + + // if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { + // // if !is_peak_value, the point is two sides away, and pmc is always true + // dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param) * 2 - 1; + // } + // transform closest point to world space // the distance should be unchanged during transformation, since no scaling is involved if constexpr (std::is_same_v) diff --git a/primitive_process/src/extrude_polyline.cpp b/primitive_process/src/extrude_polyline.cpp index 30b28d3..1b15d52 100644 --- a/primitive_process/src/extrude_polyline.cpp +++ b/primitive_process/src/extrude_polyline.cpp @@ -79,63 +79,88 @@ static inline process_return_type_t evaluate(const extrude_polyline const auto world_to_axis = inversed_affine_transform(desc.axis_to_world); const auto local_p = world_to_axis * Eigen::Vector4d{point[0], point[1], point[2], 1}; const auto [axis_closest_param, _] = desc.axis.calculate_closest_param(local_p.topRows<3>()); - const auto TBN = get_local_TBN(desc.axis, local_p, axis_closest_param.point, axis_closest_param.t); - const auto inv_TBN = inversed_affine_transform(TBN); + Eigen::Vector3d axisStart; + axisStart << desc.axis.vertices[0][0], desc.axis.vertices[0][1], 0; + const auto TBN = get_local_TBN(desc.axis, local_p, axis_closest_param.point - axisStart, axis_closest_param.t); + const auto inv_TBN = inversed_affine_transform(TBN); - std::cout << "world_to_axis: " << std::endl << world_to_axis.matrix() << std::endl; - std::cout << "axis_to_world: " << std::endl << desc.axis_to_world.matrix() << std::endl; - std::cout << "TBN" << std::endl << TBN.matrix() << std::endl; - std::cout << "inv_TBN" << std::endl << inv_TBN.matrix() << std::endl; + // std::cout << "world_to_axis: " << std::endl << world_to_axis.matrix() << std::endl; + // std::cout << "axis_to_world: " << std::endl << desc.axis_to_world.matrix() << std::endl; + // std::cout << "TBN" << std::endl << TBN.matrix() << std::endl; + // std::cout << "inv_TBN" << std::endl << inv_TBN.matrix() << std::endl; // TODO: add support for non-parallel (ray vs. profile plane) case // for now just assume that profile plane is parallel to axis auto [profile_closest_param, dist] = desc.profile.calculate_closest_param((inv_TBN * local_p).topRows<3>()); - // bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param); - // if (!isOut) { - // const auto TBN_handle = TBN.matrix(); - // // inside or projected point inside - // if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { - // // inside - // auto dist_start_plane = - // fabs(TBN_handle.col(2).topRows<3>().dot(TBN_handle.col(3).topRows<3>())); if - // (dist_start_plane < dist) { dist = dist_start_plane; - // profile_closest_param.point = } else { - // } - // dist = -fmin(dist, ); - // } else { - // // outside - // dist = TBN_handle.col(2).topRows<3>().dot(TBN_handle.col(3).topRows<3>()); - // } - // } else { - // // outside, nothing to do - // } - - const auto local_p_3d = local_p.topRows<3>() / local_p[3]; - auto profile_p = inv_TBN * local_p_3d; - if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { - // when the point is two sides away, and pmc is always true - bool pmc = desc.profile.pmc((inv_TBN * local_p_3d).topRows<2>(), profile_closest_param); - dist *= pmc * 2 - 1; - } - // max() with two planes - profile_closest_param.point = TBN * profile_closest_param.point; - Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0); - auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>()); - if (sdf_start_plane > dist) { - dist = sdf_start_plane; - profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist; - } else { - Eigen::Vector2d final_v = desc.axis.vertices.back(); - Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size()); - auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v); - if (sdf_end_plane > dist) { - dist = sdf_end_plane; - profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist; + bool isOut = desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param); + const auto tmp = inv_TBN * local_p; + profile_closest_param.point = TBN * profile_closest_param.point; + if (!isOut) { + // inside or projected point inside + const auto local_p_3d = local_p.topRows<3>() / local_p[3]; + if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { + // inside + // max() with two planes + dist = -dist; + Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0); + auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>()); + if (sdf_start_plane > dist) { + dist = sdf_start_plane; + profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist; + } else { + Eigen::Vector2d final_v = desc.axis.vertices.back(); + Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size()); + auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v); + if (sdf_end_plane > dist) { + dist = sdf_end_plane; + profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist; + } + } + } else { + // outside, must closer to the plane + if (axis_closest_param.t < EPSILON) { + Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0); + auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>()); + dist = sdf_start_plane; + profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist; + } else { + Eigen::Vector2d final_v = desc.axis.vertices.back(); + Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size()); + auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v); + dist = sdf_end_plane; + profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist; + } } + } else { + // outside, nothing to do } + + // const auto profile_p = inv_TBN * local_p_3d; + // if (!desc.axis.isEnd(axis_closest_param.t) || axis_closest_param.is_peak_value) { + // // when the point is two sides away, and pmc is always true + // bool pmc = desc.profile.pmc((inv_TBN * local_p_3d).topRows<2>(), profile_closest_param); + // dist *= pmc * 2 - 1; + // } + // // max() with two planes + // profile_closest_param.point = TBN * profile_closest_param.point; + // Eigen::Vector2d start_tangent = desc.axis.calculate_tangent(0); + // auto sdf_start_plane = -start_tangent.dot(local_p_3d.topRows<2>()); + // if (sdf_start_plane > dist) { + // dist = sdf_start_plane; + // profile_closest_param.point = local_p_3d + Eigen::Vector3d(start_tangent[0], start_tangent[1], 0) * dist; + // } else { + // Eigen::Vector2d final_v = desc.axis.vertices.back(); + // Eigen::Vector2d final_tangent = desc.axis.calculate_tangent(desc.axis.thetas.size()); + // auto sdf_end_plane = final_tangent.dot(local_p_3d.topRows<2>() - final_v); + // if (sdf_end_plane > dist) { + // dist = sdf_end_plane; + // profile_closest_param.point = local_p_3d - Eigen::Vector3d(final_tangent[0], final_tangent[1], 0) * dist; + // } + // } + // transform closest point to world space // the distance should be unchanged during transformation, since no scaling is involved if constexpr (std::is_same_v) diff --git a/primitive_process/src/helixline.cpp b/primitive_process/src/helixline.cpp index 06cf8ba..2b9c526 100644 --- a/primitive_process/src/helixline.cpp +++ b/primitive_process/src/helixline.cpp @@ -75,6 +75,13 @@ helixline::helixline(helixline_descriptor_t return {-std::cos(t * this->total_theta), -std::sin(t * this->total_theta), 0.}; } +[[nodiscard]] Eigen::Vector3d helixline::evaluate(double t) const +{ + assert(t >= 0 && t <= 1); + double theta = t * total_theta; + return {radius * std::cos(theta), radius * std::sin(theta), t * height}; +} + [[nodiscard]] line_closest_param_t helixline::calculate_closest_param(const Eigen::Ref &p) const { const auto h_p = p.z(); diff --git a/primitive_process/src/polyline.cpp b/primitive_process/src/polyline.cpp index 3610a37..2037db8 100644 --- a/primitive_process/src/polyline.cpp +++ b/primitive_process/src/polyline.cpp @@ -46,7 +46,7 @@ void polyline::build_as_axis(const polyline_descriptor_t aabb_t<2> profile_aabb; Eigen::Vector3d proj_origin; vec3d_conversion(desc.points[0], proj_origin); - build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), proj_origin, profile_aabb); + build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); const auto &profile_aabb_min = profile_aabb.min(); const auto &profile_aabb_max = profile_aabb.max(); aabb = { @@ -68,13 +68,13 @@ void polyline::build_as_axis(polyline_descriptor_t vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(2)); matrix_handle.col(2).normalize(); matrix_handle.col(1) = matrix_handle.col(2).cross(matrix_handle.col(0)); - // vec3d_conversion(desc.points[0], matrix_handle.col(3)); - vec3d_conversion(anchor_point, matrix_handle.col(3)); + vec3d_conversion(desc.points[0], matrix_handle.col(3)); + // vec3d_conversion(anchor_point, matrix_handle.col(3)); aabb_t<2> profile_aabb; Eigen::Vector3d proj_origin; vec3d_conversion(desc.points[0], proj_origin); - build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), proj_origin, profile_aabb); + build_as_profile(std::move(desc), matrix_handle.col(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); const auto &profile_aabb_min = profile_aabb.min(); const auto &profile_aabb_max = profile_aabb.max(); aabb = { @@ -150,7 +150,7 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, const auto &theta = this->thetas[index]; const auto &a = this->vertices[start_index]; - const auto &b = this->vertices[start_index + 3]; + const auto &b = this->vertices[start_index + 2]; auto &c = this->vertices[start_index + 1]; const auto center_vec_ab = 0.5 * (a + b); Eigen::Matrix temp = center_vec_ab; // for debug @@ -162,7 +162,7 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, // since all direction info is already included in the sign & magnitude of bulge const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); const auto half_vec_ab = center_vec_ab - a; - c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{-half_vec_ab[1], half_vec_ab[0]}; + c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; } aabb.extend(c); @@ -231,7 +231,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, const auto &theta = this->thetas[index]; const auto &a = this->vertices[start_index]; - const auto &b = this->vertices[start_index + 3]; + const auto &b = this->vertices[start_index + 2]; auto &c = this->vertices[start_index + 1]; const auto center_vec_ab = 0.5 * (a + b); Eigen::Matrix temp = center_vec_ab; // for debug @@ -243,7 +243,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, // since all direction info is already included in the sign & magnitude of bulge const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); const auto half_vec_ab = center_vec_ab - a; - c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{-half_vec_ab[1], half_vec_ab[0]}; + c = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; } aabb.extend(c); @@ -254,8 +254,15 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, { assert(t >= 0 && t <= this->start_indices.size()); - double frac; - const auto n = static_cast(std::modf(t, &frac)); + double int_part; + double frac; + if (t == static_cast(this->start_indices.size()) && *vertices.begin() != *vertices.rbegin()) { + frac = 1; + int_part = t - 1; + } else { + frac = std::modf(t, &int_part); + } + const auto n = static_cast(int_part); const auto iter = this->vertices.begin() + this->start_indices[n]; const auto &theta = this->thetas[n]; @@ -268,7 +275,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, // so it's tangent can be simply calculated as M_R' * vec{CA} // and M_R' can be simply obtained by adding a PI/2 rotation to the rotation matrix // CAUTION: we do not need normalize here anymore - Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI2}; + Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI2 * sign(theta)}; return rotation_matrix_deriv.toRotationMatrix() * -line_vec; } } @@ -278,8 +285,15 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, { assert(t >= 0 && t <= this->start_indices.size()); - double frac; - const auto n = static_cast(std::modf(t, &frac)); + double int_part; + double frac; + if (t == static_cast(this->start_indices.size()) && *vertices.begin() != *vertices.rbegin()) { + frac = 1; + int_part = t - 1; + } else { + frac = std::modf(t, &int_part); + } + const auto n = static_cast(int_part); const auto iter = this->vertices.begin() + this->start_indices[n]; const auto &theta = this->thetas[n]; @@ -291,8 +305,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, // as mentioned above, the matrix M_R'' can be obtained by adding a PI rotation to the rotation matrix // caution: raw direction always points to the inside of the circle // but we want it to point the inside of the polyline, so we need to reverse the direction when theta < 0 - Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI}; - return sign(theta) * rotation_matrix_deriv.toRotationMatrix() * -line_vec; + Eigen::Rotation2Dd rotation_matrix_deriv{t * this->thetas[n] + PI * sign(theta)}; + return -sign(theta) * rotation_matrix_deriv.toRotationMatrix() * -line_vec; } } @@ -334,6 +348,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, const auto base_vec2 = Eigen::Vector2d{-base_vec1[1], base_vec1[0]}; const auto p_vec = p.topRows<2>() - *(iter + 1); + const Eigen::Vector2d p_vec_tmp = p_vec; // for debugging + const auto local_x = base_vec1.dot(p_vec); const auto local_y = base_vec2.dot(p_vec); const auto phi = std::atan2(local_y, local_x); @@ -403,12 +419,67 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, [[nodiscard]] bool polyline::pmc(const Eigen::Ref &p, const line_closest_param_t &closest_param) const { - if (closest_param.is_peak_value) { - return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) < 0; + // straight polyline pmc + const auto ptInPolygon = [&](const Eigen::Ref &p) { + int intersectionCount = 0; + // int onSegIdx = -1; + constexpr int numRays = 3; + int majorityIn = 0; + for (int rayIdx = 0; rayIdx < numRays; ++rayIdx) { + double angle = (PI2 * rayIdx) / numRays; + Eigen::Vector2d rayDir(cos(angle), sin(angle)); + int crossings = 0; + + for (int i = 0; i < start_indices.size(); ++i) { + const Eigen::Vector2d &a = vertices[start_indices[i]]; + const Eigen::Vector2d &b = -EPSILON <= thetas[i] && thetas[i] <= EPSILON ? vertices[start_indices[i] + 1] + : vertices[start_indices[i] + 2]; + double dx1 = b[0] - a[0]; + double dy1 = b[1] - a[1]; + double dx2 = rayDir[0]; + double dy2 = rayDir[1]; + double determinant = dx1 * dy2 - dy1 * dx2; + + // if determinant is 0，line is parallel with the ray，no intersections + if (determinant == 0) continue; + + double t1 = ((p[0] - a[0]) * dy2 - (p[1] - a[1]) * dx2) / determinant; + double t2 = ((p[0] - a[0]) * dy1 - (p[1] - a[1]) * dx1) / determinant; + + if (t1 >= 0 && t1 <= 1 && t2 >= 0) { crossings++; } + } + if (crossings % 2 != 0) { majorityIn++; } + } + return majorityIn > numRays / 2; + }; + + bool inFan = false; + int onLinesegButHasBugle = -1; + for (int i = 0; i < start_indices.size(); ++i) { + const Eigen::Vector2d &a = vertices[start_indices[i]]; + const auto theta = thetas[i]; + if (-EPSILON <= thetas[i] && thetas[i] <= EPSILON) continue; + const Eigen::Vector2d &b = vertices[start_indices[i] + 2]; + const auto &c = vertices[start_indices[i] + 1]; + const auto ca = a - c; + if ((p - c).norm() < ca.norm()) { + const auto theta_2 = theta / 2; + const auto cos_theta_2 = std::cos(theta_2), sin_theta_2 = std::sin(theta_2); + const Eigen::Vector2d ab_arc_mid{a[0] + ca[0] * cos_theta_2 - ca[1] * sin_theta_2, + a[1] + ca[0] * sin_theta_2 + ca[1] * cos_theta_2}; + const auto in_circle_dir = Eigen::Vector2d{ab_arc_mid - c}.normalized(); + if ((p - a).dot(in_circle_dir) > 0) { + inFan = true; + break; + } + } } - // the closest point is the vertex of the line - // todo: may not be robust for rare cases - return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) < 0; + + // if (closest_param.is_peak_value) { + // return (p - closest_param.point.topRows<2>()).dot(this->calculate_normal(closest_param.t)) > 0; + // } + auto isInPolygon = ptInPolygon(p); + return !(ptInPolygon(p) ^ inFan); } [[nodiscard]] bool polyline::isEnd(const double t) const { return t >= thetas.size() - EPSILON || t < EPSILON; }