Find final path only when goal found

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

diff --git a/incl/bcar.h b/incl/bcar.h
index 408736e938dd65d37d99d25fe46fd81ba61f4cb8..fabbfafcf3d4926d6908a723a16c25dfae8a3959 100644 (file)
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -21,24 +21,48 @@ 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;
+
+                BicycleCar *bcparent_;
         public:
                 using RRTNode::RRTNode;
 
                 // getter
+                float length();
+                float wheelbase();
+                float width();
+
                 float dr();
                 float df();
 
@@ -64,21 +88,31 @@ class BicycleCar: public RRTNode {
                 float speed();
                 float steer();
 
+                BicycleCar *bcparent() const;
+
                 // setter
                 bool speed(float s);
                 bool steer(float s);
 
+                void bcparent(BicycleCar *bcp);
+
                 // other
                 std::vector<RRTEdge *> frame();
                 bool next();
                 bool next(float speed, float steer);
 
+                /** 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 */
                 float alfa();
                 /** Return true if `n` not inside of `ccl`, `ccr` */
                 bool drivable(RRTNode *n);
+                /** Return node rotated by `dh` around `c` */
+                BicycleCar *move(RRTNode *c, float dh);
 };
 
 #endif