void CCPathFinderNetwork::linkDistantNodes()
{
if( nodes.length > 0 )
{
const float BIG_FLOAT = 10000.0;
const PathNode *topLeft = findClosestNode( CCVector3( -BIG_FLOAT, 0.0, -BIG_FLOAT ) );
const PathNode *topRight = findClosestNode( CCVector3( BIG_FLOAT, 0.0, -BIG_FLOAT ) );
const PathNode *bottomLeft = findClosestNode( CCVector3( -BIG_FLOAT, 0.0, BIG_FLOAT ) );
const PathNode *bottomRight = findClosestNode( CCVector3( BIG_FLOAT, 0.0, BIG_FLOAT ) );
if( topLeft != NULL && topRight != NULL && bottomLeft != NULL && bottomRight != NULL )
{
const float startX = topLeft->point.x < bottomLeft->point.x ? topLeft->point.x : bottomLeft->point.x;
const float endX = topRight->point.x > bottomRight->point.x ? topRight->point.x : bottomRight->point.x;
const float startZ = topLeft->point.z < topRight->point.z ? topLeft->point.z : topRight->point.z;
const float endZ = bottomLeft->point.z > bottomRight->point.z ? bottomLeft->point.z : bottomRight->point.z;
float x = startX;
float z = startZ;
float increment = 150.0;
while( x < endX && z < endZ )
{
x += increment;
if( x >= endX )
{
x = startX + increment;
z += increment;
}
if( z < endZ )
{
addNode( CCVector3( x, 0.0, z ) );
}
}
}
}
}
void Individual::repairPredecessors(size_t from) {
//if (from < 0) {
// return;
//}
for (auto layer_idx = from; layer_idx < MAX_NB_LAYER; layer_idx++) {
for (auto node_idx = 0; node_idx < MAX_NB_NODE_PER_LAYER; node_idx++) {
if (!nodes[layer_idx][node_idx].activated) {
continue;
}
auto& current_node = nodes[layer_idx][node_idx];
for (auto pred_idx = 0;
pred_idx < current_node.nb_predecessor;
pred_idx++) {
auto& pred = current_node.pred[pred_idx];
current_node.pred[pred_idx] = findClosestNode(pred.x, pred.y);
}
}
}
}
void _SnacRemesher_InterpolateNodes( void* _context ) {
Snac_Context* context = (Snac_Context*)_context;
SnacRemesher_Context* contextExt = ExtensionManager_Get( context->extensionMgr,
context,
SnacRemesher_ContextHandle );
Mesh* mesh = context->mesh;
SnacRemesher_Mesh* meshExt = ExtensionManager_Get( context->meshExtensionMgr,
mesh,
SnacRemesher_MeshHandle );
NodeLayout* nLayout = mesh->layout->nodeLayout;
Node_LocalIndex newNode_i;
IndexSet* extNodes;
void interpolateNode( void* _context, Node_LocalIndex newNodeInd, Element_DomainIndex dEltInd );
/*
** Free any owned arrays that may still exist from the last node interpolation.
*/
FreeArray( meshExt->externalNodes );
meshExt->nExternalNodes = 0;
/*
** Scoot over all the new nodes and find the old element in which each one resides, then interpolate.
*/
/* Create an index set for storing any external nodes. */
extNodes = IndexSet_New( mesh->nodeLocalCount );
for( newNode_i = 0; newNode_i < mesh->nodeLocalCount; newNode_i++ ) {
Node_LocalIndex dNodeInd;
unsigned nElements;
Element_DomainIndex* elements;
Coord newPoint;
unsigned elt_i;
/* Extract the new node's coordinate. */
Vector_Set( newPoint, meshExt->newNodeCoords[newNode_i] );
/* Find the closest old node. */
dNodeInd = findClosestNode( context, newPoint, newNode_i );
/* Grab incident elements. */
{
Node_GlobalIndex gNodeInd;
gNodeInd = Mesh_NodeMapDomainToGlobal( mesh, dNodeInd );
nElements = nLayout->nodeElementCount( nLayout, gNodeInd );
if( nElements ) {
elements = Memory_Alloc_Array( Element_DomainIndex, nElements, "SnacRemesher" );
nLayout->buildNodeElements( nLayout, gNodeInd, elements );
}
else {
elements = NULL;
}
}
/* Convert global element indices to domain. */
for( elt_i = 0; elt_i < nElements; elt_i++ ) {
elements[elt_i] = Mesh_ElementMapGlobalToDomain( mesh, elements[elt_i] );
}
/* Which of the incident elements contains the node? */
for( elt_i = 0; elt_i < nElements; elt_i++ ) {
if( elements[elt_i] >= mesh->elementDomainCount ) {
continue;
}
if( pointInElement( context, newPoint, elements[elt_i] ) ) {
break;
}
}
/* Did we find the element? */
if( elt_i < nElements ) {
/* If so, call a function to locate the tetrahedra and interpolate. */
interpolateNode( context, newNode_i, elements[elt_i] );
}
else {
/* If not, then the new node finds itself outside the old mesh. In this scenario, we cannot interpolate
the nodal values with any accuracy (without knowing more about the physical problem). So, we will leave
the node with its old values and mark this node as not being interpolated so the user may deal with it. */
/* Stash the node index. */
IndexSet_Add( extNodes, newNode_i );
/* Copy across the old value. Note that this should be done using some other provided copy method. */
memcpy( (unsigned char*)meshExt->newNodes + newNode_i * mesh->nodeExtensionMgr->finalSize,
(unsigned char*)mesh->node + newNode_i * mesh->nodeExtensionMgr->finalSize,
mesh->nodeExtensionMgr->finalSize );
/* assert(0); */
}
/* Free element array. */
FreeArray( elements );
}
/* Dump the external nodes and delete the set. */
IndexSet_GetMembers( extNodes, &meshExt->nExternalNodes, &meshExt->externalNodes );
Stg_Class_Delete( extNodes );
}
void Thread_RRT::planPath(const Thread* start, const Thread* goal, vector<Thread_Motion>& movements, vector<Thread*>& intermediateThread)
{
RRT_Node startNode = RRT_Node(start);
RRT_Node goalNode = RRT_Node(goal);
initializeSearch(startNode, goalNode);
intermediateThread.push_back(new Thread(start));
intermediateThread.push_back(new Thread(goal));
/*
intermediateThread.push_back(new Thread(start));
MatrixXd resampled_1(33,3);
startNode.thread->getPoints(resampled_1);
MatrixXd resampled_2(17,3);
startNode.thread->getPoints(resampled_2);
intermediateThread.push_back(new Thread(startNode.thread, 17, 2));
intermediateThread.push_back(new Thread(resampled_2, 2, startNode.thread->length()));*/
bool goalReached = false;
double bestDistToGoal = DBL_MAX;
while (!goalReached && currNodes.size() < MAX_NUM_NODES)
{
std::cout << "curr ind" << currNodes.size() << std::endl;
//get a goal for this iteration
RRT_Node currGoal;
getNextGoal(goalNode, currGoal);
//find the nearest neighbor
int indClosest;
findClosestNode(currGoal, indClosest);
//find motion
Thread_Motion* nextMotion = new Thread_Motion();
findThreadMotion(currNodes[indClosest]->this_node, currGoal, *nextMotion);
//add new node
currNodes.push_back(new RRT_Node_And_Edge(currNodes[indClosest], nextMotion));
currNodes[indClosest]->this_node.applyMotion(currNodes.back()->this_node, *nextMotion);
double thisDistToGoal = goalNode.distanceToNode(currNodes.back()->this_node);
if (thisDistToGoal < bestDistToGoal)
{
bestDistToGoal = thisDistToGoal;
std::cout << "new best dist: " << bestDistToGoal << std::endl;
}
if (goalNode.distanceToNode(currNodes.back()->this_node) < GOAL_DISTANCE_THRESH)
{
goalReached = true;
}
}
int finalIndClosest;
findClosestNode(goalNode, finalIndClosest);
//count how many movements we need for the best solution
int numThreadsBetween = -1;
RRT_Node_And_Edge* currPtr = currNodes[finalIndClosest];
while (currPtr != NULL)
{
numThreadsBetween ++;
currPtr = currPtr->last_node;
}
std::cout << "num threads between: " << numThreadsBetween << std::endl;
//set the movements and intermediate threads for return
movements.resize(numThreadsBetween);
intermediateThread.resize(numThreadsBetween);
int movementInd = numThreadsBetween-1;
currPtr = currNodes[finalIndClosest];
while (movementInd >= 0)
{
movements[movementInd] = *(currPtr->motion_from_last);
intermediateThread[movementInd] = currPtr->this_node.thread;
currPtr->this_node.thread = NULL; //so we don't delete it!
movementInd--;
currPtr = currPtr->last_node;
}
/*intermediateThread.resize(currNodes.size());
for (int i=0; i < intermediateThread.size(); i++)
{
intermediateThread[i] = new Thread(currNodes[i]->this_node.thread);
}*/
//cleanup();
/*
movements.resize(5);
movements[0].pos_movement = Vector3d(7.0, -5.0, 1.0);
movements[0].tan_rotation = Eigen::AngleAxisd (M_PI/90.0, Vector3d(1.0, 0.0, 0.0));
movements[1].pos_movement = Vector3d(7.0, -4.0, 2.0);
movements[1].tan_rotation = Eigen::AngleAxisd (M_PI/100.0, Vector3d(1.0, 0.2, 0.0));
movements[2].pos_movement = Vector3d(6.0, -6.0, 3.0);
movements[2].tan_rotation = Eigen::AngleAxisd (M_PI/100.0, Vector3d(1.0, 0.0, 0.2));
movements[3].pos_movement = Vector3d(8.0, -4.0, 3.0);
movements[3].tan_rotation = Eigen::AngleAxisd (M_PI/80.0, Vector3d(1.0, 0.1, 0.1));
movements[4].pos_movement = Vector3d(6.0, -5.0, 2.0);
movements[4].tan_rotation = Eigen::AngleAxisd (M_PI/90.0, Vector3d(1.0, 0.1, -0.1));
intermediateThread.resize(0);
Thread* curr = new Thread(start);
Thread* next;
for (int i=0; i < movements.size(); i++)
{
next = movements[i].applyMotion(curr);
intermediateThread.push_back(next);
curr = next;
}
*/
}
void Thread_RRT::setThisGoalNearGoal(RRT_Node& goal, RRT_Node& goalThisIter)
{
//sample something in sphere between current point and goal
//first, set the things we know
Matrix4d startTrans;
goal.thread->getStartTransform(startTrans);
Vector3d posNewThread[2];
Vector3d tanNewThread[2];
double lengthNewThread[2];
double length_thread = goal.thread->length();
lengthNewThread[0] = lengthNewThread[1] = length_thread/2.0;
posNewThread[0] = startTrans.corner(Eigen::TopRight,3,1);
tanNewThread[0] = startTrans.corner(Eigen::TopLeft,3,1);
Vector3d endPos;
goal.thread->getWantedEndPosition(endPos);
Vector3d endTan;
goal.thread->getWantedEndTangent(endTan);
//random params for initializing
double curvatureNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT, randomNumUnit()*RANDOM_THREAD_MAX_CURVATURE_INIT};
double torsionNewThread[2] = {randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT, randomNumUnit()*RANDOM_THREAD_MAX_TORSION_INIT};
//find closest thread to goal
int indClosest;
findClosestNode(goal, indClosest);
Vector3d endPosClosestToGoal;
currNodes[indClosest]->this_node.thread->getWantedEndPosition(endPosClosestToGoal);
Vector3d endTanClosestToGoal;
currNodes[indClosest]->this_node.thread->getWantedEndTangent(endTanClosestToGoal);
//calculate the distance to this end
//sample from this radius, add to end point
double randX, randY, randZ, sampledRadius;
double radius_pos = (endPos - endPosClosestToGoal).norm() + ADD_TO_GOAL_DIST_RADIUS;
double dist_new_thread;
do
{
//find point in sphere with this radius, add to goal endpoint
//std::cout << "sampling end point" << std::endl;
double sampledRadius;
do
{
//std::cout << "sampling circle" << std::endl;
randX = randomMaxAbsValue(radius_pos);
randY = randomMaxAbsValue(radius_pos);
randZ = randomMaxAbsValue(radius_pos);
sampledRadius = randX*randX + randY*randY + randZ*randZ;
} while (sampledRadius >= (radius_pos*radius_pos));
posNewThread[1](0) = randX+endPos(0);
posNewThread[1](1) = randY+endPos(1);
posNewThread[1](2) = randZ+endPos(2);
dist_new_thread = (posNewThread[1]-posNewThread[0]).norm();
} while (dist_new_thread >= length_thread);
//sample from a tan around the goal tan
//sample from outside of sphere with radius equal to length between goal tan and closest tan
//normalize the length of that new tan
double radius_tan = (endTan - endTanClosestToGoal).norm() + ADD_TO_GOAL_TAN_RADIUS;
double randTheta = M_PI*randomNumUnit();
double randPhi = 2.0*M_PI*randomNumUnit();
tanNewThread[1](0) = endTan(0)+radius_tan*cos(randTheta)*sin(randPhi);
tanNewThread[1](1) = endTan(1)+radius_tan*sin(randTheta)*sin(randPhi);
tanNewThread[1](2) = endTan(2)+radius_tan*cos(randPhi);
tanNewThread[1].normalize();
// std::cout << "goal end pos: " << endPos << std::endl;
// std::cout << "goal end tan: " << endTan << std::endl;
// std::cout << "new end pos: " << posNewThread[1] << std::endl;
// std::cout << "new end tan: " << tanNewThread[1] << std::endl;
goalThisIter.thread = new Thread(goal.thread);
goalThisIter.thread->setEndConstraint(posNewThread[1], tanNewThread[1]);
// goalThisIter.thread->upsampleAndOptimize_minLength(0.065);
goalThisIter.thread->minimize_energy_fixedPieces();
//goalThisIter.thread = new Thread(curvatureNewThread, torsionNewThread, lengthNewThread, 2, posNewThread, tanNewThread);
//goalThisIter.thread = new Thread(length_thread, posNewThread[0], tanNewThread[0]);
//goalThisIter.thread->minimize_energy_fixedPieces();
goalThisIter.setPoints();
double distToGoal = goal.distanceToNode(goalThisIter);
//std::cout << "distance to actual goal: " << distToGoal << std::endl;
}
/**
* @function planBidirectionalRRT
* @brief Grows 2 RRT (Start and Goal)
*/
bool PathPlanner::planBidirectionalRrt( int _robotId,
const Eigen::VectorXi &_links,
const Eigen::VectorXd &_start,
const Eigen::VectorXd &_goal,
bool _connect,
bool _greedy, // no effect here
unsigned int _maxNodes ) {
// ============= YOUR CODE HERE ======================
// HINT: Remember trees grow towards each other!
RRT rrtF( world, _robotId, _links, _start, stepSize );
RRT rrtB( world, _robotId, _links, _goal, stepSize );
RRT::StepResult result = RRT::STEP_PROGRESS;
double smallestGap = DBL_MAX;
RRT *currentRRT;
RRT *otherRRT;
Eigen::VectorXd target;
if(_greedy)
{
std::cout << "Greedy has no effect in bidirectional" << std::endl;
}
while ( result != RRT::STEP_REACHED && smallestGap > stepSize ) {
//set rrt being expanded to the one with the fewest nodes
if(rrtF.getSize() > rrtB.getSize())//currentRRT == &rrtF)
{
currentRRT = &rrtB;
otherRRT = &rrtF;
} else
{
currentRRT = &rrtF;
otherRRT = &rrtB;
}
//compute node on other tree to expand toward
target = findClosestNode(currentRRT, otherRRT);
if( _connect ) {
// ================ YOUR CODE HERE ===============
if(rand()%5 == 0)
{
currentRRT->connect();
} else
{
currentRRT->connect(target);
}
// ===============================================
/** NO connect */
} else {
// ================== YOUR CODE HERE ===================
// =====================================================
if(rand()%5 == 0)
{
currentRRT->tryStep();
} else
{
currentRRT->tryStep(target);
}
}
target = findClosestNode(currentRRT, otherRRT);
if( _maxNodes > 0 && rrtF.getSize() + rrtB.getSize() > _maxNodes ) {
printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
return false;
}
double gap = currentRRT->getGap( target );
if( gap < smallestGap ) {
smallestGap = gap;
std::cout << "--> [planner] Gap: " << smallestGap << " Tree size forward: " << rrtF.configVector.size() << " backward: " << rrtB.configVector.size() << std::endl;
}
} // End of while
std::cout << rrtF.activeNode << " " << rrtB.activeNode << std::endl;
int acNode = rrtF.activeNode;
rrtF.activeNode = rrtF.getNearestNeighbor(findClosestNode(&rrtB, &rrtF));
if(acNode == rrtF.activeNode)
{
rrtB.activeNode = rrtB.getNearestNeighbor(findClosestNode(&rrtF, &rrtB));
}
std::cout << rrtF.activeNode << " " << rrtB.activeNode << std::endl;
printf(" --> Reached goal! : Gap: %.3f \n", currentRRT->getGap( target ) );
rrtF.tracePath( rrtF.activeNode, path, false );
rrtB.tracePath( rrtB.activeNode, path, true );
return true;
// ===================================================
}