From 0ff663da0b08c883af0e22e63fe273c9f125216c Mon Sep 17 00:00:00 2001 From: Zhicheng Wang <1627343141@qq.com> Date: Sun, 12 Jan 2025 16:22:57 +0800 Subject: [PATCH] fix helixline's CPM --- primitive_process/src/helixline.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/primitive_process/src/helixline.cpp b/primitive_process/src/helixline.cpp index 2b9c526..f520278 100644 --- a/primitive_process/src/helixline.cpp +++ b/primitive_process/src/helixline.cpp @@ -118,6 +118,7 @@ helixline::helixline(helixline_descriptor_t const auto line_theta_r = this->total_theta * this->radius; const auto inv_line_theta = 1. / this->total_theta; + const auto height_2 = this->height * this->height; const auto t_intersect = std::sqrt((theta_intersect * theta_intersect * this->radius * this->radius + h_p * h_p) / (line_theta_r * line_theta_r + this->height * this->height)); @@ -133,15 +134,15 @@ helixline::helixline(helixline_descriptor_t { const auto alpha = this->total_theta * this->radius * r_p; std::vector> peak_values(rounded_times_end - rounded_times_start + 1 + 2); - peak_values.front() = {.0, -line_theta_r * r_p * std::sin(theta_p) - h_p}; - peak_values.back() = {1., line_theta_r * r_p * std::sin(this->total_theta - theta_p) + this->height - h_p}; + peak_values.front() = {.0, -alpha * std::sin(theta_p) - h_p * this->height}; + peak_values.back() = {1., alpha * std::sin(this->total_theta - theta_p) + height_2 - h_p * this->height}; algorithm::transform(counting_iterator(rounded_times_start), counting_iterator(rounded_times_end + 1), peak_values.begin() + 1, [&](size_t k_) -> std::pair { const auto t = (theta_p + PI2 + k_ * PI) * inv_line_theta; return {std::move(t), - alpha * std::sin(t * this->total_theta - theta_p) + t * this->height - h_p}; + alpha * std::sin(t * this->total_theta - theta_p) + t * height_2 - h_p * this->height}; }); peak_value_param = algorithm::transform_reduce( counting_iterator{}, @@ -175,10 +176,10 @@ helixline::helixline(helixline_descriptor_t const auto alpha = this->total_theta * this->radius * r_p; const auto alpha_deriv = alpha * this->total_theta; while (std::abs(delta_t) >= EPSILON) { - // origin equation: f(t)=theta_all*r_all*r_p*sin(t*theta_all-theta_p)+t*height_all-h_p=0 - // derivative: f'(t)=theta_all^2*r_all*r_p*cos(t*theta_all-theta_p)+height_all=0 - const auto f = alpha * std::sin(t * this->total_theta - theta_p) + t * this->height - h_p; - const auto f_deriv = alpha_deriv * std::cos(t * this->total_theta - theta_p) + this->height; + // origin equation: f(t)=theta_all*r_all*r_p*sin(t*theta_all-theta_p)+t*height_all*height_all-h_p*height_all=0 + // derivative: f'(t)=theta_all^2*r_all*r_p*cos(t*theta_all-theta_p)+height_all*height_all=0 + const auto f = alpha * std::sin(t * this->total_theta - theta_p) + t * height_2 - h_p * this->height; + const auto f_deriv = alpha_deriv * std::cos(t * this->total_theta - theta_p) + height_2; delta_t = f / f_deriv; t -= delta_t; }