Implement ext2 init/deinit

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 7d641b61f0e9e7e906d555dbe007e2d9ec9d30ea..d572c4d47e058fbfbc29b84867ea57bfc93559bb 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -1,9 +1,52 @@
 #include "rrtext.h"
+#include "3D/Quaternion.h"
+
+void solid_collide(
+        void * client_data,
+        DtObjectRef obj1,
+        DtObjectRef obj2,
+        const DtCollData *coll_data
+)
+{
+}
 
 void RRTExt2::init_solid()
 {
+        this->sr_bc() = dtNewComplexShape();
+        dtBegin(DT_POLYGON);
+        dtVertex(this->bc().lfx(), this->bc().lfy(), 0);
+        dtVertex(this->bc().lrx(), this->bc().lry(), 0);
+        dtVertex(this->bc().rrx(), this->bc().rry(), 0);
+        dtVertex(this->bc().rfx(), this->bc().rfy(), 0);
+        dtEnd();
+        dtEndComplexShape();
+        dtCreateObject(&this->bc(), this->sr_bc());
+
+        this->sr_static_obstacles() = dtNewComplexShape();
+        for (auto &o: this->obstacles()) {
+                dtBegin(DT_POLYGON);
+                for (auto c: o.poly()) {
+                        dtVertex(std::get<0>(c), std::get<1>(c), 0);
+                }
+                dtEnd();
+        }
+        dtEndComplexShape();
+        dtCreateObject(&this->static_obstacle(), this->sr_static_obstacles());
+
+        dtSetDefaultResponse(solid_collide, DT_SIMPLE_RESPONSE, stdout);
+        dtDisableCaching(); // TODO enable
+
+        dtSelectObject(&this->bc());
+        dtLoadIdentity();
+        dtTranslate(this->nodes().front().x(), this->nodes().front().y(), 0);
+        Quaternion q(Vector(0, 0, 1), this->nodes().front().h());
+        dtRotate(q[X], q[Y], q[Z], q[W]);
 }
 
 void RRTExt2::deinit_solid()
 {
+        dtDeleteObject(&this->static_obstacle());
+        dtDeleteObject(&this->bc());
+        dtDeleteShape(this->sr_static_obstacles());
+        dtDeleteShape(this->sr_bc());
 }