Find final path only when goal found

[hubacji1/iamcar.git] / incl / bcar.h

diff --git a/incl/bcar.h b/incl/bcar.h
index 3dedf57fbe22b6336f137ddc5fc9f34ee04c15d2..fabbfafcf3d4926d6908a723a16c25dfae8a3959 100644 (file)
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -21,17 +21,35 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include <vector>
 #include "rrtnode.h"
 
+#define BCAR_HEIGHT 1.450
+#define BCAR_LENGTH 3.760
+#define BCAR_SAFETY_DIST 0
+#define BCAR_TURNING_RADIUS 10.820
+#define BCAR_WHEEL_BASE 2.450
+#define BCAR_WIDTH 1.625
+#define BCAR_DIST_REAR ((BCAR_LENGTH - BCAR_WHEEL_BASE) / 2)
+#define BCAR_DIST_FRONT (BCAR_LENGTH - BCAR_DIST_REAR)
+#define BCAR_DIAG_RADI pow(pow(BCAR_DIST_FRONT, 2) + pow(BCAR_WIDTH/2, 2), 0.5)
+#define BCAR_DIAG_RRADI pow(pow(BCAR_DIST_REAR, 2) + pow(BCAR_WIDTH/2, 2), 0.5)
+#define BCAR_OUT_RADI pow( \
+        pow(BCAR_TURNING_RADIUS + BCAR_WIDTH/2, 2) + \
+        pow(BCAR_DIST_FRONT, 2), 0.5)
+#define BCAR_OUT_RRADI pow( \
+        pow(BCAR_TURNING_RADIUS + BCAR_WIDTH/2, 2) + \
+        pow(BCAR_DIST_REAR, 2), 0.5)
+#define BCAR_IN_RADI (BCAR_TURNING_RADIUS - BCAR_WIDTH/2)
+
 #define MAXSTEER(wb, tr) ((wb) / (tr))
 
 class BicycleCar: public RRTNode {
         private:
                 // see https://en.wikipedia.org/wiki/Fiat_Punto
-                const float height_ = 1.450;
-                const float length_ = 3.760;
-                const float safety_dist_ = 0;
-                const float turning_radius_ = 10.82;
-                const float wheel_base_ = 2.450;
-                const float width_ = 1.625;
+                const float height_ = BCAR_HEIGHT;
+                const float length_ = BCAR_LENGTH;
+                const float safety_dist_ = BCAR_SAFETY_DIST;
+                const float turning_radius_ = BCAR_TURNING_RADIUS;
+                const float wheel_base_ = BCAR_WHEEL_BASE;
+                const float width_ = BCAR_WIDTH;
 
                 float speed_ = 0;
                 float steer_ = 0;
@@ -85,6 +103,8 @@ class BicycleCar: public RRTNode {
 
                 /** Return distance from wheelbase center to top corner */
                 float diag_radi();
+                /** Return distance from wheelbase center to rear corner */
+                float diag_rradi();
                 /** Outer radius of the farthest point */
                 float out_radi();
                 /** Angle between wheelbase line and outer radius */