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);
}