#include "rrt.h"
//-----state-----
State::State()
{
this->x = this->y = 0.0;
}
State::State(double x, double y)
{
this->x = x;
this->y = y;
}
//-----block----
Block::Block()
{
from.x = from.y = -1;
blockwidth = blockheight = -1;
}
//-----move block-----
MoveBlock::MoveBlock()
{
from.x = from.y = -1;
blockwidth = blockheight = -1;
TimeStamp = -1;
}
//-----State info
AllInfo::AllInfo()
{
path.clear();
timeStamp.clear();
timeSeg.clear();
}
//-----TreeNode-----
TreeNode::TreeNode()
{
state.x = state.y = 0.0;
parent = NULL;
child = NULL;
sib = NULL;
}
TreeNode::TreeNode(TreeNode *parent, State *init)
{
this->parent = parent;
state.x = init->x;
state.y = init->y;
this->child = NULL;
this->sib = NULL;
}
//-----Tree-----
Tree::Tree()
{
root.state.x = root.state.y = 0.0;
root.parent = NULL;
root.child = NULL;
root.sib = NULL;
}
Tree::Tree(State s)
{
root.state.x = s.x;
root.state.y = s.y;
root.parent = NULL;
root.parent = NULL;
root.child = NULL;
root.sib = NULL;
}
void Tree::Extend(TreeNode *node, State *state)
{
if (node->child == NULL)
node->child = new TreeNode(node, state);
else
{
TreeNode *temp = node->child;
while(temp->sib != NULL)
temp = temp->sib;
temp->sib = new TreeNode(node, state);
}
}
double Tree::Distance(State *a, State *b)
{
return sqrt((a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y));
}
/*****************************************************/
/* Name: Tree::Search */
/* Function: Search for the nearest Tree Node to a */
/* target State */
/*****************************************************/
void Tree::Search(TreeNode *root, double *min, TreeNode **temp, State *target)
{
if (root->child == NULL)
{
double dist = Distance(&(root->state), target);
if (dist < (*min))
{
*min = dist;
*temp = root;
}
}
else
{
double dist;
TreeNode *tempNode = root->child;
while(tempNode)
{
Search(tempNode, min, temp, target);
tempNode = tempNode->sib;
}
dist = Distance(&(root->state), target);
if (dist < (*min))
{
*min = dist;
*temp = root;
}
}
}
void Tree::Trace(TreeNode *source, TreeNode *target)
{
TraceList.clear();
TraceCount = 0;
TreeNode *temp = source;
while (temp != target)
{
TraceList.push_back(*temp);
temp = temp->parent;
TraceCount++;
}
TraceList.push_back(*temp);
}
void Tree::Delete(TreeNode *realRoot, TreeNode *root)
{
if (root->child == NULL)
{
if (root != realRoot)
delete root;
}
else
{
TreeNode *temp = root->child;
TreeNode *tempNode = temp->sib;
if (tempNode == NULL)
Delete(realRoot, temp);
else
{
while (tempNode)
{
Delete(realRoot, temp);
temp = tempNode;
tempNode = temp->sib;
}
Delete(realRoot, temp);
}
if (root != realRoot)
delete root;
}
}
void Tree::ReSet(State *newRoot)
{
Delete(&root, &root);
root.parent = NULL;
root.child = NULL;
root.sib = NULL;
root.state.x = newRoot->x;
root.state.y = newRoot->y;
this->TraceList.clear();
}
//-----rrt-----
//the interface of rrt.
RRT::RRT(string zoneFile)
{
//initiliase some parameter before run this algorithm
Width = -1;
Height = -1;
Targets.clear();
Map.clear();
Blocks.clear();
NodeCache.clear();
BuildTime = 1;
FileName = zoneFile;
TimeStamp = 0;
//interpret the input map;
Interpret(zoneFile);
#ifdef DRAW
InitRRT(NULL);
cvNamedWindow("zone");
cvShowImage("zone", pic);
cvWaitKey(0);
cvDestroyWindow("zone");
cvReleaseImage(&pic);
#endif
//real-time planning the routine
Plan();
printOut();
}
void RRT::Interpret(string zoneFile)
{
ifstream in;
in.open(zoneFile.c_str());
string label;
while (in >> label)
{
if (label == "width")
{
in >> Width;
if (Height != -1)
{
for (int i = 0; i < Width * Height; i++)
Map.push_back(0);
}
}
else if (label == "height")
{
in >> Height;
if (Width != -1)
{
for (int i = 0; i < Width * Height; i++)
Map.push_back(0);
}
}
else if (label == "entry")
{
in >> Start.x;
in >> Start.y;
Map[Start.y * Width + Start.x] = ENTRY_POINT;
RRTTree.root.state.x = Start.x;
RRTTree.root.state.y = Start.y;
}
else if (label == "exit")
{
in >> Target.x;
in >> Target.y;
Map[Target.y * Width + Target.x] = EXIT_POINT;
}
else if (label == "step")
{
in >> Step;
}
else if (label == "padding")
{
in >> Padding;
}
else if (label == "goalprob")
{
in >> GoalProb;
}
else if (label == "errtprob")
{
in >> ERRTProb;
}
else if (label == "threshold")
{
in >> Threshold;
}
else if (label == "checkpoint")
{
State temp;
in >> temp.x;
in >> temp.y;
Targets.push_back(temp);
Map[temp.y * Width + temp.x] = CHECK_POINT;
}
else if (label == "moving")
{
MoveBlock temp;
in >> temp.from.x;
in >> temp.from.y;
in >> temp.blockwidth;
in >> temp.blockheight;
in >> temp.TimeStamp;
in >> temp.block_z;
MBS.push_back(temp);
}
else if (label == "block")
{
Block temp;
in >> temp.from.x;
in >> temp.from.y;
in >> temp.blockwidth;
in >> temp.blockheight;
in >> temp.block_z;
Blocks.push_back(temp);
for (int i = temp.from.x; i < temp.from.x + temp.blockwidth; i++)
{
for (int j = temp.from.y; j < temp.from.y + temp.blockheight; j++)
{
Map[j * Width + i] = BLOCK_POINT;
}
}
}
else
{
cout << "Invaild data file!\n";
exit(1);
}
}
Targets.push_back(Target);
in.close();
}
//init the picture which show the result of the rrt algorithm
void RRT::InitRRT(State *Point)
{
pic = cvCreateImage(cvSize((int)Width, (int)Height), 8, 3);
for (int i = 0; i < pic->imageSize; i++)
pic->imageData[i] = -1;
vector<State>::iterator ii;
for (ii = Targets.begin(); ii != Targets.end(); ii++)
cvCircle(pic, cvPoint(ii->x, ii->y), Threshold, cvScalar(238, 151, 87), -1, 8, 0);
vector<MoveBlock>::iterator iptr;
for (iptr = MBS.begin(); iptr != MBS.end(); iptr++)
{
#ifdef PRINTOUT
cout << iptr->TimeStamp << endl;
cout << TimeStamp << endl;
#endif
if (iptr->TimeStamp == TimeStamp)
{
#ifdef PRINTOUT
cout << "==TimeStamp" << endl;
#endif
cvRectangle(pic, cvPoint(iptr->from.x, iptr->from.y), cvPoint(iptr->from.x+iptr->blockwidth, iptr->from.y+iptr->blockheight), cvScalar(150,150,150), CV_FILLED, 8, 0);
}
}
vector<Block>::iterator iii;
for (iii = Blocks.begin(); iii != Blocks.end(); iii ++)
cvRectangle(pic, cvPoint(iii->from.x, iii->from.y), cvPoint(iii->from.x + iii->blockwidth, iii->from.y + iii->blockheight), cvScalar(0,0,0), CV_FILLED, 8, 0);
cvCircle(pic, cvPoint(Start.x, Start.y), Threshold, cvScalar(92, 92, 252), -1, 8, 0);
cvCircle(pic, cvPoint(Target.x, Target.y), Threshold, cvScalar(92, 92, 252), -1, 8, 0);
if (Point != NULL)
cvCircle(pic, cvPoint(Point->x, Point->y), Threshold, cvScalar(232, 94, 184), -1, 8, 0);
}
//calculate eucliean distance
double RRT::Distance(State *a, State *b)
{
return sqrt((a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y));
}
//uniform distribution
double RRT::RandomReal()
{
return ((double)rand()) / RAND_MAX;
}
//get a random state in a comtraint domain
State RRT::RandomState()
{
State temp;
double r = RandomReal();
temp.x = (int)(Padding + (Width - Padding * 2) * r);
r = RandomReal();
temp.y = (int)(Padding + (Height - Padding * 2) * r);
return temp;
}
//choose target state according the random probability
State RRT::ChooseTarget()
{
double p = RandomReal();
if (0 <= p && p < GoalProb)
return Target;
else if (GoalProb <= p && p <= ERRTProb)
{
