#include <cmath>
#include "steer.h"
+#define ST2WHEELBASE 2.450
+#define ST2TURNINGRADI 10.82
+#define ST2MAXSTEERING atan(ST2WHEELBASE / ST2TURNINGRADI)
+
std::vector<RRTNode *> st1(
RRTNode *init,
RRTNode *goal)
}
return nodes;
}
+
+std::vector<RRTNode *> st2(
+ RRTNode *init,
+ RRTNode *goal)
+{
+ float speed = 1;
+ float angl = atan2(goal->y() - init->y(), goal->x() - init->x());
+ float sa = angl - init->h(); // steering angle
+ float co = cos(angl - init->h());
+ float si = sin(angl - init->h());
+ if (co < 0) {
+ speed = -speed;
+ if (si > 0)
+ sa = -sa;
+ } else {
+ if (si < 0)
+ sa = -sa;
+ }
+ if (sa > ST2MAXSTEERING)
+ sa = ST2MAXSTEERING;
+ if (sa < -ST2MAXSTEERING)
+ sa = -ST2MAXSTEERING;
+ float new_h = init->h() + (speed / ST2WHEELBASE) * tan(sa);
+ float new_x = init->x() + speed * cos(new_h);
+ float new_y = init->y() + speed * sin(new_h);
+ std::vector<RRTNode *> nodes;
+ nodes.push_back(new RRTNode(new_x, new_y, new_h));
+ return nodes;
+}