From 4e9a70134693b58f55fb83c4a930c807b3be80fb Mon Sep 17 00:00:00 2001 From: Dtouch Date: Tue, 7 Jan 2025 20:59:40 +0800 Subject: [PATCH] extrusion sdf debug --- application/main.cpp | 188 +++++++++++++++--- application/xmake.lua | 4 + .../interface/internal_primitive_desc.hpp | 6 +- primitive_process/src/extrude_helixline.cpp | 8 +- primitive_process/src/extrude_polyline.cpp | 81 ++++++-- primitive_process/src/polyline.cpp | 90 +++++---- 6 files changed, 292 insertions(+), 85 deletions(-) diff --git a/application/main.cpp b/application/main.cpp index 5e95095..f0121b8 100644 --- a/application/main.cpp +++ b/application/main.cpp @@ -6,48 +6,170 @@ #include #include +#include "Eigen/Core" #include "primitive_descriptor.h" #include "internal_primitive_desc.hpp" -void testExtrudeSDF() { - printf("Testing extrude polyline SDF\n"); - auto profilePoints = std::array{raw_vector3d_t{-1., 0., 0.}, raw_vector3d_t{1., 0., 0.}}; - auto profileBulges = std::array{1.0, 1.0}; - polyline_descriptor_t profile{ - 2, - profilePoints.data(), - 2, - profileBulges.data(), - raw_vector3d_t{0., 0., 1.}, - true - }; - auto axisPoints = std::array{raw_vector3d_t{0., 0., 0.}, raw_vector3d_t{0., 0., 2.}}; - auto axisBulges = std::array{0.0}; - polyline_descriptor_t axis{ - 2, - axisPoints.data(), - 1, - axisBulges.data(), - raw_vector3d_t{0., 1., 0.}, - false - }; - - extrude_polyline_descriptor_t extrude{ - 1, - &profile, - axis +void testExtrudeSDFXoY() +{ + printf("Testing extrude polyline SDF XoY\n"); + auto profilePoints = std::array{ + raw_vector3d_t{-1., 0., 0.}, + raw_vector3d_t{1., 0., 0.} + }; + auto profileBulges = std::array{1.0, 1.0}; + polyline_descriptor_t profile{ + 2, + profilePoints.data(), + 2, + profileBulges.data(), + raw_vector3d_t{0., 0., 1.}, + true + }; + auto axisPoints = std::array{ + raw_vector3d_t{0., 0., 0.}, + raw_vector3d_t{0., 0., 2.} + }; + auto axisBulges = std::array{0.0}; + polyline_descriptor_t axis{ + 2, + axisPoints.data(), + 1, + axisBulges.data(), + raw_vector3d_t{0., 1., 0.}, + false + }; + + extrude_polyline_descriptor_t extrude{1, &profile, axis}; + + aabb_t<> aabb{}; + 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}; + 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", + p.x(), + p.y(), + p.z(), + res, + closest_point.x(), + closest_point.y(), + closest_point.z()); +} + +void testExtrudeSDFZoX() +{ + printf("Testing extrude polyline SDF ZoX\n"); + auto profilePoints = std::array{ + raw_vector3d_t{3., 1., 2.}, + raw_vector3d_t{1., 1., 2.}, + raw_vector3d_t{1., 1., 4.} + }; + auto profileBulges = std::array{1.0, 1.0, 1.0}; + polyline_descriptor_t profile{ + 3, + profilePoints.data(), + 3, + 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} + }; + auto axisBulges = std::array{0.0}; + polyline_descriptor_t axis{ + 2, + axisPoints.data(), + 1, + axisBulges.data(), + raw_vector3d_t{0., 0., 1.}, + false + }; + + extrude_polyline_descriptor_t extrude{1, &profile, axis}; - aabb_t<> aabb{}; - internal::extrude_polyline extrude_polyline{extrude, aabb}; - Eigen::Vector3d p{1., 2., 1.}; - double res = extrude_polyline.evaluate_sdf(p); - printf("SDF at (%f, %f, %f) is %f\n", p.x(), p.y(), p.z(), res); + 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}; + 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", + p.x(), + p.y(), + p.z(), + res, + closest_point.x(), + closest_point.y(), + closest_point.z()); +} + +void testPolylineSDF() +{ + printf("Testing polyline SDF\n"); + auto points = std::array{ + raw_vector3d_t{-1., 1., 0.}, + raw_vector3d_t{1., 1., 0.} + }; + // auto bulges = std::array{0.41421356237309503, 1.0}; // 0.41421356237309503 is tan(PI/8) + auto bulges = std::array{-1., -1.0}; // 0.41421356237309503 is tan(PI/8) + polyline_descriptor_t polyline{ + 2, + points.data(), + 2, + bulges.data(), + raw_vector3d_t{0., 0., 1.}, + true + }; + + aabb_t<2> aabb{}; + internal::polyline internal_polyline; + // Eigen::Transform axis_to_world{}; + // const auto& matrix_handle = axis_to_world.matrix(); + // const Eigen::Matrix4d matrix_handle = Eigen::Matrix4d::Identity(); + internal_polyline.build_as_profile(polyline, + Eigen::Vector3d{1., 0., 0.}, + Eigen::Vector3d{0., 1., 0.}, + Eigen::Vector3d{0., 0., 0.}, + aabb); + std::array testPts = { + // Eigen::Vector3d{-1., 0., 0.}, + // Eigen::Vector3d{0., 0., 0.}, + // Eigen::Vector3d{1., 0., 0.}, + // Eigen::Vector3d{1., 1., 0.}, + // Eigen::Vector3d{1., 2., 0.}, + Eigen::Vector3d{0., 1., 0.}, + + // Eigen::Vector3d{-1., 0., 1.}, + // Eigen::Vector3d{0., 0., 1.}, + // Eigen::Vector3d{1., 0., 1.}, + // Eigen::Vector3d{1., 1., 1.}, + // Eigen::Vector3d{1., 2., 1.}, + // Eigen::Vector3d{0., 1., 1.}, + }; + for (const auto &pt : testPts) { + auto [axis_closest_param, distance] = internal_polyline.calculate_closest_param(pt); + printf("For query point (%f, %f, %f), closest param: %f closest point (%f, %f, %f), distance: %f\n", + pt.x(), + pt.y(), + pt.z(), + axis_closest_param.t, + axis_closest_param.point.x(), + axis_closest_param.point.y(), + axis_closest_param.point.z(), + distance); + } } int main() { - testExtrudeSDF(); + // testExtrudeSDFZoX(); + testExtrudeSDFXoY(); + // testPolylineSDF(); // std::cout << "Setting scene..." << std::endl; // sphere_descriptor_t sphere1{ diff --git a/application/xmake.lua b/application/xmake.lua index 9aae8ad..d35d93e 100644 --- a/application/xmake.lua +++ b/application/xmake.lua @@ -10,4 +10,8 @@ target("test_frontend") set_kind("binary") add_deps("frontend") add_files("main.cpp") + add_linkdirs("E:\\CProj\\ImplicitSurfaceNetwork\\build\\windows\\x64\\debug") + after_build(function(target) + print("Hello, xmake!") + end) target_end() \ No newline at end of file diff --git a/primitive_process/interface/internal_primitive_desc.hpp b/primitive_process/interface/internal_primitive_desc.hpp index 4fe69da..aafe61e 100644 --- a/primitive_process/interface/internal_primitive_desc.hpp +++ b/primitive_process/interface/internal_primitive_desc.hpp @@ -179,11 +179,13 @@ struct PE_API polyline { void build_as_axis(const polyline_descriptor_t&, const raw_vector3d_t&, Eigen::Transform&, - aabb_t<>&); + aabb_t<>&, + const raw_vector3d_t&); void build_as_axis(polyline_descriptor_t&&, const raw_vector3d_t&, Eigen::Transform&, - aabb_t<>&); + aabb_t<>&, + const raw_vector3d_t&); void build_as_profile(const polyline_descriptor_t&, const Eigen::Ref&, const Eigen::Ref&, diff --git a/primitive_process/src/extrude_helixline.cpp b/primitive_process/src/extrude_helixline.cpp index ddfcc14..c9dde04 100644 --- a/primitive_process/src/extrude_helixline.cpp +++ b/primitive_process/src/extrude_helixline.cpp @@ -66,10 +66,10 @@ 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 (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); - } + 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 diff --git a/primitive_process/src/extrude_polyline.cpp b/primitive_process/src/extrude_polyline.cpp index ec368e0..f413375 100644 --- a/primitive_process/src/extrude_polyline.cpp +++ b/primitive_process/src/extrude_polyline.cpp @@ -1,15 +1,22 @@ +#include "Eigen/Core" #include "internal_primitive_desc.hpp" +#include +#include namespace internal { extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aabb_t<> &aabb) { - this->axis.build_as_axis(desc.axis, desc.profiles[0].reference_normal, this->axis_to_world, aabb); + this->axis.build_as_axis(desc.axis, + desc.profiles[0].reference_normal, + this->axis_to_world, + aabb, + desc.profiles[0].points[0]); const auto &matrix_handle = this->axis_to_world.matrix(); aabb_t<2> profile_aabb{}; this->profile.build_as_profile(desc.profiles[0], - matrix_handle.col(0), matrix_handle.col(1), + matrix_handle.col(2), matrix_handle.col(3), profile_aabb); const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff(); @@ -19,12 +26,16 @@ extrude_polyline::extrude_polyline(const extrude_polyline_descriptor_t &desc, aa extrude_polyline::extrude_polyline(extrude_polyline_descriptor_t &&desc, aabb_t<> &aabb) { - this->axis.build_as_axis(std::move(desc.axis), desc.profiles[0].reference_normal, this->axis_to_world, aabb); + this->axis.build_as_axis(std::move(desc.axis), + desc.profiles[0].reference_normal, + this->axis_to_world, + aabb, + desc.profiles[0].points[0]); const auto &matrix_handle = this->axis_to_world.matrix(); aabb_t<2> profile_aabb{}; this->profile.build_as_profile(std::move(desc.profiles[0]), - matrix_handle.col(0), matrix_handle.col(1), + matrix_handle.col(2), matrix_handle.col(3), profile_aabb); const auto profile_max_extent = profile_aabb.max().cwiseMax(profile_aabb.min().cwiseAbs()).maxCoeff(); @@ -40,11 +51,12 @@ static inline auto get_local_TBN(const polyline &line, double t) { Eigen::Vector2d local_tangent, local_normal; - if (t >= 0 && t <= line.start_indices.size() && std::abs(t - std::round(t)) <= EPSILON) { + if (t > EPSILON && t < line.start_indices.size() - EPSILON && std::abs(t - std::round(t)) <= EPSILON + && (q.topRows<2>() - p.topRows<2>()).norm() > EPSILON) { // if between two segments, use local vec{pq} on the plane as normal local_normal = (q.topRows<2>() - p.topRows<2>()).normalized(); if (local_normal.dot(line.calculate_normal(t)) < 0) local_normal *= -1; - local_tangent = {local_normal[1], -local_normal[0]}; + local_tangent = {-local_normal[1], local_normal[0]}; } else { local_tangent = line.calculate_tangent(t); local_normal = line.calculate_normal(t); @@ -66,25 +78,70 @@ 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, axis_closest_t] = desc.axis.calculate_closest_param(local_p.topRows<3>()); + 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); + 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>()); - if (!desc.axis.isEnd(axis_closest_t) || !axis_closest_param.is_peak_value) { - // when the point is two sides away, and pmc is always true - dist *= desc.profile.pmc((inv_TBN * local_p).topRows<2>(), profile_closest_param); - } + // 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; + } + } // transform closest point to world space // the distance should be unchanged during transformation, since no scaling is involved if constexpr (std::is_same_v) return dist; else - return desc.axis_to_world * TBN * profile_closest_param.point; + return desc.axis_to_world * profile_closest_param.point; } double extrude_polyline::evaluate_sdf([[maybe_unused]] const Eigen::Ref &point) const diff --git a/primitive_process/src/polyline.cpp b/primitive_process/src/polyline.cpp index 4e88c5c..268af56 100644 --- a/primitive_process/src/polyline.cpp +++ b/primitive_process/src/polyline.cpp @@ -2,6 +2,7 @@ #include +#include "Eigen/Core" #include "internal_primitive_desc.hpp" static inline auto manually_projection(const raw_vector3d_t &point, @@ -30,18 +31,20 @@ namespace internal void polyline::build_as_axis(const polyline_descriptor_t &desc, const raw_vector3d_t &profile_ref_normal, Eigen::Transform &axis_to_world, - aabb_t<> &aabb) + aabb_t<> &aabb, + const raw_vector3d_t &anchor_point) { auto &matrix_handle = axis_to_world.matrix(); - vec3d_conversion(desc.reference_normal, matrix_handle.col(1)); - matrix_handle.col(1).normalize(); - vec3d_conversion(profile_ref_normal, matrix_handle.col(2)); + vec3d_conversion(profile_ref_normal, matrix_handle.col(0)); + matrix_handle.col(0).normalize(); + vec3d_conversion(desc.reference_normal, matrix_handle.col(2)); matrix_handle.col(2).normalize(); - matrix_handle.col(0) = matrix_handle.col(1).cross(matrix_handle.col(2)); - vec3d_conversion(desc.points[0], matrix_handle.col(3)); + 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)); aabb_t<2> profile_aabb; - build_as_profile(desc, matrix_handle.col(2), matrix_handle.col(0), matrix_handle.col(3), profile_aabb); + build_as_profile(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 = { @@ -54,18 +57,20 @@ void polyline::build_as_axis(const polyline_descriptor_t void polyline::build_as_axis(polyline_descriptor_t &&desc, const raw_vector3d_t &profile_ref_normal, Eigen::Transform &axis_to_world, - aabb_t<> &aabb) + aabb_t<> &aabb, + const raw_vector3d_t &anchor_point) { auto &matrix_handle = axis_to_world.matrix(); - vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(1)); - matrix_handle.col(1).normalize(); - vec3d_conversion(profile_ref_normal, matrix_handle.col(2)); + vec3d_conversion(profile_ref_normal, matrix_handle.col(0)); + matrix_handle.col(0).normalize(); + vec3d_conversion(std::move(desc.reference_normal), matrix_handle.col(2)); matrix_handle.col(2).normalize(); - matrix_handle.col(0) = matrix_handle.col(1).cross(matrix_handle.col(2)); - vec3d_conversion(desc.points[0], matrix_handle.col(3)); + 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)); aabb_t<2> profile_aabb; - build_as_profile(std::move(desc), matrix_handle.col(2), matrix_handle.col(0), matrix_handle.col(3), 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 = { @@ -105,6 +110,10 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, } }); // i.e. iff close curve, deduce loop to simple segements by copying the first vertex to the end + Eigen::Vector3d proj_x_dir_tmp = proj_x_dir; + Eigen::Vector3d proj_y_dir_tmp = proj_y_dir; + Eigen::Vector3d proj_origin_tmp = proj_origin; // for debug + this->vertices.resize(this->start_indices.back() + 1); this->start_indices.pop_back(); circle_start_indices.shrink_to_fit(); @@ -136,26 +145,27 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, const auto &bulge = desc.bulges[index]; const auto &theta = this->thetas[index]; - const auto &a = this->vertices[start_index]; - const auto &b = this->vertices[start_index + 3]; - auto &c = this->vertices[start_index + 1]; - auto &d = this->vertices[start_index + 2]; - const auto half_vec_ab = 0.5 * (a + b); + const auto &a = this->vertices[start_index]; + const auto &b = this->vertices[start_index + 3]; + auto &c = this->vertices[start_index + 1]; + auto &d = this->vertices[start_index + 2]; + const auto center_vec_ab = 0.5 * (a + b); + const auto half_vec_ab = b - center_vec_ab; + Eigen::Matrix temp = center_vec_ab; // for debug if (theta == PI) { - c = std::move(half_vec_ab); + c = std::move(center_vec_ab); } else { // NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2) // and no more operations are needed for identifying the circle center // since all direction info is already included in the sign & magnitude of bulge const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); - c = half_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]}; } const auto vec_ca = a - c; if (bulge > 0) d = c + Eigen::Vector2d{vec_ca[1], -vec_ca[0]}; else d = c + Eigen::Vector2d{-vec_ca[1], vec_ca[0]}; - aabb.extend(c); aabb.extend(d); }); @@ -222,19 +232,21 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, const auto &bulge = desc.bulges[index]; const auto &theta = this->thetas[index]; - const auto &a = this->vertices[start_index]; - const auto &b = this->vertices[start_index + 3]; - auto &c = this->vertices[start_index + 1]; - auto &d = this->vertices[start_index + 2]; - const auto half_vec_ab = 0.5 * (a + b); + const auto &a = this->vertices[start_index]; + const auto &b = this->vertices[start_index + 3]; + auto &c = this->vertices[start_index + 1]; + auto &d = this->vertices[start_index + 2]; + const auto center_vec_ab = 0.5 * (a + b); + const auto half_vec_ab = b - center_vec_ab; + Eigen::Matrix temp = center_vec_ab; // for debug if (theta == PI) { - c = std::move(half_vec_ab); + c = std::move(center_vec_ab); } else { // NOTE: bulge = tan(theta/4), so we can directly calculate tan(theta/2) // and no more operations are needed for identifying the circle center // since all direction info is already included in the sign & magnitude of bulge const auto inv_tan_half_theta = (1 - bulge * bulge) / (2 * bulge); - c = half_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]}; } const auto vec_ca = a - c; if (bulge > 0) @@ -274,11 +286,11 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, if (this->thetas[n] <= EPSILON) { // HINT: assume the plane normal (ref_normal) to be {0, 0, 1}, then the normal should be this const auto temp = (*(iter + 1) - *iter).normalized(); - return {temp.y(), -temp.x()}; + return Eigen::Vector2d{-temp.y(), temp.x()}.normalized(); } else { const auto alpha = std::cos(t * this->thetas[n]); const auto beta = std::sin(t * this->thetas[n]); - return (alpha + beta) * *(iter + 1) - alpha * *iter - beta * *(iter + 2); + return ((alpha + beta) * *(iter + 1) - alpha * *iter - beta * *(iter + 2)).normalized(); } } @@ -329,7 +341,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, const auto p_vec_norm = p_vec.norm(); const auto r = base_vec1.norm(); const auto dis_on_plane = p_vec_norm - r; - const auto closest_point = p_vec / p_vec_norm * r + *(iter + 1); + auto closest_point = *iter; + if (p_vec_norm >= EPSILON) { closest_point = p_vec / p_vec_norm * r + *(iter + 1); } return { {closest_point.x(), closest_point.y(), 0}, phi / theta, @@ -338,6 +351,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, }; std::vector closest_params(this->thetas.size()); + const Eigen::Vector3d pTmp = p; // for debugging algorithm::transform(counting_iterator(), counting_iterator(this->thetas.size()), closest_params.begin(), @@ -368,10 +382,18 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, dist }; }); + const auto back = this->vertices.back(); + if (*vertices.begin() != back) { + closest_params.emplace_back(comparable_closest_param_t{ + {back.x(), back.y(), 0}, + static_cast(thetas.size()), + std::sqrt((p.topRows<2>() - back).squaredNorm() + p[2] * p[2]) + }); + } const auto iter_ = algorithm::min_element(closest_params.begin(), closest_params.end()); return { - line_closest_param_t{std::move(iter->point), iter->t, false}, - iter->dist * iter->dist_sign + line_closest_param_t{std::move(iter_->point), iter_->t, false}, + iter_->dist * iter_->dist_sign }; } }