e595dbc88e
This was SVN commit r5393.
1546 lines
48 KiB
C++
1546 lines
48 KiB
C++
//Search.cpp
|
|
|
|
//DJD: Abstract space searching function definitions {
|
|
|
|
#include <SE/se_dcdt.h>
|
|
|
|
#include <math.h>
|
|
#include <queue>
|
|
|
|
//functions for comparing SearchNodes
|
|
bool operator <(SearchNode sn1, SearchNode sn2)
|
|
{
|
|
//compares the f-values, but switches them so the lowest value
|
|
//is kept at the start of the queue instead of the greatest
|
|
return (sn1.f() < sn2.f());
|
|
}
|
|
/*
|
|
bool operator <(SearchNode *sn1, SearchNode *sn2)
|
|
{
|
|
return ((sn1->f() < sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() < sn2->h())));
|
|
}
|
|
*/
|
|
struct check : public std::binary_function <SearchNode *, SearchNode *, bool>
|
|
{
|
|
bool operator()(SearchNode *&sn1, SearchNode *&sn2) const
|
|
{
|
|
return ((sn1->f() > sn2->f()) || ((sn1->f() == sn2->f()) && (sn1->h() > sn2->h())));;
|
|
}
|
|
};
|
|
|
|
//functions for comparing SearchNodes
|
|
bool operator ==(SearchNode sn1, SearchNode sn2)
|
|
{
|
|
//compares the f-values, but switches them so the lowest value
|
|
//is kept at the start of the queue instead of the greatest
|
|
return (sn1.Triangle() == sn2.Triangle());
|
|
}
|
|
|
|
bool SeDcdt::IDA(SrArray<SearchNode *> startNodes, SrArray<SearchNode *> goalNodes,
|
|
SearchNode* &goal, float &depthBound, float r)
|
|
{
|
|
//TODO: actual search!
|
|
return false;
|
|
}
|
|
|
|
bool SeDcdt::IDA(SrArray<SearchNode *> startNodes, SrArray<SearchNode *> goalNodes, SrArray<SeBase *>& path, float r)
|
|
{
|
|
SrArray<SearchNode *> copy;
|
|
float depthBound = INFINITY;
|
|
for (int i = 0; i < startNodes.size(); i++)
|
|
{
|
|
if (startNodes[i]->f() < depthBound)
|
|
{
|
|
depthBound = startNodes[i]->f();
|
|
}
|
|
}
|
|
SearchNode *goal = NULL;
|
|
while (true)
|
|
{
|
|
copy.size(0);
|
|
for (int i = 0; i < startNodes.size(); i++)
|
|
{
|
|
copy.push() = startNodes[i];
|
|
}
|
|
if (!IDA(copy, goalNodes, goal, depthBound, r) || (goal != NULL))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
path.size(0);
|
|
if (goal == NULL)
|
|
{
|
|
return false;
|
|
}
|
|
SrPolygon tempPath;
|
|
SrArray<SeBase *> channel;
|
|
while (goal != NULL)
|
|
{
|
|
path.push() = goal->Triangle()->se();
|
|
SearchNode *temp = goal;
|
|
goal = goal->Back();
|
|
delete temp;
|
|
}
|
|
path.revert();
|
|
return true;
|
|
}
|
|
|
|
//gets the closest point on an unconstrained edge of the triangle to the point given
|
|
//(that is at least r from a vertex) . . . this function can be sped up
|
|
SrPnt2 SeDcdt::ClosestPointTo(SeDcdtFace *face, float x, float y, float r)
|
|
{
|
|
if (InTriangle(face, x, y))
|
|
{
|
|
SrPnt2 p;
|
|
p.set(x, y);
|
|
return p;
|
|
}
|
|
SrPnt2 closestPoint, point;
|
|
float closestDistance = INFINITY, distance;
|
|
SeBase *s = face->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
float x1, y1, x2, y2, x3, y3;
|
|
TriangleVertices(s, x1, y1, x2, y2, x3, y3);
|
|
if (Blocked(s))
|
|
{
|
|
s = s->nxt();
|
|
continue;
|
|
}
|
|
bool accute1 = IsAccute(AngleBetween(x1, y1, x2, y2, x, y));
|
|
bool accute2 = IsAccute(AngleBetween(x2, y2, x1, y1, x, y));
|
|
if (accute1 && accute2)
|
|
{
|
|
//get closest point on this line
|
|
distance = PointLineDistance(x, y, x1, y1, x2, y2);
|
|
float theta = atan2(y2 - y1, x2 - x1);
|
|
theta += (theta > PI / 2.0f) ? -3.0f * PI / 2.0f : PI / 2.0f;
|
|
point.set(x + cos(theta) * distance, y + sin(theta) * distance);
|
|
if (Length(point.x, point.y, x1, y1) < r)
|
|
{
|
|
accute2 = false;
|
|
}
|
|
else if (Length(point.x, point.y, x2, y2) < r)
|
|
{
|
|
accute1 = false;
|
|
}
|
|
}
|
|
if (!accute1 && accute2)
|
|
{
|
|
//get point distance r from (x2, y2) in the direction of (x1, y1)
|
|
float theta = atan2(y1 - y2, x1 - x2);
|
|
point.set(x2 + cos(theta) * r, y2 + sin(theta) * r);
|
|
}
|
|
else if (accute1 && !accute2)
|
|
{
|
|
//get point distance r from (x1, y1) in the direction of (x2, y2)
|
|
float theta = atan2(y2 - y1, x2 - x1);
|
|
point.set(x1 + cos(theta) * r, y1 + sin(theta) * r);
|
|
}
|
|
distance = Length(point.x, point.y, x, y);
|
|
if (distance < closestDistance)
|
|
{
|
|
closestPoint.set(point.x, point.y);
|
|
closestDistance = distance;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
return closestPoint;
|
|
}
|
|
|
|
//walks from the degree-1 source triangle to the degree-2 destination rectangle at the root of the tree
|
|
bool SeDcdt::WalkBetween(SrArray<SeBase *>& path, SeDcdtFace *sourceFace, SeDcdtFace *destinationFace, float r, int direction)
|
|
{
|
|
SeDcdtFace *last = NULL;
|
|
//go through all faces between the source and destination faces
|
|
while (true)
|
|
{
|
|
//if we have reached the destination face,
|
|
if (sourceFace == destinationFace)
|
|
{
|
|
//add it to the path and exit successfully
|
|
path.push() = destinationFace->se();
|
|
return true;
|
|
}
|
|
//get the elements of the current triangle
|
|
SeBase *s = sourceFace->se();
|
|
//go through the edges of the triangle
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
//if the destination face is across the current edge,
|
|
if ((i == direction) || ((direction == INVALID)
|
|
&& (sourceFace->link->Adjacent(i) == destinationFace) && (s->sym()->fac() != last)))
|
|
{
|
|
last = sourceFace;
|
|
direction = INVALID;
|
|
//make sure the path is wide enough
|
|
if (sourceFace->link->Choke(i) < 2.0f * r)
|
|
{
|
|
//if not, empty the path and return failure
|
|
path.size(0);
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
//if so, add the current face to the path
|
|
path.push() = sourceFace->se();
|
|
//and move to the next one
|
|
sourceFace = (SeDcdtFace *)s->sym()->fac();
|
|
float x, y;
|
|
TriangleMidpoint(sourceFace, x, y);
|
|
break;
|
|
}
|
|
}
|
|
//move to the next edge in the current triangle
|
|
s = s->nxt();
|
|
}
|
|
}
|
|
}
|
|
|
|
//checks if a unit of radius r can go through the second triangle between the first and the third
|
|
bool SeDcdt::CanCross(SeBase *first, SeBase *second, SeBase *third, float r)
|
|
{
|
|
//and goes through its edges
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
//gets the face across the current edge
|
|
SeFace *current = second->sym()->fac();
|
|
//if it is neither the first nor the third
|
|
if ((current != first->fac()) && (current != third->fac()))
|
|
{
|
|
//return if the width between the first and the third is large enough for radius r
|
|
return (((SeDcdtFace *)second)->link->Width((i + 1) % 3) >= 2.0f * r);
|
|
}
|
|
//move to the next edge
|
|
second = second->nxt();
|
|
}
|
|
sr_out.warning("ERROR: could not find desired face (should never happen)\n");
|
|
return false;
|
|
}
|
|
|
|
//finds a path in the degree-1 tree with both the start and goal in it
|
|
bool SeDcdt::Degree1Path(SrArray<SeBase *>& path, float x1, float y1, SeDcdtFace *startFace,
|
|
float x2, float y2, SeDcdtFace *goalFace, float r)
|
|
{
|
|
//starts marking the mesh
|
|
_mesh->begin_marking();
|
|
//create a priority queue for searching the tree
|
|
// std::priority_queue<SearchNode> pq;
|
|
std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> pq;
|
|
float x, y;
|
|
//each face is represented by its midpoint (since only one path must exist)
|
|
TriangleMidpoint(startFace, x, y);
|
|
SrPnt2 p;
|
|
p.set(x, y);
|
|
//creates the first search node
|
|
SearchNode *current = new SearchNode(Length(x, y, x1, y1), Length(x, y, x2, y2), p, startFace, NULL);
|
|
//put it in the queue to be searched
|
|
pq.push(current);
|
|
//continues until the tree has been exhausted or a path has been found
|
|
while (!pq.empty())
|
|
{
|
|
//gets the first node to search
|
|
/*SearchNode */current = pq.top();
|
|
pq.pop();
|
|
//checks if that node is the goal
|
|
if (InTriangle(current->Triangle(), x2, y2))
|
|
{
|
|
while (!pq.empty())
|
|
{
|
|
pq.top()->Close();
|
|
delete pq.top();
|
|
pq.pop();
|
|
}
|
|
SearchNode *temp = current;
|
|
//if so, visits all the triangles between the start and the goal
|
|
while (current != NULL)
|
|
{
|
|
//and adds each to the path
|
|
path.push() = current->Triangle()->se();
|
|
//until the first node is reached
|
|
// if (current->Back() == NULL)
|
|
// {
|
|
// break;
|
|
// }
|
|
//moves to the previous triangle
|
|
current = current->Back();
|
|
}
|
|
temp->Close();
|
|
delete temp;
|
|
//since they were visited in reverse, flip the path around
|
|
path.revert();
|
|
path.pop();
|
|
//return a path was successfully found
|
|
// return true;
|
|
_mesh->end_marking();
|
|
if (ValidPath(path, x1, y1, x2, y2, r))
|
|
{
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
path.size(0);
|
|
return false;
|
|
}
|
|
}
|
|
//gets the triangle associated with the current node
|
|
SeDcdtFace *currentFace = current->Triangle();
|
|
//marks it as visited
|
|
_mesh->mark(currentFace);
|
|
//get the elements of the triangle
|
|
SeBase *s = currentFace->se();
|
|
//go through the edges
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
//gets the face on the opposite side of this edge
|
|
SeDcdtFace *tempFace = (SeDcdtFace *)s->sym()->fac();
|
|
//only consider it if it's degree-1 and hasn't been visited before
|
|
if ((!_mesh->marked(tempFace)) && (tempFace->link->Degree() == 1) && (!Blocked(s)))
|
|
{
|
|
/*
|
|
//make sure the current triangle is wide enough
|
|
float width;
|
|
//if it's the first triangle, assume it is for now
|
|
if (current.Back() == NULL)
|
|
{
|
|
width = INFINITY;
|
|
}
|
|
//otherwise, get the width through this triangle between
|
|
//the last triangle and the next one
|
|
else
|
|
{
|
|
if (s->nxt()->sym()->fac() == current.Back()->Triangle())
|
|
{
|
|
width = currentFace->link->Width(i);
|
|
}
|
|
else
|
|
{
|
|
width = currentFace->link->Width((i + 2) % 3);
|
|
}
|
|
}
|
|
//only add this triangle if the path is wide enough
|
|
if (width >= 2.0f * r)
|
|
{
|
|
*/
|
|
//get the new triangle's midpoint
|
|
TriangleMidpoint(tempFace, x, y);
|
|
p.set(x, y);
|
|
//create a search node corresponding to this triangle
|
|
SearchNode *temp = new SearchNode(current->g() + Length(current->Point().x, current->Point().y, x, y),
|
|
Length(x, y, x2, y2), p, tempFace, current);
|
|
//and add it to the search queue
|
|
pq.push(temp);
|
|
current->OpenChild();
|
|
// }
|
|
}
|
|
//move to the next edge
|
|
s = s->nxt();
|
|
}
|
|
if (current->OpenChildren() <= 0)
|
|
{
|
|
current->Close();
|
|
delete current;
|
|
}
|
|
}
|
|
//stop marking the mesh
|
|
_mesh->end_marking();
|
|
//the tree was exhausted and a path not found - return failure
|
|
return false;
|
|
}
|
|
|
|
//follows a degree-2 ring from one node to another, making sure that each is wide enough for the given radius
|
|
bool SeDcdt::Degree2Path(SrArray<SeBase *>& path, SeDcdtFace *startFace, SeDcdtFace *nextFace,
|
|
SeDcdtFace *goalFace, float r)
|
|
{
|
|
//empties the path first
|
|
path.size(0);
|
|
//goes until it has reached the goal triangle
|
|
while (nextFace != goalFace)
|
|
{
|
|
//gets the elements of the current triangle
|
|
SeBase *s = nextFace->se();
|
|
//goes through its edges
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
//when the face across the current edge is the last triangle,
|
|
if (s->sym()->fac() == startFace)
|
|
{
|
|
//check if the triangle across the next edge is the next triangle in the ring
|
|
if (nextFace->link->Angle((i + 1) % 3) == INFINITY)
|
|
{
|
|
//makes sure this triangle is wide enough for radius r
|
|
if (nextFace->link->Width(i) < 2.0f * r)
|
|
{
|
|
//if not, empties the path and returns failure
|
|
path.size(0);
|
|
return false;
|
|
}
|
|
//otherwise, adds the current triangle to the path
|
|
path.push() = nextFace->se();
|
|
//and moves to the next triangle in the ring
|
|
startFace = nextFace;
|
|
nextFace = (SeDcdtFace *)s->nxt()->sym()->fac();
|
|
}
|
|
else
|
|
{
|
|
//similar to above, checks the width
|
|
if (nextFace->link->Width((i + 2) % 3) < 2.0f * r)
|
|
{
|
|
path.size(0);
|
|
return false;
|
|
}
|
|
//and continues along the ring
|
|
path.push() = nextFace->se();
|
|
startFace = nextFace;
|
|
nextFace = (SeDcdtFace *)s->nxt()->nxt()->sym()->fac();
|
|
}
|
|
//process the next triangle in the ring
|
|
break;
|
|
}
|
|
//move to the next edge
|
|
s = s->nxt();
|
|
}
|
|
}
|
|
//return that a path was found successfully
|
|
return true;
|
|
}
|
|
|
|
//determines if the given endpoint has to be checked against the current triangle
|
|
bool SeDcdt::ValidEndpoint(SeBase *s, float x, float y, bool left)
|
|
{
|
|
//gets the vertices of the current triangle
|
|
float x1, y1, x2, y2, x3, y3, x4, y4;
|
|
TriangleVertices(s, x1, y1, x2, y2, x3, y3);
|
|
//checks if a base angle is obtuse
|
|
if ((abs(AngleBetween(x1, y1, x2, y2, x3, y3)) >= PI / 2.0f)
|
|
|| (abs(AngleBetween(x1, y1, x3, y3, x2, y2)) >= PI / 2.0f))
|
|
{
|
|
//if so, we don't have to worry
|
|
return true;
|
|
}
|
|
//gets the angle of the opposite edge
|
|
float theta = atan2(y3 - y2, x3 - x2);
|
|
//turn 90 degrees clockwise
|
|
theta += (theta <= PI / -2.0f) ? 3.0f * PI / 2.0f : PI / -2.0f;
|
|
//create a point this angle from the "top" vertex
|
|
x4 = x1 + cos(theta);
|
|
y4 = y1 + sin(theta);
|
|
//returns whether the unit doesn't have to pass through the narrowest point in this triangle
|
|
return ((Orientation(x1, y1, x4, y4, x, y) > 0) == (left));
|
|
}
|
|
|
|
//checks whether there is a valid path from the start or goal through their corresponding triangles
|
|
bool SeDcdt::ValidEndpoint(SeDcdtFace *first, SeDcdtFace *second, float x, float y, float r)
|
|
{
|
|
SeBase *s = first->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (s->sym()->fac() == second)
|
|
{
|
|
// return (!(((!ValidEndpoint(s, x, y, false)) && (first->link->Width((i + 2) % 3) < 2.0f * r))
|
|
// || ((!ValidEndpoint(s->nxt(), x, y, true)) && (first->link->Width(i) < 2.0f * r))));
|
|
return (((ValidEndpoint(s, x, y, false)) || (first->link->Width((i + 2) % 3) >= 2.0f * r))
|
|
&& ((ValidEndpoint(s->nxt(), x, y, true)) || (first->link->Width(i) >= 2.0f * r)));
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
//checks if the start and goal points can validly enter the path
|
|
bool SeDcdt::ValidEndpoints(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r)
|
|
{
|
|
// return ((path.size() > 1) && (ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r))
|
|
// && (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r)));
|
|
return ((path.size() < 2) || ((ValidEndpoint((SeDcdtFace *)path[0]->fac(), (SeDcdtFace *)path[1]->fac(), x1, y1, r))
|
|
&& (ValidEndpoint((SeDcdtFace *)path[path.size() - 1]->fac(), (SeDcdtFace *)path[path.size() - 2]->fac(), x2, y2, r))));
|
|
}
|
|
|
|
//checks if a path is valid for a unit of radius r
|
|
bool SeDcdt::ValidPath(SrArray<SeBase *>& path, float x1, float y1, float x2, float y2, float r)
|
|
{
|
|
//first checks if the endpoints are valid
|
|
if (!ValidEndpoints(path, x1, y1, x2, y2, r))
|
|
{
|
|
return false;
|
|
}
|
|
//then goes through the interior points
|
|
for (int i = 1; i < path.size() - 1; i++)
|
|
{
|
|
//gets the elements of the current triangle in the path
|
|
SeBase *s = path[i]->fac()->se();
|
|
//goes through the edges in this triangle
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
//find the edge across which is the next triangle in the path
|
|
if (s->sym()->fac() == path[i + 1]->fac())
|
|
{
|
|
//get the width through this triangle between the last one in the path and the next one
|
|
float width = ((SeDcdtFace *)s->fac())->link->Width(
|
|
(s->nxt()->sym()->fac() == path[i - 1]->fac()) ? j : ((j + 2) % 3));
|
|
//if it's less than the diameter of the unit, the path is invalid
|
|
if (width < 2.0f * r)
|
|
{
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
//moves to the next edge in the triangle
|
|
s = s->nxt();
|
|
}
|
|
}
|
|
//if no triangle in the path was invalid, the path is valid
|
|
return true;
|
|
}
|
|
|
|
void SeDcdt::ConstructBasePath(SrArray<SeBase *> &path, SearchNode *goalNode, SeDcdtFace *start, SeDcdtFace *goal, int direction)
|
|
{
|
|
SrArray<SeBase *> backwardsPath;
|
|
SeDcdtFace *goal2;
|
|
if (goal->link->Degree() == 1)
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (goal->link->Adjacent(i) != NULL)
|
|
{
|
|
WalkBetween(backwardsPath, goal, goal->link->Adjacent(i), 0.0f);
|
|
backwardsPath.pop();
|
|
// WalkBetween(backwardsPath, goal->link->Adjacent(i), goalNode->Triangle(), 0.0f);
|
|
// backwardsPath.pop();
|
|
goal2 = goal->link->Adjacent(i);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
goal2 = goal;
|
|
}
|
|
if (goal->link->Degree() < 3)
|
|
{
|
|
WalkBetween(backwardsPath, goal2, goalNode->Triangle(), 0.0f, direction);
|
|
backwardsPath.pop();
|
|
}
|
|
float x, y;
|
|
while (goalNode->Back() != NULL)
|
|
{
|
|
TriangleMidpoint(goalNode->Triangle(), x, y);
|
|
sr_out << "SearchNode at (" << x << ", " << y << ")\n";
|
|
SrArray<SeBase *> tempPath;
|
|
WalkBetween(tempPath, goalNode->Back()->Triangle(), goalNode->Triangle(), 0.0f, goalNode->Direction());
|
|
while (!tempPath.empty())
|
|
{
|
|
backwardsPath.push() = tempPath.pop();
|
|
}
|
|
// WalkBetween(backwardsPath, goalNode->Triangle(), goalNode->Back()->Triangle(), 0.0f);
|
|
backwardsPath.pop();
|
|
goalNode = goalNode->Back();
|
|
}
|
|
TriangleMidpoint(goalNode->Triangle(), x, y);
|
|
sr_out << "SearchNode at (" << x << ", " << y << ")\n";
|
|
SeDcdtFace *start2;
|
|
if (start->link->Degree() == 1)
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (start->link->Adjacent(i) != NULL)
|
|
{
|
|
WalkBetween(path, start, start->link->Adjacent(i), 0.0f);
|
|
path.pop();
|
|
// WalkBetween(path, start->link->Adjacent(i), goalNode->Triangle(), 0.0f);
|
|
start2 = start->link->Adjacent(i);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
start2 = start;
|
|
}
|
|
if (start->link->Degree() < 3)
|
|
{
|
|
WalkBetween(path, start2, goalNode->Triangle(), 0.0f, goalNode->Direction());
|
|
}
|
|
while (!backwardsPath.empty())
|
|
{
|
|
path.push() = backwardsPath.pop();
|
|
}
|
|
path.pop();
|
|
/*
|
|
float x, y;
|
|
sr_out << "\n{";
|
|
for (int i = 0; i < path.size() - 1; i++)
|
|
{
|
|
TriangleMidpoint((SeDcdtFace *)path[i]->fac(), x, y);
|
|
sr_out << "(" << x << ", " << y << "), ";
|
|
}
|
|
TriangleMidpoint((SeDcdtFace *)path[path.size() - 1], x, y);
|
|
sr_out << "(" << x << ", " << y << ")}\n\n";
|
|
*/
|
|
}
|
|
|
|
//searches the abstract graph for a path between two points
|
|
bool SeDcdt::SearchPath(/*SrArray<SeBase *>& path,*/ float x1, float y1, float x2, float y2, float r, int update)
|
|
{
|
|
sr_out << "SEARCH PATH STARTED:\n";
|
|
//initialize the path to empty
|
|
currentPath.size(0);
|
|
SeBase *start, *goal;
|
|
//find the triangle containing the start point
|
|
SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start);
|
|
//if it wasn't found, return a path could not be found
|
|
if (startResult == SeTriangulator::LocateResult::NotFound)
|
|
{
|
|
return false;
|
|
}
|
|
//find the triangle containing the goal point
|
|
SeTriangulator::LocateResult goalResult = LocatePoint(x2, y2, goal);
|
|
//if it couldn't, return a failure
|
|
if (goalResult == SeTriangulator::LocateResult::NotFound)
|
|
{
|
|
return false;
|
|
}
|
|
SeDcdtFace *startFace = (SeDcdtFace *)start->fac();
|
|
SeDcdtFace *goalFace = (SeDcdtFace *)goal->fac();
|
|
sr_out << "START AND GOAL FACES FOUND\n";
|
|
//if either triangle isn't abstracted, this search fails
|
|
//also if the triangles aren't in the same connected component, no path connecting them exists
|
|
if ((startFace->link == NULL) || (goalFace->link == NULL)
|
|
|| (startFace->link->Component() != goalFace->link->Component()))
|
|
{
|
|
sr_out << ((startFace->link == NULL) ? "start face not abstracted" : (goalFace->link == NULL) ?
|
|
"goal face not abstracted" : "start and goal in different components") << srnl;
|
|
return false;
|
|
}
|
|
sr_out << "START AND GOAL ARE ABSTRACTED AND IN THE SAME COMPONENT\n";
|
|
//check that the start and goal points aren't within the radius of any constraints
|
|
if ((!ValidPosition(x1, y1, startFace, r)) || (!ValidPosition(x2, y2, goalFace, r)))
|
|
{
|
|
//if either are, return false
|
|
return false;
|
|
}
|
|
//if the start and goal are in the same triangle,
|
|
if (startFace == goalFace)
|
|
{
|
|
//the path is a straight line
|
|
currentPath.push().set(x1, y1);
|
|
currentPath.push().set(x2, y2);
|
|
return true;
|
|
}
|
|
sr_out << "START AND GOAL ARE VALID POSITIONS AND NOT IN THE SAME TRIANGLE\n";
|
|
//if both start and goal faces are degree-1,
|
|
if ((startFace->link->Degree() == 1) && (goalFace->link->Degree() == 1))
|
|
{
|
|
//checks if the start (and thus the goal as well) is in a tree component
|
|
bool inTree = true;
|
|
//(true if there are no adjacent degree-2 nodes)
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (startFace->link->Adjacent(i) != NULL)
|
|
{
|
|
inTree = false;
|
|
}
|
|
}
|
|
//if they are in a tree component,
|
|
if (inTree)
|
|
{
|
|
//perform search in it
|
|
SrArray<SeBase *> channel;
|
|
if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r))
|
|
{
|
|
GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
sr_out << "START AND GOAL ARE NOT IN A TREE\n";
|
|
//if the start face is degree-1, moves to the adjacent degree-2 node
|
|
SeDcdtFace *startFace2;
|
|
float startAngle;
|
|
float startChoke;
|
|
if (startFace->link->Degree() == 1)
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
SeDcdtFace *adjacent = startFace->link->Adjacent(i);
|
|
if (adjacent != NULL)
|
|
{
|
|
startFace2 = adjacent;
|
|
startAngle = startFace->link->Angle(i);
|
|
startChoke = startFace->link->Choke(i);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
startFace2 = startFace;
|
|
startAngle = 0.0f;
|
|
startChoke = INFINITY;
|
|
}
|
|
//if the goal face is degree-1, moves to the adjacent degree-2 node
|
|
SeDcdtFace *goalFace2;
|
|
float goalAngle;
|
|
float goalChoke;
|
|
if (goalFace->link->Degree() == 1)
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
SeDcdtFace *adjacent = goalFace->link->Adjacent(i);
|
|
if (adjacent != NULL)
|
|
{
|
|
goalFace2 = adjacent;
|
|
goalAngle = goalFace->link->Angle(i);
|
|
goalChoke = goalFace->link->Choke(i);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
goalFace2 = goalFace;
|
|
goalAngle = 0.0f;
|
|
goalChoke = INFINITY;
|
|
}
|
|
//if the start and goal faces are both degree-1 or 2,
|
|
if ((startFace->link->Degree() < 3) && (goalFace->link->Degree() < 3))
|
|
{
|
|
//checks if the start and goal have the same root,
|
|
if (startFace2 == goalFace2)
|
|
{
|
|
//if the start face is at the root of the tree that the goal face is in,
|
|
if (startFace2 == startFace)
|
|
{
|
|
SrArray<SeBase *> channel;
|
|
//walk from the goal face to the start face
|
|
if (WalkBetween(channel, goalFace, startFace, r))
|
|
{
|
|
channel.revert();
|
|
channel.pop();
|
|
GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
//if the goal face is at the root of the tree that the start face is in,
|
|
else if (goalFace2 == goalFace)
|
|
{
|
|
SrArray<SeBase *> channel;
|
|
//walk from the start face to the goal face
|
|
if (WalkBetween(channel, startFace, goalFace, r))
|
|
{
|
|
channel.pop();
|
|
GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
//if the start and goal are both in the same tree component,
|
|
else
|
|
{
|
|
SrArray<SeBase *> channel;
|
|
//perform search in tree component
|
|
if (Degree1Path(channel, x1, y1, startFace, x2, y2, goalFace, r))
|
|
{
|
|
GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
sr_out << "ONE IS NOT AT THE ROOT OF THE OTHER\n";
|
|
//if we can't get from the start or goal to the rest of the graph, no path exists
|
|
if ((startChoke < 2.0f * r) || (goalChoke < 2.0f * r))
|
|
{
|
|
sr_out << "startChoke = " << startChoke << ", goalChoke = " << goalChoke << srnl;
|
|
return false;
|
|
}
|
|
sr_out << "START AND GOAL CAN GET ON TO THE GRAPH\n";
|
|
//TODO: if the nodes are both in a degree-2 ring, walk from the start to the goal (both ways)
|
|
//TODO: checking that the path is wide enough at all times
|
|
//TODO: if both paths are wide enough, run the funnel algorithm and take the shorter path
|
|
//TODO: if only one is wide enough, take that, and if neither are, return false
|
|
if ((startFace2->link->Degree() == 2) && (goalFace2->link->Degree() == 2))
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (startFace2->link->Adjacent(i) != NULL)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
if (i >= 3)
|
|
{
|
|
//degree-2 ring!
|
|
SrArray<SeBase *> startPath, goalPath, leftPath, rightPath;
|
|
if (startFace == startFace2)
|
|
{
|
|
startPath.size(0);
|
|
}
|
|
else if (!WalkBetween(startPath, startFace, startFace2, r))
|
|
{
|
|
return false;
|
|
}
|
|
if (goalFace == goalFace2)
|
|
{
|
|
goalPath.size(0);
|
|
}
|
|
else if (!WalkBetween(goalPath, goalFace, goalFace2, r))
|
|
{
|
|
return false;
|
|
}
|
|
goalPath.revert();
|
|
SeBase *s = startFace2->se();
|
|
bool leftValid, rightValid;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (startFace2->link->Angle(i) == INVALID)
|
|
{
|
|
rightValid = Degree2Path(rightPath, startFace2, (SeDcdtFace *)s->nxt()->sym()->fac(), goalFace2, r);
|
|
leftValid = Degree2Path(leftPath, startFace2, (SeDcdtFace *)s->nxt()->nxt()->sym()->fac(), goalFace2, r);
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
if (!leftValid && !rightValid)
|
|
{
|
|
return false;
|
|
}
|
|
SrArray<SeBase *> fullLeftPath, fullRightPath;
|
|
fullLeftPath.size(0);
|
|
fullRightPath.size(0);
|
|
if (leftValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], leftPath[0], r)
|
|
&& CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], leftPath[leftPath.size() - 1], r))
|
|
{
|
|
for (int i = 0; i < startPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = startPath[i];
|
|
}
|
|
for (int i = 0; i < leftPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = leftPath[i];
|
|
}
|
|
for (int i = 0; i < goalPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = goalPath[i];
|
|
}
|
|
}
|
|
if (rightValid && CanCross(startPath[startPath.size() - 2], startPath[startPath.size() - 1], rightPath[0], r)
|
|
&& CanCross(goalPath[goalPath.size() - 2], goalPath[goalPath.size() - 1], rightPath[rightPath.size() - 1], r))
|
|
{
|
|
for (int i = 0; i < startPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = startPath[i];
|
|
}
|
|
for (int i = 0; i < rightPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = rightPath[i];
|
|
}
|
|
for (int i = 0; i < goalPath.size(); i++)
|
|
{
|
|
fullLeftPath.push() = goalPath[i];
|
|
}
|
|
}
|
|
if ((fullLeftPath.size() == 0) && (fullRightPath.size() == 0))
|
|
{
|
|
return false;
|
|
}
|
|
else if ((fullLeftPath.size() > 0) && (fullRightPath.size() > 0))
|
|
{
|
|
SrPolygon funnelLeft, funnelRight;
|
|
float leftLength = GetShortestPath(funnelLeft, fullLeftPath, x1, y1, x2, y2, r);
|
|
float rightLength = GetShortestPath(funnelRight, fullRightPath, x1, y1, x2, y2, r);
|
|
if (leftLength < rightLength)
|
|
{
|
|
fullRightPath.size(0);
|
|
//maybe save other values?
|
|
}
|
|
else
|
|
{
|
|
fullLeftPath.size(0);
|
|
//maybe save other values?
|
|
}
|
|
}
|
|
SrArray<SeBase *> channel;
|
|
if (fullRightPath.size() == 0)
|
|
{
|
|
for (int i = 0; i < fullLeftPath.size(); i++)
|
|
{
|
|
channel.push() = fullLeftPath[i];
|
|
}
|
|
}
|
|
else //if (fullLeftPath.size() == 0)
|
|
{
|
|
for (int i = 0; i < fullRightPath.size(); i++)
|
|
{
|
|
channel.push() = fullRightPath[i];
|
|
}
|
|
}
|
|
GetShortestPath(currentPath, channel, x1, y1, x2, y2, r);
|
|
return true;
|
|
}
|
|
}
|
|
sr_out << "NOT A DEGREE-2 RING\n";
|
|
//TODO: next, if either start or goal triangle is degree-2, move to adjacent degree-3 node(s)
|
|
//TODO: (whenever performing these "moves", record the distance or angle travelled)
|
|
|
|
//TODO: search (A* or IDA*?) from start degree-3 node(s) to goal ones on degree-3 nodes only
|
|
//TODO: whenever a path is found, run funnel algorithm to determine the length
|
|
//TODO: if this path is shorter than the shortest path so far, it becomes the shortest
|
|
//TODO: ends when either there are no more nodes to try (connected component exhausted)
|
|
//TODO: or the shortest path being searched is >= the shortest path found
|
|
//TODO: return true and the shortest path found, false if none were found
|
|
// SrArray<SearchNode *> startNodes;
|
|
// std::priority_queue<SearchNode> q;
|
|
SeDcdtFace *startTriangle1 = NULL, *startTriangle2 = NULL;
|
|
std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> q;
|
|
if (startFace2->link->Degree() == 3)
|
|
{
|
|
SrPnt2 p = ClosestPointTo(startFace2, x2, y2, r);
|
|
// startNodes.push() = new SearchNode(Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
|
|
// Length(p.x, p.y, x2, y2), p, startFace2, NULL);
|
|
q.push(new SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
|
|
Length(p.x, p.y, x2, y2), p, startFace2, NULL));
|
|
}
|
|
else
|
|
{
|
|
SeBase *s = startFace2->se();
|
|
// SeDcdtFace *first = NULL;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
SeDcdtFace *face = startFace2->link->Adjacent(i);
|
|
if (face == NULL)
|
|
{
|
|
s = s->nxt();
|
|
continue;
|
|
}
|
|
((startTriangle1 == NULL) ? startTriangle1 : startTriangle2) = face;
|
|
SrPnt2 p = ClosestPointTo(face, x2, y2, r);
|
|
SrPnt2 p2 = ClosestPointTo(face, x1, y1, r);
|
|
SrPnt2 p3 = ClosestPointTo(startFace2, x1, y1, r);
|
|
float distance1 = Maximum(startAngle * r, Length(x1, y1, p3.x, p3.y));
|
|
float distance2 = startFace2->link->Angle(i) * r;
|
|
float x_1, y_1, x_2, y_2, x_3, y_3;
|
|
TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
|
|
float theta = (startFace2->link->Adjacent((i + 1) % 3) == NULL) ?
|
|
AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_2, y_2, x_1, y_1, x_3, y_3);
|
|
q.push(new SearchNode(Maximum(Length(p2.x, p2.y, x1, y1), distance1 + distance2 + theta * r),
|
|
Length(p.x, p.y, x2, y2), p, face, NULL, i));
|
|
s = s->nxt();
|
|
}
|
|
}
|
|
SrArray<SearchNode> goalNodes;
|
|
if (goalFace2->link->Degree() == 3)
|
|
{
|
|
SrPnt2 p = ClosestPointTo(goalFace2, x2, y2, r);
|
|
goalNodes.push() = SearchNode(Length(p.x, p.y, x1, y1),
|
|
Length(p.x, p.y, x2, y2), p, goalFace2, NULL);
|
|
}
|
|
else
|
|
{
|
|
// SeDcdtFace *first = NULL;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
SeDcdtFace *face = goalFace2->link->Adjacent(i);
|
|
if (face == NULL)
|
|
{
|
|
continue;
|
|
}
|
|
//TODO: better estimation of distance to goal (and what of g-value?)
|
|
SrPnt2 p = ClosestPointTo(face, x2, y2, r);
|
|
goalNodes.push(SearchNode(0.0f, //Maximum(Length(p.x, p.y, x1, y1), startAngle * r),
|
|
Length(p.x, p.y, x2, y2), p, face, NULL, i));
|
|
}
|
|
}
|
|
int expanded = 0;
|
|
float shortestPathLength = INFINITY;
|
|
SrPolygon shortestPath;
|
|
float toGoal = Minimum(goalNodes[0].h(), (goalNodes.size() > 1) ? goalNodes[1].h() : INFINITY);
|
|
if ((q.size() == 2) && (goalNodes.size() == 2) &&
|
|
(((startTriangle1 == goalNodes[0].Triangle()) && (startTriangle2 == goalNodes[1].Triangle())) ||
|
|
((startTriangle1 == goalNodes[1].Triangle()) && (startTriangle2 == goalNodes[0].Triangle()))))
|
|
{
|
|
//if they're on the same edge, walk between them for one path,
|
|
// set start to one and goal to other degree-3 nodes and continue
|
|
SeDcdtFace *face = startFace2;
|
|
SeDcdtFace *destinationFace;
|
|
float destinationAngle;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (face->link->Adjacent(i) == goalNodes[0].Triangle())
|
|
{
|
|
destinationAngle = face->link->Angle(i);
|
|
break;
|
|
}
|
|
}
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (goalFace2->link->Adjacent(i) == goalNodes[0].Triangle())
|
|
{
|
|
destinationFace = goalNodes[(goalFace2->link->Angle(i) < destinationAngle) ? 0 : 1].Triangle();
|
|
break;
|
|
}
|
|
}
|
|
float x, y;
|
|
TriangleMidpoint(destinationFace, x, y);
|
|
sr_out << "destinationFace = (" << x << ", " << y << ")\n";
|
|
SrArray<SeBase *> channel;
|
|
// bool sameEdge = false;
|
|
while (true)
|
|
{
|
|
TriangleMidpoint(face, x, y);
|
|
sr_out << "face = (" << x << ", " << y << ")\n";
|
|
if (face == destinationFace)
|
|
{
|
|
//not on the same edge - skip to below
|
|
break;
|
|
}
|
|
else if (face == goalFace2)
|
|
{
|
|
//on the same edge - construct channel: startFace -> startFace2 -> goalFace2 -> goalFace
|
|
//calculate path and measure length and save values
|
|
// sameEdge = true;
|
|
SrArray<SeBase *> startPath;
|
|
WalkBetween(startPath, startFace, startFace2, 0.0f);
|
|
startPath.pop();
|
|
SrArray<SeBase *> goalPath;
|
|
WalkBetween(goalPath, goalFace, goalFace2, 0.0f);
|
|
channel.revert();
|
|
float x, y;
|
|
sr_out << "startPath:\n";
|
|
for (int i = 0; i < startPath.size(); i++)
|
|
{
|
|
TriangleMidpoint(((SeDcdtFace *)startPath[i]->fac()), x, y);
|
|
sr_out << "(" << x << ", " << y << ")\n";
|
|
}
|
|
sr_out << "channel:\n";
|
|
for (int i = 0; i < channel.size(); i++)
|
|
{
|
|
TriangleMidpoint(((SeDcdtFace *)channel[i]->fac()), x, y);
|
|
sr_out << "(" << x << ", " << y << ")\n";
|
|
}
|
|
sr_out << "goalPath:\n";
|
|
for (int i = 0; i < goalPath.size(); i++)
|
|
{
|
|
TriangleMidpoint(((SeDcdtFace *)goalPath[i]->fac()), x, y);
|
|
sr_out << "(" << x << ", " << y << ")\n";
|
|
}
|
|
while (!channel.empty())
|
|
{
|
|
startPath.push() = channel.pop();
|
|
}
|
|
while (!goalPath.empty())
|
|
{
|
|
startPath.push() = goalPath.pop();
|
|
}
|
|
startPath.pop();
|
|
if (ValidPath(startPath, x1, y1, x2, y2, r))
|
|
{
|
|
shortestPathLength = GetShortestPath(shortestPath, startPath, x1, y1, x2, y2, r);
|
|
}
|
|
//then make goalNodes only the one at destinationFace and remove that one from q
|
|
//then skip down to searching
|
|
goalNodes.remove((goalNodes[0].Triangle() == destinationFace) ? 1 : 0);
|
|
if (q.top()->Triangle() == destinationFace)
|
|
{
|
|
q.pop();
|
|
}
|
|
else
|
|
{
|
|
SearchNode *temp = q.top();
|
|
q.pop();
|
|
q.pop();
|
|
q.push(temp);
|
|
}
|
|
break;
|
|
}
|
|
SeBase *s = face->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (face->link->Adjacent(i) == destinationFace)
|
|
{
|
|
channel.push() = s;
|
|
face = (SeDcdtFace *)s->sym()->fac();
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
}
|
|
/*
|
|
//if they just have the same endpoints, walk both ways around and choose the shortest
|
|
if (!sameEdge)
|
|
{
|
|
while (!channel.empty())
|
|
{
|
|
channel.pop();
|
|
}
|
|
SrArray<SeBase *> startPath, startLeftPath, startRightPath, goalPath, goalLeftPath, goalRightPath;
|
|
WalkBetween(startPath, startFace, startFace2, 0.0f);
|
|
WalkBetween(startLeftPath, startFace2, goalNodes[0].Triangle(), 0.0f);
|
|
WalkBetween(startRightPath, startFace2, goalNodes[1].Triangle(), 0.0f);
|
|
WalkBetween(goalPath, goalFace, goalFace2, 0.0f);
|
|
WalkBetween(goalLeftPath, goalFace2, goalNodes[0].Triangle(), 0.0f);
|
|
WalkBetween(goalRightPath, goalFace2, goalNodes[1].Triangle(), 0.0f);
|
|
SrArray<SeBase *> leftChannel, rightChannel;
|
|
for (int i = 0; i < startPath.size() - 1; i++) // int i = 1 ?
|
|
{
|
|
leftChannel.push() = startPath[i];
|
|
rightChannel.push() = startPath[i];
|
|
}
|
|
for (int i = 0; i < startLeftPath.size() - 1; i++)
|
|
{
|
|
leftChannel.push() = startLeftPath[i];
|
|
}
|
|
for (int i = 0; i < startRightPath.size() - 1; i++)
|
|
{
|
|
rightChannel.push() = startRightPath[i];
|
|
}
|
|
for (int i = goalLeftPath.size() - 1; i > 0; i--)
|
|
{
|
|
leftChannel.push() = goalLeftPath[i];
|
|
}
|
|
for (int i = goalRightPath.size() - 1; i > 0; i--)
|
|
{
|
|
rightChannel.push() = goalRightPath[i];
|
|
}
|
|
for (int i = goalPath.size() - 1; i >= 0; i--) // i > 0 ?
|
|
{
|
|
leftChannel.push() = goalPath[i];
|
|
rightChannel.push() = goalPath[i];
|
|
}
|
|
SrPolygon leftPath, rightPath;
|
|
float leftPathLength = (ValidPath(leftChannel, x1, y1, x2, y2, r)) ?
|
|
GetShortestPath(leftPath, leftChannel, x1, y1, x2, y2, r) : INFINITY;
|
|
float rightPathLength = (ValidPath(rightChannel, x1, y1, x2, y2, r)) ?
|
|
GetShortestPath(rightPath, rightChannel, x1, y1, x2, y2, r) : INFINITY;
|
|
if (Minimum(leftPathLength, rightPathLength) == INFINITY)
|
|
{
|
|
return false;
|
|
}
|
|
currentPath = (leftPathLength < rightPathLength) ? leftPath : rightPath;
|
|
return true;
|
|
}
|
|
*/
|
|
}
|
|
/*
|
|
SrArray<SeBase *> firstChannel;
|
|
if (IDA(startNodes, goalNodes, firstChannel, r))
|
|
{
|
|
//construct channel
|
|
SrPolygon firstPath;
|
|
float depthBound = GetShortestPath(firstPath, firstChannel, x1, y1, x2, y2, r);
|
|
SearchNode *goalNode;
|
|
IDA(startNodes, goalNodes, goalNode, depthBound, r);
|
|
}
|
|
*/
|
|
while (!q.empty())
|
|
{
|
|
SearchNode *current = q.top();
|
|
q.pop();
|
|
float x, y;
|
|
TriangleMidpoint(current->Triangle(), x, y);
|
|
sr_out << "Expanding (" << x << ", " << y << ") g = " <<
|
|
current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n";
|
|
if ((current->f() + toGoal) >= shortestPathLength)
|
|
{
|
|
while (!q.empty())
|
|
{
|
|
q.top()->Close();
|
|
delete q.top();
|
|
q.pop();
|
|
}
|
|
break;
|
|
}
|
|
if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge())
|
|
{
|
|
sr_out << "PRUNED!\n";
|
|
continue;
|
|
}
|
|
expanded++;
|
|
bool found = false;
|
|
for (int i = 0; i < goalNodes.size(); i++)
|
|
{
|
|
if (current->Triangle() == goalNodes[i].Triangle())
|
|
{
|
|
SrArray<SeBase *> channel;
|
|
ConstructBasePath(channel, current, startFace, goalFace, goalNodes[i].Direction());
|
|
current->Close();
|
|
delete current;
|
|
if (ValidEndpoints(channel, x1, y1, x2, y2, r))
|
|
{
|
|
SrPolygon currentShortestPath;
|
|
float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r);
|
|
char string[20];
|
|
sprintf(string, "%f", currentPathLength);
|
|
if (strcmp(string, "-1.#IND00") == 0)//((currentPathLength < 0.0f) || (currentPathLength > 100.0f))
|
|
{
|
|
float x, y;
|
|
sr_out << "CHANNEL = {";
|
|
for (int j = 0; j < channel.size() - 1; j++)
|
|
{
|
|
TriangleMidpoint((SeDcdtFace *)channel[j]->fac(), x, y);
|
|
sr_out << "(" << x << ", " << y << "), ";
|
|
}
|
|
TriangleMidpoint((SeDcdtFace *)channel[channel.size() - 1]->fac(), x, y);
|
|
sr_out << "(" << x << ", " << y << ")}\n";
|
|
sr_out << "PATH = {";
|
|
for (int j = 0; j < currentShortestPath.size() - 1; j++)
|
|
{
|
|
sr_out << "(" << currentShortestPath[j].x << ", " << currentShortestPath[j].y << "), ";
|
|
}
|
|
sr_out << "(" << currentShortestPath[currentShortestPath.size() - 1].x
|
|
<< ", " << currentShortestPath[currentShortestPath.size() - 1].y << ")}\n";
|
|
}
|
|
sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n";
|
|
if (currentPathLength < shortestPathLength)
|
|
{
|
|
shortestPathLength = currentPathLength;
|
|
shortestPath.size(0);
|
|
shortestPath = currentShortestPath;
|
|
}
|
|
}
|
|
found = true;
|
|
}
|
|
}
|
|
if (found)
|
|
{
|
|
continue;
|
|
}
|
|
// SeBase *s = current.Triangle()->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (((current->Triangle() == startTriangle1) && (current->Triangle()->link->Adjacent(i) == startTriangle2))
|
|
|| ((current->Triangle() == startTriangle2) && (current->Triangle()->link->Adjacent(i) == startTriangle1)))
|
|
{
|
|
continue;
|
|
}
|
|
if (current->Triangle()->link->Adjacent(i) != NULL) //!Blocked(s))
|
|
{
|
|
SeDcdtFace *childFace = current->Triangle()->link->Adjacent(i); //(SeDcdtFace *)s->sym()->fac();
|
|
float width = (current->Back() == NULL) ? INFINITY :
|
|
(current->Triangle()->link->Adjacent((i + 1) % 3) == current->Back()->Triangle()) ?
|
|
current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3);
|
|
width = Minimum(current->Triangle()->link->Choke(i), width);
|
|
if ((!current->Searched(childFace)) && (width >= 2.0f * r)
|
|
&& ((current->Back() == NULL) || (childFace != current->Back()->Triangle())))
|
|
{
|
|
SrPnt2 p = ClosestPointTo(childFace, x2, y2, r);
|
|
// float x_1, y_1, x_2, y_2, x_3, y_3;
|
|
// TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
|
|
// float theta = (current.Back() == NULL) ? 0.0f : (current.Triangle()->link->Adjacent((i + 1) % 3) == current.Back()->Triangle()) ?
|
|
// AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2);
|
|
float h = Length(p.x, p.y, x2, y2);
|
|
float minDistance = current->g() + Maximum(current->Triangle()->link->Angle(i) * r, current->h() - h);
|
|
SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r);
|
|
minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance);
|
|
if (minDistance < childFace->link->ShortestPath(update))
|
|
{
|
|
childFace->link->ShortestPath(minDistance, update);
|
|
}
|
|
else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge())
|
|
{
|
|
sr_out << "PREPRUNED!\n";
|
|
continue;
|
|
}
|
|
q.push(new SearchNode(minDistance, h, p, childFace, current, i));
|
|
current->OpenChild();
|
|
TriangleMidpoint(childFace, x, y);
|
|
sr_out << " Generated (" << x << ", " << y << ") g = "
|
|
<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n";
|
|
}
|
|
}
|
|
// s = s->nxt();
|
|
}
|
|
if (current->OpenChildren() <= 0)
|
|
{
|
|
current->Close();
|
|
delete current;
|
|
}
|
|
}
|
|
//TODO: make sure the SeBase*s are being added to the paths the right way
|
|
//TODO: (interior edges => path lengths should be # of triangles in path - 1)
|
|
//TODO: improve way GetShortestPath gets the vertices (see SeTriangulator::get_channel_boundary())
|
|
//TODO: save stuff to member variables?
|
|
sr_out << expanded << " NODES EXPANDED IN SEARCH\n";
|
|
currentPath = shortestPath;
|
|
return (shortestPathLength < INFINITY);
|
|
}
|
|
|
|
//searches the abstract graph for a path between two points
|
|
bool SeDcdt::SearchPathBase(float x1, float y1, float x2, float y2, float r, int update)
|
|
{
|
|
int expanded = 0;
|
|
//initialize the path to empty
|
|
currentPath.size(0);
|
|
SeBase *start;//, *goal;
|
|
//find the triangle containing the start point
|
|
SeTriangulator::LocateResult startResult = LocatePoint(x1, y1, start);
|
|
//if it wasn't found, return a path could not be found
|
|
if (startResult == SeTriangulator::LocateResult::NotFound)
|
|
{
|
|
return false;
|
|
}
|
|
SeDcdtFace *startFace = (SeDcdtFace *)start->fac();
|
|
std::priority_queue<SearchNode *, std::vector<SearchNode *>, check> q;
|
|
SrPnt2 p = ClosestPointTo(startFace, x2, y2, r);
|
|
q.push(new SearchNode(0.0f /*Length(x1, y1, p.x, p.y)*/, Length(p.x, p.y, x2, y2), p, startFace, NULL));
|
|
float shortestPathLength = INFINITY;
|
|
SrPolygon shortestPath;
|
|
while (!q.empty())
|
|
{
|
|
SearchNode *current = q.top();
|
|
q.pop();
|
|
expanded++;
|
|
float x, y;
|
|
TriangleMidpoint(current->Triangle(), x, y);
|
|
sr_out << "Expanding (" << x << ", " << y << ") g = " <<
|
|
current->g() << ", h = " << current->h() << ", f = " << current->f() << "\n";
|
|
if (current->f() >= shortestPathLength)
|
|
{
|
|
while (!q.empty())
|
|
{
|
|
q.top()->Close();
|
|
delete q.top();
|
|
q.pop();
|
|
}
|
|
break;
|
|
}
|
|
else if (current->g() >= current->Triangle()->link->ShortestPath(update) + current->Triangle()->link->LongestEdge())
|
|
{
|
|
sr_out << "PRUNED!\n";
|
|
continue;
|
|
}
|
|
else if (InTriangle(current->Triangle(), x2, y2))
|
|
{
|
|
SrArray<SeBase *> channel = ConstructBaseChannel(current);
|
|
current->Close();
|
|
delete current;
|
|
if (ValidEndpoints(channel, x1, y1, x2, y2, r))
|
|
{
|
|
SrPolygon currentShortestPath;
|
|
float currentPathLength = GetShortestPath(currentShortestPath, channel, x1, y1, x2, y2, r);
|
|
sr_out << "GENERATED PATH WITH LENGTH = " << currentPathLength << "\n";
|
|
if (currentPathLength < shortestPathLength)
|
|
{
|
|
shortestPathLength = currentPathLength;
|
|
shortestPath.size(0);
|
|
shortestPath = currentShortestPath;
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
SeBase *s = current->Triangle()->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (!Blocked(s))
|
|
{
|
|
SeDcdtFace *childFace = (SeDcdtFace *)s->sym()->fac();
|
|
float width = (current->Back() == NULL) ? INFINITY : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ?
|
|
current->Triangle()->link->Width(i) : current->Triangle()->link->Width((i + 2) % 3);
|
|
if ((!current->Searched(childFace)) && (width >= 2.0f * r)
|
|
&& ((current->Back() == NULL) || (childFace != current->Back()->Triangle())))
|
|
{
|
|
p = ClosestPointTo(childFace, x2, y2, r);
|
|
float x_1, y_1, x_2, y_2, x_3, y_3;
|
|
TriangleVertices(s, x_1, y_1, x_2, y_2, x_3, y_3);
|
|
float theta = (current->Back() == NULL) ? 0.0f : (s->nxt()->sym()->fac() == current->Back()->Triangle()) ?
|
|
AngleBetween(x_1, y_1, x_2, y_2, x_3, y_3) : AngleBetween(x_3, y_3, x_1, y_1, x_2, y_2);
|
|
float h = Length(p.x, p.y, x2, y2);
|
|
float minDistance = current->g() + Maximum(theta * r, current->h() - h);
|
|
SrPnt2 p2 = ClosestPointTo(childFace, x1, y1, r);
|
|
minDistance = Maximum(Length(x1, y1, p2.x, p2.y), minDistance);
|
|
// minDistance = Maximum(Length(x1, y1, p.x, p.y), minDistance);
|
|
if (minDistance < childFace->link->ShortestPath(update))
|
|
{
|
|
childFace->link->ShortestPath(minDistance, update);
|
|
}
|
|
else if (minDistance >= childFace->link->ShortestPath(update) + childFace->link->LongestEdge())
|
|
{
|
|
sr_out << "PREPRUNED!\n";
|
|
continue;
|
|
}
|
|
q.push(new SearchNode(minDistance, h, p, childFace, current));
|
|
current->OpenChild();
|
|
TriangleMidpoint(childFace, x, y);
|
|
sr_out << " Generated (" << x << ", " << y << ") g = "
|
|
<< minDistance << ", h = " << h << ", f = " << (minDistance + h) << "\n";
|
|
}
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
if (current->OpenChildren() <= 0)
|
|
{
|
|
current->Close();
|
|
delete current;
|
|
}
|
|
}
|
|
sr_out << expanded << " NODES EXPANDED IN SEARCH\n";
|
|
currentPath = shortestPath;
|
|
//TODO: save results as local variables?
|
|
return (shortestPathLength < INFINITY);
|
|
}
|
|
|
|
//construct the channel given the last SearchNode in the solution
|
|
SrArray<SeBase *> SeDcdt::ConstructBaseChannel(SearchNode *goal)
|
|
{
|
|
SrArray<SeBase *> channel;
|
|
if (goal->Back() == NULL)
|
|
{
|
|
return channel;
|
|
}
|
|
SearchNode *current = goal->Back();
|
|
while (current->Back() != NULL)
|
|
{
|
|
SeBase *s = current->Triangle()->se();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (s->sym()->fac() == current->Back()->Triangle())
|
|
{
|
|
channel.push() = s;
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
current = current->Back();
|
|
}
|
|
channel.revert();
|
|
return channel;
|
|
}
|
|
|
|
//uses the modified funnel algorithm to get the shortest path for nonzero-radius units
|
|
//and return the length of this path
|
|
float SeDcdt::GetShortestPath(SrPolygon &path, SrArray<SeBase *> Channel, float x1, float y1, float x2, float y2, float r)
|
|
{
|
|
//initializes the path
|
|
path.open(true);
|
|
path.size(0);
|
|
//gets the edges that make up the channel
|
|
// SrArray<SeBase *> Channel = _triangulator->get_channel_interior_edges();
|
|
//if there are no edges, there is no path
|
|
if (Channel.size() < 1)
|
|
{
|
|
return 0.0f;
|
|
}
|
|
//if there is only one, the path is a straight line
|
|
else if (Channel.size() < 2)
|
|
{
|
|
path.push().set(x1, y1);
|
|
path.push().set(x2, y2);
|
|
return Length(x1, y1, x2, y2);
|
|
}
|
|
//otherwise, initialize a funnel deque
|
|
//with the starting point
|
|
FunnelDeque fd(x1, y1, r);
|
|
//get the first edge in the channel
|
|
SeBase *s = Channel[0];
|
|
//get the reference whose sym edge
|
|
//belongs to the next triangle in the channel
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
if (s->sym()->fac() == Channel[1]->fac())
|
|
{
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
//get the point on the right side of that edge
|
|
SrPnt2 p;
|
|
p.set(((SeDcdtVertex *)s->vtx())->p);
|
|
//insert it into the deque
|
|
fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
|
|
SeVertex *right = s->vtx();
|
|
//get the point on the left side of that edge
|
|
s = s->nxt();
|
|
SeVertex *left = s->vtx();
|
|
p.set(((SeDcdtVertex *)s->vtx())->p);
|
|
//insert it into the deque
|
|
fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
|
|
//go through the other edges in the funnel
|
|
for (int i = 1; i < Channel.size() - 1; i++)
|
|
{
|
|
s = Channel[i];
|
|
//get the reference whose sym edge
|
|
//belongs to the next triangle in the channel
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
if (s->sym()->fac() == Channel[i + 1]->fac())
|
|
{
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
//if that edge shares a vertex with the right side
|
|
//of the funnel, the opposite point should be
|
|
//added to the left side of the funnel
|
|
if (s->vtx() == right)
|
|
{
|
|
p.set(((SeDcdtVertex *)s->nxt()->vtx())->p);
|
|
fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
|
|
left = s->nxt()->vtx();
|
|
}
|
|
//otherwise, add it to the right side of the funnel
|
|
else //(s->nxt()->vtx() == left)
|
|
{
|
|
p.set(((SeDcdtVertex *)s->vtx())->p);
|
|
fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
|
|
right = s->vtx();
|
|
}
|
|
}
|
|
//go to the last edge in the channel
|
|
s = Channel[Channel.size() - 1];
|
|
//find the reference across which is
|
|
//the triangle with the goal in it
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
if (InTriangle((SeDcdtFace *)s->sym()->fac(), x2, y2))
|
|
{
|
|
break;
|
|
}
|
|
s = s->nxt();
|
|
}
|
|
//like above, determines whether to add
|
|
//the point to the left or right side of the funnel
|
|
if (s->vtx() == right)
|
|
{
|
|
p.set(((SeDcdtVertex *)s->nxt()->vtx())->p);
|
|
fd.Add(p, FunnelDeque::CornerType::RightTangent, path);
|
|
}
|
|
else //(s->nxt()->vtx() == left)
|
|
{
|
|
p.set(((SeDcdtVertex *)s->vtx())->p);
|
|
fd.Add(p, FunnelDeque::CornerType::LeftTangent, path);
|
|
}
|
|
//finally, add the goal point to the funnel
|
|
p.set(x2, y2);
|
|
fd.Add(p, FunnelDeque::CornerType::Point, path);
|
|
//calculate the length of the path sand return it
|
|
return fd.Length(path);
|
|
}
|
|
|
|
SrPolygon SeDcdt::GetChannelBoundary()
|
|
{
|
|
SrPolygon boundary;
|
|
return boundary;
|
|
}
|
|
|
|
SrPolygon &SeDcdt::GetPath()
|
|
{
|
|
return currentPath;
|
|
}
|
|
|
|
//Abstract space searching function definitions }
|