Add and use #cusp

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index d2c93180f99d2000e4c07d156e88520d5cffb910..68b4f6e32171f554e026f3bb171a820f970ff07f 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -1,3 +1,9 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
 #include <algorithm>
 #include <cassert>
 #include "rrts.hh"
@@ -53,6 +59,21 @@ RRTNode::p(RRTNode& p)
        }
 }
 
+unsigned int
+RRTNode::cusp() const
+{
+       return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+       this->cusp_ = p.cusp();
+       if (this->sp() != p.sp() || this->sp() == 0.0) {
+               this->cusp_++;
+       }
+}
+
 bool
 RRTNode::operator==(RRTNode const& n)
 {
@@ -102,6 +123,7 @@ RRTS::join_steered(RRTNode* f)
                RRTNode* t = &this->nodes_.back();
                t->p(*f);
                t->c(this->cost_build(*f, *t));
+               t->cusp(*f);
                this->steered_.erase(this->steered_.begin());
                f = t;
        }
@@ -137,6 +159,7 @@ RRTS::connect()
        t = &this->nodes_.back();
        t->p(*f);
        t->c(this->cost_build(*f, *t));
+       t->cusp(*f);
        this->steered_.erase(this->steered_.begin());
        return true;
 }