X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar2.git/blobdiff_plain/fb4a506532215af72eac32914899486847d61b9f..bc0e538f21a11143089d38495b6cd523ca646757:/rrts/src/reeds_shepp.cpp diff --git a/rrts/src/reeds_shepp.cpp b/rrts/src/reeds_shepp.cpp index d49ad57..9bb15fc 100644 --- a/rrts/src/reeds_shepp.cpp +++ b/rrts/src/reeds_shepp.cpp @@ -566,18 +566,18 @@ void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, doubl s[0] += ( sin(phi+v) - sin(phi)); s[1] += (-cos(phi+v) + cos(phi)); s[2] = phi + v; - s[4] = 1; + s[4] = 1; break; case RS_RIGHT: s[0] += (-sin(phi-v) + sin(phi)); s[1] += ( cos(phi-v) - cos(phi)); s[2] = phi - v; - s[4] = -1; + s[4] = -1; break; case RS_STRAIGHT: s[0] += (v * cos(phi)); s[1] += (v * sin(phi)); - s[4] = 0; + s[4] = 0; break; case RS_NOP: break;