From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 25 Feb 2019 14:24:12 +0000 (+0100)
Subject: Add move method to rotate new node
X-Git-Tag: v0.5.0~1^2~14
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/a2c1b88e6ae8309ec84160e8a11b5031cab11fb8

Add move method to rotate new node
---

diff --git a/incl/bcar.h b/incl/bcar.h
index 408736e..c79466e 100644
--- 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 bb34e4c..41a8f56 100644
--- 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);
+}