Add top and bottom parking slot SOLID objects

diff --git a/src/psp.cc b/src/psp.cc
index 72429b7f62339c6f2285c713906f7a0cb67c05c3..d7b885235f8d0298e458daaad26eff1ebb6981ab 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -41,6 +41,10 @@ bool PSPlanner::collide()
         ); // for right and left
         // create objects
         dtCreateObject(&this->cc(), bcbox);
+        double ps_b = 0;
+        double ps_t = 0;
+        dtCreateObject(&ps_b, tbox);
+        dtCreateObject(&ps_t, tbox);
         dtDisableCaching();
         dtSetDefaultResponse(do_nothing, DT_SIMPLE_RESPONSE, stdout);
         // properly position objects
@@ -54,12 +58,42 @@ bool PSPlanner::collide()
         );
         Quaternion cc_q(-this->cc().h(), 0, 0);
         dtRotate(cc_q[X], cc_q[Y], cc_q[Z], cc_q[W]);
+        dtSelectObject(&ps_b);
+        dtLoadIdentity();
+        center_shift = - 2 / 2;
+        dtTranslate(
+                this->ps().x1()
+                + (this->ps().x2() - this->ps().x1()) / 2
+                + center_shift * cos(this->ps().heading()),
+                this->ps().y1()
+                + (this->ps().y2() - this->ps().y1()) / 2
+                + center_shift * sin(this->ps().heading()),
+                0
+        );
+        cc_q = Quaternion(-this->ps().heading(), 0, 0);
+        dtRotate(cc_q[X], cc_q[Y], cc_q[Z], cc_q[W]);
+        dtSelectObject(&ps_t);
+        dtLoadIdentity();
+        center_shift = 2 / 2;
+        dtTranslate(
+                this->ps().x4()
+                + (this->ps().x3() - this->ps().x4()) / 2
+                + center_shift * cos(this->ps().heading()),
+                this->ps().y4()
+                + (this->ps().y3() - this->ps().y4()) / 2
+                + center_shift * sin(this->ps().heading()),
+                0
+        );
+        cc_q = Quaternion(-this->ps().heading(), 0, 0);
+        dtRotate(cc_q[X], cc_q[Y], cc_q[Z], cc_q[W]);
         // check collisions
         bool collide = true;
         if (dtTest() == 0)
                 collide = false;
         // delete shapes and objects
         dtDeleteObject(&this->cc());
+        dtDeleteObject(&ps_b);
+        dtDeleteObject(&ps_t);
         dtDeleteShape(bcbox);
         dtDeleteShape(tbox);
         dtDeleteShape(rbox);