From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 17 Sep 2019 12:48:12 +0000 (+0200)
Subject: Implement ext2 collide methods
X-Git-Tag: w/solid~3^2~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/32b97dbf3bada509f61060911da5a0f7946a04f9

Implement ext2 collide methods
---

diff --git a/src/rrtext2.cc b/src/rrtext2.cc
index 432e4de..74f7cb2 100644
--- 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);
 }