2 #define CUTE_C2_IMPLEMENTATION
8 this->c2_bc().count = 4;
9 this->c2_bc().verts[0].x = bc.lfx();
10 this->c2_bc().verts[0].y = bc.lfy();
11 this->c2_bc().verts[1].x = bc.lrx();
12 this->c2_bc().verts[1].y = bc.lry();
13 this->c2_bc().verts[2].x = bc.rrx();
14 this->c2_bc().verts[2].y = bc.rry();
15 this->c2_bc().verts[3].x = bc.rfx();
16 this->c2_bc().verts[3].y = bc.rfy();
18 for (auto o: this->obstacles()) {
20 c2tmp.count = (o.poly().size() < C2_MAX_POLYGON_VERTS)
22 : C2_MAX_POLYGON_VERTS
25 for (auto c: o.poly()) {
26 if (i < C2_MAX_POLYGON_VERTS) {
27 c2tmp.verts[i].x = std::get<0>(c);
28 c2tmp.verts[i].y = std::get<1>(c);
32 this->c2_obstacles().push_back(c2tmp);
36 void RRTExt2::deinit()
38 this->c2_obstacles().clear();
42 std::tuple<bool, unsigned int, unsigned int>
43 RRTExt2::collide_steered_from(RRTNode &f)
45 this->c2x_bc().p.x = f.x();
46 this->c2x_bc().p.y = f.y();
47 this->c2x_bc().r.c = cos(f.h());
48 this->c2x_bc().r.s = sin(f.h());
49 for (auto &o: this->c2_obstacles()) {
51 &this->c2_bc(), &this->c2x_bc(),
54 return std::make_tuple(true, 0, 0);
58 for (auto &n: this->steered()) {
59 this->c2x_bc().p.x = n.x();
60 this->c2x_bc().p.y = n.y();
61 this->c2x_bc().r.c = cos(n.h());
62 this->c2x_bc().r.s = sin(n.h());
63 for (auto &o: this->c2_obstacles()) {
65 &this->c2_bc(), &this->c2x_bc(),
68 return std::make_tuple(true, i, 0);
73 return std::make_tuple(false, 0, 0);
75 std::tuple<bool, unsigned int, unsigned int>
76 RRTExt2::collide_tmp_steered_from(RRTNode &f)
78 this->c2x_bc().p.x = f.x();
79 this->c2x_bc().p.y = f.y();
80 this->c2x_bc().r.c = cos(f.h());
81 this->c2x_bc().r.s = sin(f.h());
82 for (auto &o: this->c2_obstacles()) {
84 &this->c2_bc(), &this->c2x_bc(),
87 return std::make_tuple(true, 0, 0);
91 for (auto n: this->tmp_steered_) {
92 this->c2x_bc().p.x = n.x();
93 this->c2x_bc().p.y = n.y();
94 this->c2x_bc().r.c = cos(n.h());
95 this->c2x_bc().r.s = sin(n.h());
96 for (auto &o: this->c2_obstacles()) {
98 &this->c2_bc(), &this->c2x_bc(),
101 return std::make_tuple(true, i, 0);
106 return std::make_tuple(false, 0, 0);
109 std::tuple<bool, unsigned int, unsigned int>
110 RRTExt2::collide_two_nodes(RRTNode &f, RRTNode &t)
112 this->c2x_bc().p.x = f.x();
113 this->c2x_bc().p.y = f.y();
114 this->c2x_bc().r.c = cos(f.h());
115 this->c2x_bc().r.s = sin(f.h());
116 for (auto &o: this->c2_obstacles()) {
118 &this->c2_bc(), &this->c2x_bc(),
121 return std::make_tuple(true, 1, 0);
124 this->c2x_bc().p.x = t.x();
125 this->c2x_bc().p.y = t.y();
126 this->c2x_bc().r.c = cos(t.h());
127 this->c2x_bc().r.s = sin(t.h());
128 for (auto &o: this->c2_obstacles()) {
130 &this->c2_bc(), &this->c2x_bc(),
133 return std::make_tuple(true, 2, 0);
136 return std::make_tuple(false, 0, 0);