Add right boundary parking slot SOLID object

diff --git a/src/psp.cc b/src/psp.cc
index 464649b0bfd7e0b1fa33a219504d0a2ba75a53b8..471d6cfb1ff3d635667c18de44b4386acda0c5a0 100644 (file)
--- 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);