#include <vector>
-#define IS_NEAR(a, b) ({ __typeof__ (a) _a = (a); \
- __typeof__ (b) _b = (b); \
- pow(pow(_b->x() - _a->x(), 2) + \
- pow(_b->y() - _a->y(), 2), 0.5) < 0.2 && \
- std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; })
+#define IS_NEAR(a, b) ({ \
+ __typeof__ (a) _a = (a); \
+ __typeof__ (b) _b = (b); \
+ pow(pow(_b->x() - _a->x(), 2) \
+ + pow(_b->y() - _a->y(), 2), 0.5) < 0.2 \
+ && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
+
+#define GOAL_IS_NEAR(a, b) ({ \
+ __typeof__ (a) _a = (a); \
+ __typeof__ (b) _b = (b); \
+ pow(pow(_b->x() - _a->x(), 2) \
+ + pow(_b->y() - _a->y(), 2), 0.5) < 0.05 \
+ && std::abs(_b->h() - _a->h()) < M_PI / 32 ? true : false; \
+})
template <typename T> int sgn(T val) {
return (T(0) < val) - (val < T(0));
float h_ = 0;
float t_ = 0;
float s_ = 0;
+
+ RRTNode *rs_ = nullptr; // random sample of added point
public:
RRTNode();
RRTNode(float x, float y);
float t() const;
float s() const;
+ RRTNode *rs() const;
+
float ccost() const;
float dcost() const;
float ocost() const;
void h(float ch);
void t(float ct);
void s(float cs);
+
+ void rs(RRTNode *rs);
+
bool add_child(RRTNode *node, float cost);
bool add_child(RRTNode *node, float cost, float time);
bool rem_child(RRTNode *node);
static bool comp_ccost(RRTNode *n1, RRTNode *n2);
float update_ccost();
bool visit();
+ /** Return ``true`` if ``n`` is in front of ``this``.
+ *
+ * @param n The node that is beeing checked.
+ */
+ bool inFront(RRTNode *n);
};
class RRTEdge {
RRTNode *init() const;
RRTNode *goal() const;
+
+ RRTNode *intersect(RRTEdge *e, bool segment);
};
#endif