Hello, I have a question about the function dubins_LRL, I read the paper "Classification of the Dubins set", and in the paper p= mode2pi(acos(tmp0)), but in your implementation p = mode2pi(2*pi - acos(tmp0)), and out[2] is also different. Could you please tell me why your implementation is different from the paper? Thank you!
Hello, I have a question about the function dubins_LRL, I read the paper "Classification of the Dubins set", and in the paper p= mode2pi(acos(tmp0)), but in your implementation p = mode2pi(2*pi - acos(tmp0)), and out[2] is also different. Could you please tell me why your implementation is different from the paper? Thank you!
int dubins_LRL(DubinsIntermediateResults in, double out[3]) { double tmp0 = (6. - in->d_sq + 2in->c_ab + 2in->d(in->sb - in->sa)) / 8.; double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb ); if( fabs(tmp0) <= 1) { double p = mod2pi( 2*M_PI - acos( tmp0) ); double t = mod2pi(-in->alpha - phi + p/2.); out[0] = t; out[1] = p; out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p)); return EDUBOK; } return EDUBNOPATH; }