Implement ext2 collide methods

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 432e4de091e6f2adb91f3fb1b00de25a3c780585..74f7cb2b04fbe626ce9fbcf9b053fbf405b5468c 100644 (file)
--- a/src/rrtext2.cc
+++ b/src/rrtext2.cc
@@ -4,12 +4,48 @@
 std::tuple<bool, unsigned int, unsigned int>
 RRTExt2::collide_steered_from(RRTNode &f)
 {
+        {
+                dtSelectObject(&this->bc());
+                dtLoadIdentity();
+                dtTranslate(f.x(), f.y(), 0);
+                Quaternion q(Vector(0, 0, 1), f.h());
+                dtRotate(q[X], q[Y], q[Z], q[W]);
+                if (dtTest())
+                        return std::make_tuple(true, 0, 0);
+        }
+        for (auto n: this->steered()) {
+                dtSelectObject(&this->bc());
+                dtLoadIdentity();
+                dtTranslate(n.x(), n.y(), 0);
+                Quaternion q(Vector(0, 0, 1), n.h());
+                dtRotate(q[X], q[Y], q[Z], q[W]);
+                if (dtTest())
+                        return std::make_tuple(true, 0, 0);
+        }
         return std::make_tuple(false, 0, 0);
 }
 
 std::tuple<bool, unsigned int, unsigned int>
 RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t)
 {
+        {
+                dtSelectObject(&this->bc());
+                dtLoadIdentity();
+                dtTranslate(f.x(), f.y(), 0);
+                Quaternion q(Vector(0, 0, 1), f.h());
+                dtRotate(q[X], q[Y], q[Z], q[W]);
+                if (dtTest())
+                        return std::make_tuple(true, 0, 0);
+        }
+        {
+                dtSelectObject(&this->bc());
+                dtLoadIdentity();
+                dtTranslate(t.x(), t.y(), 0);
+                Quaternion q(Vector(0, 0, 1), t.h());
+                dtRotate(q[X], q[Y], q[Z], q[W]);
+                if (dtTest())
+                        return std::make_tuple(true, 0, 0);
+        }
         return std::make_tuple(false, 0, 0);
 }