From 1bb69ee626db544269b98200458cf33104b11ab4 Mon Sep 17 00:00:00 2001 From: Zhicheng Wang <1627343141@qq.com> Date: Wed, 8 Jan 2025 01:05:25 +0800 Subject: [PATCH] new robust representation of circle; now directions should always be correct; fixed the translation from axis to profile --- .../interface/primitive_descriptor.h | 1 + primitive_process/src/extrude_polyline.cpp | 4 +- primitive_process/src/polyline.cpp | 97 ++++++++++--------- 3 files changed, 53 insertions(+), 49 deletions(-) diff --git a/primitive_process/interface/primitive_descriptor.h b/primitive_process/interface/primitive_descriptor.h index 6bb0ab7..4a34d13 100644 --- a/primitive_process/interface/primitive_descriptor.h +++ b/primitive_process/interface/primitive_descriptor.h @@ -72,6 +72,7 @@ typedef struct { // clockwise order // CAUTION: iff {is_closed} is true, then {point_number} == {bulge_number} should be agreed; iff {is_closed} is false, // then {point_number} == {bulge_number} + 1 should be agreed. +// CAUTION: bulge should be in range [-1, 1] typedef struct { uint32_t point_number; // The point number of the polyline raw_vector3d_t* points; // The points of the polyline diff --git a/primitive_process/src/extrude_polyline.cpp b/primitive_process/src/extrude_polyline.cpp index f413375..30b28d3 100644 --- a/primitive_process/src/extrude_polyline.cpp +++ b/primitive_process/src/extrude_polyline.cpp @@ -64,8 +64,8 @@ static inline auto get_local_TBN(const polyline &line, Eigen::Transform TBN{}; auto &handle = TBN.matrix(); - handle.col(0)[2] = 1; - handle.col(1).topRows<2>() = std::move(local_normal); + handle.col(0).topRows<2>() = std::move(local_normal); + handle.col(1)[2] = 1; handle.col(2).topRows<2>() = std::move(local_tangent); handle.col(3).topRows<3>() = std::move(q); diff --git a/primitive_process/src/polyline.cpp b/primitive_process/src/polyline.cpp index 268af56..3610a37 100644 --- a/primitive_process/src/polyline.cpp +++ b/primitive_process/src/polyline.cpp @@ -43,8 +43,10 @@ void polyline::build_as_axis(const polyline_descriptor_t // 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(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); + 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); const auto &profile_aabb_min = profile_aabb.min(); const auto &profile_aabb_max = profile_aabb.max(); aabb = { @@ -69,8 +71,10 @@ void polyline::build_as_axis(polyline_descriptor_t // 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(0), matrix_handle.col(1), matrix_handle.col(3), profile_aabb); + 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); const auto &profile_aabb_min = profile_aabb.min(); const auto &profile_aabb_max = profile_aabb.max(); aabb = { @@ -101,12 +105,12 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, std::plus{}, [&](size_t index) { auto &theta = this->thetas[index]; - theta = std::atan(fabs(desc.bulges[index])) * 4; + theta = std::atan(desc.bulges[index]) * 4; if (theta <= EPSILON) { return uint8_t{1}; } else { circle_start_indices.push_back(static_cast(index)); - return uint8_t{3}; + return uint8_t{2}; } }); // i.e. iff close curve, deduce loop to simple segements by copying the first vertex to the end @@ -148,9 +152,7 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, 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(center_vec_ab); @@ -159,15 +161,11 @@ void polyline::build_as_profile(const polyline_descriptor_t &desc, // 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 = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; + 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]}; } - 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); }); } @@ -192,12 +190,12 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, std::plus{}, [&](size_t index) { auto &theta = this->thetas[index]; - theta = std::atan(fabs(desc.bulges[index])) * 4; - if (theta <= EPSILON) { + theta = std::atan(desc.bulges[index]) * 4; + if (-EPSILON <= theta && theta <= EPSILON) { return uint8_t{1}; } else { circle_start_indices.push_back(static_cast(index)); - return uint8_t{3}; + return uint8_t{2}; } }); // i.e. iff close curve, deduce loop to simple segements by copying the first vertex to the end @@ -235,9 +233,7 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, 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(center_vec_ab); @@ -246,16 +242,11 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, // 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 = center_vec_ab + inv_tan_half_theta * Eigen::Vector2d{half_vec_ab[1], -half_vec_ab[0]}; + 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]}; } - 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); }); } @@ -263,15 +254,22 @@ 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)); - const auto iter = this->vertices.begin() + this->start_indices[n]; - if (this->thetas[n] <= EPSILON) { - return (*(iter + 1) - *iter).normalized(); + double frac; + const auto n = static_cast(std::modf(t, &frac)); + const auto iter = this->vertices.begin() + this->start_indices[n]; + const auto &theta = this->thetas[n]; + + const auto line_vec = (*(iter + 1) - *iter).normalized(); // i.e. vec{AB} (line) or vec{AC} (circle) + if (-EPSILON <= theta && theta <= EPSILON) { + return line_vec; } else { - const auto alpha_deriv = -std::sin(t * this->thetas[n]); - const auto beta_deriv = std::cos(t * this->thetas[n]); - return (alpha_deriv * *iter + beta_deriv * *(iter + 2) - (alpha_deriv + beta_deriv) * *(iter + 1)).normalized(); + // NOTE: it can be proved that P = M_R * A + (1 - M_R) * C, + // where M_R is the rotation matrix corresponding to delta theta + // 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}; + return rotation_matrix_deriv.toRotationMatrix() * -line_vec; } } @@ -280,17 +278,21 @@ 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)); - const auto iter = this->vertices.begin() + this->start_indices[n]; - if (this->thetas[n] <= EPSILON) { + double frac; + const auto n = static_cast(std::modf(t, &frac)); + const auto iter = this->vertices.begin() + this->start_indices[n]; + const auto &theta = this->thetas[n]; + + const auto line_vec = (*(iter + 1) - *iter).normalized(); + if (-EPSILON <= theta && theta <= 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 Eigen::Vector2d{-temp.y(), temp.x()}.normalized(); + return {-line_vec[1], line_vec[0]}; } 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)).normalized(); + // 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; } } @@ -329,14 +331,14 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, const auto iter = this->vertices.begin() + this->start_indices[index]; const auto theta = this->thetas[index]; const auto base_vec1 = *iter - *(iter + 1); - const auto base_vec2 = *(iter + 2) - *(iter + 1); + const auto base_vec2 = Eigen::Vector2d{-base_vec1[1], base_vec1[0]}; const auto p_vec = p.topRows<2>() - *(iter + 1); 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); // illegal solve, return null - if (phi < 0 || phi > theta) return {}; + if (!((0 <= phi && phi <= theta) || (theta <= phi && phi <= 0))) return {}; const auto p_vec_norm = p_vec.norm(); const auto r = base_vec1.norm(); @@ -356,7 +358,8 @@ void polyline::build_as_profile(polyline_descriptor_t &&desc, counting_iterator(this->thetas.size()), closest_params.begin(), [&](size_t index) { - if (this->thetas[index] <= EPSILON) + const auto &theta = this->thetas[index]; + if (-EPSILON <= theta && theta <= EPSILON) return line_cpm(index); else return circle_cpm(index);