Implement SOLID, add current car SOLID object

diff --git a/src/psp.cc b/src/psp.cc
index 4761535a917d521dd009ba40f721bd78804b1900..72429b7f62339c6f2285c713906f7a0cb67c05c3 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -8,13 +8,62 @@
         #include <SOLID/solid.h>
         #include <3D/Point.h>
         #include <3D/Quaternion.h>
+        void do_nothing(
+                void *client_data,
+                DtObjectRef obj1,
+                DtObjectRef obj2,
+                const DtCollData *coll_data
+        )
+        {
+        }
 #endif /* USE_SOLID */
 
 bool PSPlanner::collide()
 {
 #ifdef USE_SOLID
-        // TODO
-        return false;
+        // create shapes
+        DtShapeRef bcbox = dtBox(this->cc().w(), this->cc().l(), 1);
+        DtShapeRef tbox = dtBox(
+                sqrt(
+                        pow(this->ps().x4() - this->ps().x3(), 2)
+                        + pow(this->ps().y4() - this->ps().y3(), 2)
+                ),
+                2,
+                1
+        ); // for top and bottom
+        DtShapeRef rbox = dtBox(
+                2,
+                sqrt(
+                        pow(this->ps().x4() - this->ps().x1(), 2)
+                        + pow(this->ps().y4() - this->ps().y1(), 2)
+                ) + 2 * 2,
+                1
+        ); // for right and left
+        // create objects
+        dtCreateObject(&this->cc(), bcbox);
+        dtDisableCaching();
+        dtSetDefaultResponse(do_nothing, DT_SIMPLE_RESPONSE, stdout);
+        // properly position objects
+        dtSelectObject(&this->cc());
+        dtLoadIdentity();
+        double center_shift = this->cc().l() / 2 - this->cc().dr();
+        dtTranslate(
+                this->cc().x() + center_shift * cos(this->cc().h()),
+                this->cc().y() + center_shift * sin(this->cc().h()),
+                0
+        );
+        Quaternion cc_q(-this->cc().h(), 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());
+        dtDeleteShape(bcbox);
+        dtDeleteShape(tbox);
+        dtDeleteShape(rbox);
+        return collide;
 #else /* USE_SOLID */
         if(std::get<0>(intersect(
                 this->cc().lfx(), this->cc().lfy(),