Add default sampling info setter

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5bb1385616c49b3b0826a3409bf8ae303338524f..bf366dd056624663be91466f8dfdc95a38d2670e 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1060,6 +1060,17 @@ int RRTBase::YI(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::defaultSamplingInfo()
+{
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
+}
+
 void RRTBase::setSamplingInfo(SamplingInfo si)
 {
         this->ndx_ = std::normal_distribution<float>(si.x0, si.x);