Add move method to rotate new node

diff --git a/incl/bcar.h b/incl/bcar.h
index 408736e938dd65d37d99d25fe46fd81ba61f4cb8..c79466e1c8f8c692b1b1820c5a7f29aabfd78516 100644 (file)
--- a/incl/bcar.h
+++ b/incl/bcar.h
@@ -79,6 +79,8 @@ class BicycleCar: public RRTNode {
                 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

diff --git a/vehicle_platform/bcar.cc b/vehicle_platform/bcar.cc
index bb34e4ceb201ce7d9e0acfddf5be1277b88b2dcd..41a8f569952fbc6ab28b25a3f7e5b906b9806c1f 100644 (file)
--- a/vehicle_platform/bcar.cc
+++ b/vehicle_platform/bcar.cc
@@ -244,3 +244,20 @@ bool BicycleCar::drivable(RRTNode *n)
                 drivable = false;
         return drivable;
 }
+
+BicycleCar *BicycleCar::move(RRTNode *c, float dh)
+{
+        float zx = c->x();
+        float zy = c->y();
+        float px = this->x();
+        float py = this->y();
+        px -= zx;
+        py -= zy;
+        float nx = px * cos(dh) - py * sin(dh);
+        float ny = px * sin(dh) + py * cos(dh);
+        px = nx + zx;
+        py = ny + zy;
+        float ph = this->h();
+        ph += dh;
+        return new BicycleCar(px, py, ph);
+}