/* ********************************************************************************************* */
bool RRT::connect(const VectorXd &target) {
// Get the index of the nearest neighbor in the tree to the given target
int NNidx = getNearestNeighbor(target);
// Keep taking steps towards the target until a collision happens
StepResult result = STEP_PROGRESS;
while(result == STEP_PROGRESS) {
result = tryStepFromNode(target, NNidx);
NNidx = configVector.size() - 1;
}
return (result == STEP_REACHED);
}
/**
* @function connect
* @brief
*/
bool JG_RRT::connect( const Eigen::VectorXd &target )
{
int NNidx = getNearestNeighbor(target);
StepResult result = STEP_PROGRESS;
int i = 0;
while(result == STEP_PROGRESS)
{
result = tryStep(target, NNidx);
NNidx = configVector.size() - 1;
i++;
}
return (result == STEP_REACHED);
}
/**
* @function tryStep
*/
JG_RRT::StepResult JG_RRT::tryStep( const Eigen::VectorXd &qtry )
{
int NNidx = getNearestNeighbor( qtry );
return tryStep( qtry, NNidx );
}
/* ********************************************************************************************* */
RRT::StepResult RRT::tryStep(const VectorXd &qtry) {
int NNidx = getNearestNeighbor(qtry);
return tryStepFromNode(qtry, NNidx);
}
/**
* @function tryStep
* @brief Try to advance one stepSize towards _qtry
*/
B1RRT::StepResult B1RRT::tryStep( const Eigen::VectorXd &_qtry ) {
int NNIdx = getNearestNeighbor( _qtry );
return tryStepFromNode( _qtry, NNIdx );
}
