Rewrite rrtext6

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

diff --git a/src/rrtext6.cc b/src/rrtext6.cc
index db7d661e9143c61b301eb7aaf335106a27f8d82a..934c871d6ec2568e6753b01204717a4bc0d11030 100644 (file)
--- a/src/rrtext6.cc
+++ b/src/rrtext6.cc
@@ -1,18 +1,21 @@
-#include "rrtext.h"
+#include "rrtext.hh"
 #include "reeds_shepp.h"
 
-double RRTExt6::cost_build(RRTNode &f, RRTNode &t)
+namespace rrts {
+
+double
+RRTExt6::cost_build(RRTNode const& f, RRTNode const& t) const
 {
        double q0[] = {f.x(), f.y(), f.h()};
        double q1[] = {t.x(), t.y(), t.h()};
-       ReedsSheppStateSpace rsss(this->bc.mtr());
+       ReedsSheppStateSpace rsss(this->bc_.mtr());
        return rsss.distance(q0, q1);
 }
 
-double RRTExt6::cost_search(RRTNode &f, RRTNode &t)
+double
+RRTExt6::cost_search(RRTNode const& f, RRTNode const& t) const
 {
-       double q0[] = {f.x(), f.y(), f.h()};
-       double q1[] = {t.x(), t.y(), t.h()};
-       ReedsSheppStateSpace rsss(this->bc.mtr());
-       return rsss.distance(q0, q1);
+       return this->cost_build(f, t);
 }
+
+} // namespace rrts