gi

Author: Jason27chan

src/rrt/2dplane/PlaneStateSpace.hpp

@@ -33,10 +33,54 @@ 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 rand1 = (float) rand() / RAND_MAX;
+        float rand2 = (float) rand() / RAND_MAX;
         float offsetY = 0;
         float offsetX = 0;
+        float logitX;
+        float logitY;
+
+        if (rand1 < .5) {
+            if (rand1 < (float) goalState.x() / width()) {
+                logitX = logit(goalBias, rand1);
+                offsetX = goalState.x() * logitX;
+            } else {
+                logitX = -1 * (logit(goalBias, rand1));
+                offsetX = goalState.x() * logitX;
+            }
+        } else {
+            if (rand1 > (float) (width() - goalState.x())/width()) {
+                logitX = logit(goalBias, rand1);
+                offsetX = goalState.x() * logitX;
+            } else {
+                logitX = -logit(goalBias, rand1);
+                offsetX = goalState.x() * logitX;
+            }
+        }
+
+        if (rand2 < .5) {
+            if (rand2 < (float) goalState.y() / height()) {
+                logitY = logit(goalBias, rand2);
+                offsetY = goalState.y() * logitY;
+            } else {
+                logitY = -1 * logit(goalBias, rand2);
+                offsetY = goalState.y() * logitY;
+            }
+        } else {
+            if (rand2 > (float) (height() - goalState.y())/height()) {
+                logitY = logit(goalBias, rand2);
+                offsetY = goalState.y() * logitY;
+            } else {
+                logitY = -logit(goalBias, rand2);
+                offsetY = goalState.y() * logitY;
+            }
+        }
+
+
+
+        // float logitX = logit(goalBias, rand1);
+        // float logitY = logit(goalBias, rand2);
+
 
         // Scale X value based on distance from border
         if (logitX > 0) {