From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 5 Apr 2019 16:09:06 +0000 (+0200)
Subject: Add 2nd simple sampling function
X-Git-Tag: v0.6.0~4^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/64642a6245e9f6b188c7cd43ce8a3b776642d56b

Add 2nd simple sampling function
---

diff --git a/base/sample.cc b/base/sample.cc
index ac7ab17..374f367 100644
--- a/base/sample.cc
+++ b/base/sample.cc
@@ -29,3 +29,14 @@ RRTNode *sa1()
                 static_cast<float>(RAND_MAX / (2 * M_PI));
         return new RRTNode(new_x, new_y, new_h);
 }
+
+RRTNode *sa2(float bx, float by, float br)
+{
+        float r = static_cast<float>(rand());
+        r /= static_cast<float>(RAND_MAX / br);
+        float sh = static_cast<float>(rand());
+        sh /= static_cast<float>(RAND_MAX / (2 * M_PI));
+        float h = static_cast<float>(rand());
+        h /= static_cast<float>(RAND_MAX / (2 * M_PI));
+        return new RRTNode(bx + r * cos(sh), by + r * sin(sh), h);
+}
diff --git a/incl/sample.h b/incl/sample.h
index 32f5eff..3d65f84 100644
--- a/incl/sample.h
+++ b/incl/sample.h
@@ -35,5 +35,6 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #define GLVERTEX(n) ((n)->x() * GLPLWSCALE), ((n)->y() * GLPLHSCALE)
 
 RRTNode *sa1();
+RRTNode *sa2(float bx, float by, float br);
 
 #endif