/**
* @function plan
* @brief
*/
bool JG_RRT::plan( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
const std::vector< Eigen::VectorXd > &_guidingNodes,
std::vector<Eigen::VectorXd> &path )
{
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Add the guiding nodes
addGuidingNodes( _guidingNodes );
double p;
while( goalDistVector[activeNode] > distanceThresh )
{
//-- Generate the probability
p = RANDNM( 0.0, 1.0 );
//-- Apply either extension to goal (J) or random connection
if( p < p_goal )
{
if( extendGoal() == true )
{
break;
}
}
else
{
tryStep(); /*extendRandom();*/
}
// Check if we are still inside
if( configVector.size() > maxNodes )
{ cout<<"-- Exceeded "<<maxNodes<<" allowed - ws_dist: "<<goalDistVector[rankingVector[0]]<<endl;
break;
}
}
//-- If a path is found
if( goalDistVector[activeNode] < distanceThresh )
{ tracePath( activeNode, path );
cout<<"JG - Got a path! - nodes: "<<path.size()<<" tree size: "<<configVector.size()<<endl;
return true;
}
else
{ cout<<"--(!) JG :No successful path found! "<<endl;
return false;
}
}
void AAStar::findPath(std::vector<ANode*>& path) {
float c;
ANode *x, *y;
init();
printf("Pathfinding...");
open.push(start);
start->open = true;
visited.push_back(start);
while (!open.empty()) {
x = open.top(); open.pop();
x->open = false;
if (x == goal) {
tracePath(path);
printf("[done]\n");
return;
}
x->closed = true;
successors(x, succs);
while (!succs.empty()) {
y = succs.front(); succs.pop();
c = x->g + (y->w * heuristic(x, y));
if (y->open && c < y->g)
y->open = false;
/* Only happens with an admissable heuristic */
if (y->closed && c < y->g)
y->closed = false;
if (!y->open && !y->closed) {
y->g = (unsigned int) c;
y->parent = x;
y->h = (y->w * heuristic(y, goal));
open.push(y);
y->open = true;
visited.push_back(y);
history.push_back(y);
history.push_back(x);
}
}
}
printf("[failed]\n");
}
void App::samplePixel(int x, int y, int threadID) {
float randX,randY,randZ;
Random& rng = m_rng[threadID];
for (int i = 0; i < m_featureData.getSamplePerPixelToGo(x,y) ; ++i){
rng.sphere(randX,randY,randZ);
randX = (randX/2.0) + 0.5;
randY = (randY/2.0) + 0.5;
Ray initialRay = m_debugCamera->worldRay(x + randX, y + randY ,Rect2D(Vector2(m_imgWidth,m_imgHeight)));
Point3 pointInFocalPlane = initialRay.intersection(m_focalPlane);
rng.sphere(randX,randY,randZ);
Point3 newCamPos = initialRay.origin() + (Vector3(randX,randY,0.0)*m_world->blurRadius);
Vector3 newDir = (pointInFocalPlane - newCamPos).unit();
tracePath(Ray(newCamPos,newDir), m_world, rng,x,y);
}
}
bool AAStar::findPath(std::list<ANode*>* path) {
float g;
ANode *x, *y;
init();
start->open = true;
start->g = 0.0f;
start->h = heuristic(start,goal);
open.push(start);
while (!open.empty()) {
x = open.top(); open.pop();
if (x == goal) {
if (path)
tracePath(x, *path);
return true;
}
x->open = false;
x->closed = true;
successors(x, succs);
while (!succs.empty()) {
y = succs.front(); succs.pop();
if (y->closed)
continue;
g = x->g + y->w*heuristic(x,y);
if (y->open && g < y->g)
y->open = false;
if (!y->open) {
y->g = g;
y->parent = x;
y->h = heuristic(y, goal);
y->open = true;
open.push(y);
}
visited++;
}
}
return false;
}
