Add node type to steer cb

diff --git a/src/rrts.cc b/src/rrts.cc
index c089b85fb52dbf95cc365f176901e95ca01ebca8..46dad56965500cfc050481a86f481335da0c4c60 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -9,6 +9,10 @@
         pow(log(_cV) / _cV, 1.0 / 3.0); \
 })
 
+template <typename T> int sgn(T val) {
+        return (T(0) < val) - (val < T(0));
+}
+
 RRTNode::RRTNode()
 {
 }
@@ -143,11 +147,18 @@ std::vector<RRTNode *> RRTS::nv(RRTNode &t)
 int cb_rs_steer(double q[4], void *user_data)
 {
         std::vector<RRTNode> *nodes = (std::vector<RRTNode> *) user_data;
+        RRTNode *ln = nullptr;
+        if (nodes->size() > 0)
+                ln = &nodes->back();
         nodes->push_back(RRTNode());
         nodes->back().x(q[0]);
         nodes->back().y(q[1]);
         nodes->back().h(q[2]);
         nodes->back().sp(q[3]);
+        if (nodes->back().sp() == 0)
+                nodes->back().set_t(RRTNodeType::cusp);
+        else if (ln != nullptr && sgn(ln->sp()) != sgn(nodes->back().sp()))
+                ln->set_t(RRTNodeType::cusp);
         return 0;
 }