0

I want to use clothoid obtain the next point, i found the formulars in the figure and source code, but it is difficult for me to understand it , who can help me to tackle the problem?

enter image description here

void end_of_clothoid(double x_i, double y_i, double theta_i, double kappa_i, double sigma, double direction,
                     double length, double *x_f, double *y_f, double *theta_f, double *kappa_f)
{
  // x_f = x_i + int_0_length(cos(theta_i + kappa_i*s + 0.5*sigma_i*s^2)ds)
  // y_f = y_i + int_0_length(sin(theta_i + kappa_i*s + 0.5*sigma_i*s^2)ds)
  double sgn_sigma = sgn(sigma);
  double abs_sigma = fabs(sigma);
  double sqrt_sigma_inv = 1 / sqrt(abs_sigma);
  double k1 = theta_i - 0.5 * direction * kappa_i * kappa_i / sigma;
  double k2 = SQRT_PI_INV * sqrt_sigma_inv * (abs_sigma * length + sgn_sigma * kappa_i);
  double k3 = SQRT_PI_INV * sqrt_sigma_inv * sgn_sigma * kappa_i;
  double cos_k1 = cos(k1);
  double sin_k1 = sin(k1);
  double fresnel_s_k2;
  double fresnel_c_k2;
  double fresnel_s_k3;
  double fresnel_c_k3;
  fresnel(k2, fresnel_s_k2, fresnel_c_k2);
  fresnel(k3, fresnel_s_k3, fresnel_c_k3);
  *x_f = x_i +
         SQRT_PI * sqrt_sigma_inv *
             (direction * cos_k1 * (fresnel_c_k2 - fresnel_c_k3) - sgn_sigma * sin_k1 * (fresnel_s_k2 - fresnel_s_k3));
  *y_f = y_i +
         SQRT_PI * sqrt_sigma_inv *
             (direction * sin_k1 * (fresnel_c_k2 - fresnel_c_k3) + sgn_sigma * cos_k1 * (fresnel_s_k2 - fresnel_s_k3));

// theta_f = theta_i + kappa_ilength + 0.5sigmalength^2 theta_f = pify(theta_i + kappa_i * direction * length + 0.5 * sigma * direction * length * length); // kappa_f = kappa_i + sigma * d * length kappa_f = kappa_i + sigma length; }

Yihu
  • 1
  • Please clarify your specific problem or provide additional details to highlight exactly what you need. As it's currently written, it's hard to tell exactly what you're asking. – Community Aug 01 '23 at 14:12

0 Answers0