From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 25 Jul 2019 12:45:57 +0000 (+0200)
Subject: Add right boundary parking slot SOLID object
X-Git-Tag: SOLID
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/psp.git/commitdiff_plain/48cd35c0a1faf7990f41e1cace7908d7035a019f?hp=c9c7b5437d15d5bf81a9323a15387133c99a3baa

Add right boundary parking slot SOLID object
---

diff --git a/src/psp.cc b/src/psp.cc
index 464649b..471d6cf 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -43,8 +43,10 @@ bool PSPlanner::collide()
         dtCreateObject(&this->cc(), bcbox);
         double ps_b = 0;
         double ps_t = 0;
+        double ps_r = 0;
         dtCreateObject(&ps_b, tbox);
         dtCreateObject(&ps_t, tbox);
+        dtCreateObject(&ps_r, rbox);
         dtDisableCaching();
         dtSetDefaultResponse(do_nothing, DT_SIMPLE_RESPONSE, stdout);
         // properly position objects
@@ -86,6 +88,22 @@ bool PSPlanner::collide()
                 + center_shift * sin(this->ps().heading()),
                 0
         );
+        dtSelectObject(&ps_r);
+        dtLoadIdentity();
+        cc_q = Quaternion(-this->ps().heading(), 0, 0);
+        dtRotate(cc_q[X], cc_q[Y], cc_q[Z], cc_q[W]);
+        center_shift = 2 / 2 + 0.01;
+        double center_shift_angle = this->ps().heading();
+        center_shift_angle += (this->ps().right()) ? - M_PI / 2 : + M_PI / 2;
+        dtTranslate(
+                this->ps().x2()
+                + (this->ps().x3() - this->ps().x2()) / 2
+                + center_shift * cos(center_shift_angle),
+                this->ps().y2()
+                + (this->ps().y3() - this->ps().y2()) / 2
+                + center_shift * sin(center_shift_angle),
+                0
+        );
         // check collisions
         bool collide = true;
         if (dtTest() == 0)
@@ -94,6 +112,7 @@ bool PSPlanner::collide()
         dtDeleteObject(&this->cc());
         dtDeleteObject(&ps_b);
         dtDeleteObject(&ps_t);
+        dtDeleteObject(&ps_r);
         dtDeleteShape(bcbox);
         dtDeleteShape(tbox);
         dtDeleteShape(rbox);