From 32b97dbf3bada509f61060911da5a0f7946a04f9 Mon Sep 17 00:00:00 2001 From: Jiri Vlasak Date: Tue, 17 Sep 2019 14:48:12 +0200 Subject: [PATCH] Implement ext2 collide methods --- src/rrtext2.cc | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) 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 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 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); } -- 2.39.2