#endif
// RRT
-#include "sample.h"
#include "cost.h"
#include "steer.h"
-#include "nn.h"
-#include "nv.h"
#if USE_GL > 0
extern SDL_Window* gw;
bool RRTBase::glplot()
{
#if USE_GL > 0
+ float glplwscale = 1.0 / ((this->VMAX) - (this->VMIN));
+ float glplhscale = 1.0 / ((this->HMAX) - (this->HMIN));
glClear(GL_COLOR_BUFFER_BIT);
glLineWidth(1);
glPointSize(1);
glColor3f(0.5, 0.5, 0.5);
BicycleCar bc(tmp->x(), tmp->y(), tmp->h());
glVertex2f(
- bc.lfx() * GLPLWSCALE,
- bc.lfy() * GLPLHSCALE
+ bc.lfx() * glplwscale,
+ bc.lfy() * glplhscale
);
glVertex2f(
- bc.lrx() * GLPLWSCALE,
- bc.lry() * GLPLHSCALE
+ bc.lrx() * glplwscale,
+ bc.lry() * glplhscale
);
glVertex2f(
- bc.lrx() * GLPLWSCALE,
- bc.lry() * GLPLHSCALE
+ bc.lrx() * glplwscale,
+ bc.lry() * glplhscale
);
glVertex2f(
- bc.rrx() * GLPLWSCALE,
- bc.rry() * GLPLHSCALE
+ bc.rrx() * glplwscale,
+ bc.rry() * glplhscale
);
glVertex2f(
- bc.rrx() * GLPLWSCALE,
- bc.rry() * GLPLHSCALE
+ bc.rrx() * glplwscale,
+ bc.rry() * glplhscale
);
glVertex2f(
- bc.rfx() * GLPLWSCALE,
- bc.rfy() * GLPLHSCALE
+ bc.rfx() * glplwscale,
+ bc.rfy() * glplhscale
);
}
}
std::vector<RRTNode *> RRTBase::findt()
{
- return this->findt(this->goal_);
+ RRTNode *goal = this->goal_;
+ for (auto g: this->goals()) {
+ if (goal->parent() == nullptr || g->ccost() < g->ccost())
+ goal = g;
+ }
+ if (goal->parent() == nullptr)
+ this->goal_found(false);
+ return this->findt(goal);
}
std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
{
float step = (this->HMAX - this->HMIN) / IXSIZE;
float index = (int) (floor(n->x() - this->HMIN) / step);
+ if (index < 0) index = 0;
+ if (index >= IXSIZE) index = IXSIZE - 1;
return index;
}
{
float step = (this->VMAX - this->VMIN) / IYSIZE;
float index = (int) (floor(n->y() - this->VMIN) / step);
+ if (index < 0) index = 0;
+ if (index >= IYSIZE) index = IYSIZE - 1;
return index;
}
// RRT Framework
+void RRTBase::defaultSamplingInfo()
+{
+ float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+ float hrange = (this->HMAX - this->HMIN) / 2;
+ float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+ float vrange = (this->VMAX - this->VMIN) / 2;
+ this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+ this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+ this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
+}
+
void RRTBase::setSamplingInfo(SamplingInfo si)
{
this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
RRTNode *RRTBase::nn(RRTNode *rs)
{
int iy = this->YI(rs);
+ float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
struct mcnn nn;
nn.nn = nullptr;
nn.mc = 9999;
unsigned int i = 0; // vector step
unsigned int j = 0; // array step
int iyj = 0;
- while (nn.mc > j * IYSTEP) {
+ while (nn.mc > j * iy_step) {
iyj = (int) (iy + j);
if (iyj >= IYSIZE)
iyj = IYSIZE - 1;
{
std::vector<RRTNode *> nvs;
unsigned int iy = this->YI(node);
- unsigned int iy_dist = floor(dist / IYSTEP) + 1;
+ float iy_step = (this->VMAX - this->VMIN) / IYSIZE;
+ unsigned int iy_dist = floor(dist / iy_step) + 1;
unsigned int i = 0; // vector index
unsigned int j = 0; // array index
unsigned int jmin = 0; // minimal j index