From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 7 Sep 2021 08:23:28 +0000 (+0200)
Subject: Add and use #cusp
X-Git-Tag: v0.9.0~2^2~4
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/36455eefe1f6fbadd9215cd7f6c344d1f8c46223

Add and use #cusp
---

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 02cb9c6..3cee13b 100644
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -161,7 +161,8 @@ class RRTExt11 : public virtual RRTS {
 /*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
  *
  * Use Reeds & Shepp path length for building tree data structure and Euclidean
- * distance plus (abs) heading difference for searching it.
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
  *
  * \ingroup ext-cost
  * \see https://doi.org/10.1109/TITS.2015.2477355
diff --git a/incl/rrts.hh b/incl/rrts.hh
index 7b7f4fc..603c0a8 100644
--- a/incl/rrts.hh
+++ b/incl/rrts.hh
@@ -36,6 +36,7 @@ private:
 	double c_ = 0.0;
 	double cc_ = 0.0;
 	RRTNode* p_ = nullptr;
+	unsigned int cusp_ = 0;
 public:
 	/*! Get cost to parent. */
 	double c() const;
@@ -52,6 +53,12 @@ public:
 	/*! Set parent node. */
 	void p(RRTNode& p);
 
+	/*! Get number of backward-forward direction changes. */
+	unsigned int cusp() const;
+
+	/*! Set number of backward-forward direction changes. */
+	void cusp(RRTNode const& p);
+
 	bool operator==(RRTNode const& n);
 };
 
diff --git a/src/rrtext10.cc b/src/rrtext10.cc
index 63e2ade..83fb6ed 100644
--- a/src/rrtext10.cc
+++ b/src/rrtext10.cc
@@ -21,7 +21,7 @@ RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
 double
 RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-	return f.edist(t) + std::abs(t.h() - f.h());
+	return f.edist(t) + std::abs(t.h() - f.h()) + f.cusp() * 0.1;
 }
 
 } // namespace rrts
diff --git a/src/rrts.cc b/src/rrts.cc
index 23f60a0..68b4f6e 100644
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -59,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)
 {
@@ -108,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;
 	}
@@ -143,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;
 }