Add 2nd simple sampling function

diff --git a/base/sample.cc b/base/sample.cc
index ac7ab176b8a02e4a08494b3e6e721888af33ac5b..374f3679f69dff800a60a38907c9e9fb1c3aad95 100644 (file)
--- 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 32f5eff553ec4ca8b3bc1d869c5791a8e618edf6..3d65f84ddd0e89073115294ab77fd7ec1482de5e 100644 (file)
--- 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