finish randomBiasState

Author: Jason27chan

src/rrt/2dplane/PlaneStateSpace.hpp

@@ -33,25 +33,17 @@ class PlaneStateSpace : public StateSpace<POINT_CLASS> {
      */
     POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const {
         // Generates random value based on goalBias
-        float logitX = logit(goalBias, (float) rand() / RAND_MAX);
-        float logitY = logit(goalBias, (float) rand() / RAND_MAX);
+        float logitX = logit(goalBias / 3 + .67, (float) rand() / RAND_MAX);
+        float logitY = logit(goalBias / 3 + .67, (float) rand() / RAND_MAX);
         float offsetY = 0;
         float offsetX = 0;
 
         // Scale X value based on distance from border
-        if (logitX > 0) {
-           offsetX = (width() - goalState.x()) * logitX;
-        } else if (logitX < 0) {
-            offsetX = goalState.x() * logitX;
-        }
+        offsetX = std::max(goalState.x(), width() - goalState.x()) * logitX;
 
         // Scale Y value based on distance from border
-        if (logitY > 0) {
-            offsetY = (height() - goalState.y()) * logitY;
-        } else if (logitY < 0) {
-            offsetY = goalState.y() * logitY;
-        }
-
+        offsetY = std::max(goalState.y(), height() - goalState.y()) * logitY;
+        
         int randX = goalState.x() + offsetX;
         int randY = goalState.y() + offsetY;
         return POINT_CLASS(randX, randY);

src/rrt/Tree.hpp

@@ -241,11 +241,13 @@ class Tree {
         float r =
             rand() /
             (float)RAND_MAX;  //  r is between 0 and one since we normalize it
-        if (r < goalBias() + waypointBias() && _waypoints.size() > 0) {
+        if (r < goalBias()) {
+            return extend(goalState());
+        } else if (r < goalBias() + waypointBias() && _waypoints.size() > 0) {
             const T& waypoint = _waypoints[rand() % _waypoints.size()];
             return extend(waypoint);
         } else {
-            return extend(_stateSpace->randomBiasState(goalState(), goalBias()));
+            return extend(_stateSpace->randomState());
         }
     }