CellSet EdgeCell::spatialBoundary() const
{
if(isClosed())
{
return CellSet();
}
else
{
CellSet left = startVertices();
CellSet right = endVertices();
left.unite(right);
return left;
}
}
InbetweenEdge::InbetweenEdge(VAC * vac,
const Path & beforePath,
const Path & afterPath,
const AnimatedVertex & startAnimatedVertex,
const AnimatedVertex & endAnimatedVertex) :
Cell(vac),
InbetweenCell(vac),
EdgeCell(vac),
beforePath_(beforePath),
afterPath_(afterPath),
startAnimatedVertex_(startAnimatedVertex),
endAnimatedVertex_(endAnimatedVertex)
{
// Check pre-conditions
assert(beforePath_.isValid());
assert(afterPath_.isValid());
assert(beforePath_.time() < afterPath_.time());
assert(startAnimatedVertex_.isValid());
assert(endAnimatedVertex_.isValid());
assert(beforePath_.startVertex() == startAnimatedVertex_.beforeVertex());
assert(afterPath_.startVertex() == startAnimatedVertex_.afterVertex());
assert(beforePath_.endVertex() == endAnimatedVertex_.beforeVertex());
assert(afterPath_.endVertex() == endAnimatedVertex_.afterVertex());
// Cache star
foreach(VertexCell * vertex, startVertices())
addMeToSpatialStarOf_(vertex);
foreach(VertexCell * vertex, endVertices())
addMeToSpatialStarOf_(vertex);
foreach(KeyCell * kcell, beforeCells())
addMeToTemporalStarAfterOf_(kcell);
foreach(KeyCell * kcell, afterCells())
addMeToTemporalStarBeforeOf_(kcell);
}
//------------------------A* algorithm----------------------------
bool aStar(std::vector<Vector2>& pathVertices, Vector2 pathStart, const Vector2& pathFinish, const std::vector<Triangle>& triangles, Graph& graph, const std::vector<Block>& blocks, const float* screenBoundaries)
{
//Move starting point inside boundaries
if (pathStart.mX < screenBoundaries[0])
{
pathStart.mX = screenBoundaries[0];
}
if (pathStart.mX > screenBoundaries[1])
{
pathStart.mX = screenBoundaries[1];
}
if (pathStart.mY < screenBoundaries[2])
{
pathStart.mY = screenBoundaries[2];
}
if (pathStart.mY > screenBoundaries[3])
{
pathStart.mY = screenBoundaries[3];
}
triIterator startTriangle = triangles.end();
triIterator finishTriangle = triangles.end();
for (triIterator it = triangles.begin(); it != triangles.end(); ++it)
{
if (it->containsPoint(pathStart))
{
startTriangle = it;
}
if (it->containsPoint(pathFinish))
{
finishTriangle = it;
}
}
//Start and finish in same triangle, no pathfinding required
if (startTriangle == finishTriangle)
{
pathVertices.push_back(pathStart);
pathVertices.push_back(pathFinish);
return true;
}
std::set<Vector2> closed, open;
const Vector2 finArr[] = { finishTriangle->mMidP1, finishTriangle->mMidP2, finishTriangle->mMidP3 };
std::vector<Vector2> finishVertices(finArr, finArr + sizeof(finArr) / sizeof(finArr[0]));
const Vector2 stArr[] = { startTriangle->mMidP1, startTriangle->mMidP2, startTriangle->mMidP3 };
std::vector<Vector2> startVertices(stArr, stArr + sizeof(stArr) / sizeof(stArr[0]));
closed.insert(pathStart);
//Add starting triangle midpoints to open
for (vecCIter it = startVertices.begin(); it != startVertices.end(); ++it)
{
open.insert(*it);
graph.setParent(*it, pathStart);
graph.addNeighbor(pathStart, *it);
graph.setG(*it, pathStart.distance(*it));
}
while (true)
{
//Failed
if (open.empty())
{
return false;
}
//Select vertex with lowest F
Vector2 currentVertex = getVertexWithMinF(open, graph, pathFinish);
float parentG = graph.getG(graph.getParent(currentVertex));
//A neighbor of finish point found -> success
if (std::find(finishVertices.begin(), finishVertices.end(), currentVertex) != finishVertices.end())
{
pathVertices.push_back(pathFinish);
pathVertices.push_back(currentVertex);
Vector2 parent = graph.getParent(currentVertex);
while (parent != pathStart)
{
pathVertices.push_back(parent);
parent = graph.getParent(parent);
}
pathVertices.push_back(parent);
std::reverse(pathVertices.begin(), pathVertices.end());
return true;
}
closed.insert(currentVertex);
open.erase(currentVertex);
//Insert unclosed neighbors and modify parents if lower G is found
const std::set<Vector2>& neighbors = graph.getNeighbors(currentVertex);
for (setCIter it = neighbors.begin(); it != neighbors.end(); ++it)
{
//Not in closed and does not intersect block
if (closed.find(*it) == closed.end() && !intersectsBlock(currentVertex, *it, blocks))
{
setCIter currNeighbor = open.find(*it);
//Not in open
if (currNeighbor == open.end())
{
open.insert(*it);
graph.setParent(*it, currentVertex);
graph.setG(*it, parentG + currentVertex.distance(*it));
}
//In list, check if G better from here
else if (parentG + currentVertex.distance(*currNeighbor) < graph.getG(*currNeighbor))
{
graph.setParent(*currNeighbor, currentVertex);
graph.setG(*currNeighbor, parentG + currentVertex.distance(*currNeighbor));
}
}
}
}
}
const ID &
MyRCM::number(Graph &theGraph, const ID &startVertices)
{
// first check our size, if not same make new
if (numVertex != theGraph.getNumVertex()) {
// delete the old
if (theRefResult != 0)
delete theRefResult;
numVertex = theGraph.getNumVertex();
theRefResult = new ID(numVertex);
if (theRefResult == 0) {
opserr << "ERROR: MyRCM::number - Out of Memory\n";
theRefResult = new ID(0);
numVertex = 0;
return *theRefResult;
}
}
// see if we can do quick return
if (numVertex == 0)
return *theRefResult;
ID copyStart(startVertices);
// we determine which node to start with
int minStartVertexTag =0;
int minAvgProfile = 0;
int startVerticesSize = startVertices.Size();
for (int j=0; j<startVerticesSize; j++) {
// we first set the Tmp of all vertices to -1, indicating
// they have not yet been added.
Vertex *vertexPtr;
VertexIter &vertexIter = theGraph.getVertices();
while ((vertexPtr = vertexIter()) != 0)
vertexPtr->setTmp(-1);
// we now set up; setting our markers and set first vertices
VertexIter &vertexIter2 = theGraph.getVertices();
int currentMark = numVertex-1; // marks current vertex visiting.
int nextMark = currentMark-1;
for (int k=0; k<startVerticesSize; k++)
if (k != j)
copyStart(k) = 0;
else
copyStart(k) = 1;
vertexPtr = theGraph.getVertexPtr(startVertices(j));
(*theRefResult)(currentMark) = vertexPtr->getTag();
vertexPtr->setTmp(currentMark);
int numFromStart = 1;
int avgProfile = 1;
while (numFromStart < startVerticesSize) {
// get the current vertex and its adjacency
vertexPtr = theGraph.getVertexPtr((*theRefResult)(currentMark));
const ID &adjacency = vertexPtr->getAdjacency();
// go through the vertices adjacency and add vertices which
// have not yet been Tmp'ed to the (*theRefResult)
int size = adjacency.Size();
for (int i=0; i<size; i++) {
int vertexTag = adjacency(i);
int loc =startVertices.getLocation(vertexTag);
if (loc >= 0) {
vertexPtr = theGraph.getVertexPtr(vertexTag);
if ((vertexPtr->getTmp()) == -1) {
vertexPtr->setTmp(nextMark);
copyStart(loc) = 1;
numFromStart++;
avgProfile += currentMark - nextMark;
(*theRefResult)(nextMark--) = vertexTag;
}
}
}
// go to the next vertex
// we decrement because we are doing reverse Cuthill-McKee
currentMark--;
// check to see if graph is disconneted
if (currentMark == nextMark && numFromStart < startVerticesSize) {
// loop over iter till we get a vertex not yet included
for (int l=0; l<startVerticesSize; l++)
if (copyStart(l) == 0) {
int vertexTag = startVertices(l);
vertexPtr = theGraph.getVertexPtr(vertexTag);
nextMark--;
copyStart(l) = 1;
vertexPtr->setTmp(currentMark);
numFromStart++;
(*theRefResult)(currentMark) = vertexPtr->getTag();
l =startVerticesSize;
}
}
}
currentMark = numVertex-1; // set current to the first again
nextMark = numVertex - startVerticesSize -1;
// we continue till the ID is full
while (nextMark >= 0) {
// get the current vertex and its adjacency
vertexPtr = theGraph.getVertexPtr((*theRefResult)(currentMark));
const ID &adjacency = vertexPtr->getAdjacency();
// go through the vertices adjacency and add vertices which
// have not yet been Tmp'ed to the (*theRefResult)
int size = adjacency.Size();
for (int i=0; i<size; i++) {
int vertexTag = adjacency(i);
vertexPtr = theGraph.getVertexPtr(vertexTag);
if ((vertexPtr->getTmp()) == -1) {
vertexPtr->setTmp(nextMark);
avgProfile += currentMark - nextMark;
(*theRefResult)(nextMark--) = vertexTag;
}
}
// go to the next vertex
// we decrement because we are doing reverse Cuthill-McKee
currentMark--;
// check to see if graph is disconneted
if ((currentMark == nextMark) && (currentMark >= 0)) {
// loop over iter till we get a vertex not yet Tmped
while (((vertexPtr = vertexIter2()) != 0) &&
(vertexPtr->getTmp() != -1))
;
nextMark--;
vertexPtr->setTmp(currentMark);
(*theRefResult)(currentMark) = vertexPtr->getTag();
}
}
if (j == 0 || minAvgProfile > avgProfile) {
minStartVertexTag = startVertices(j);
minAvgProfile = avgProfile;
}
}
// now we numebr based on minStartVErtexTag
// we first set the Tmp of all vertices to -1, indicating
// they have not yet been added.
Vertex *vertexPtr;
VertexIter &vertexIter = theGraph.getVertices();
while ((vertexPtr = vertexIter()) != 0)
vertexPtr->setTmp(-1);
// we now set up; setting our markers and set first vertices
VertexIter &vertexIter2 = theGraph.getVertices();
int currentMark = numVertex-1; // marks current vertex visiting.
int nextMark = currentMark-1;
vertexPtr = theGraph.getVertexPtr(minStartVertexTag);
(*theRefResult)(currentMark) = vertexPtr->getTag();
vertexPtr->setTmp(currentMark);
currentMark--;
int loc = startVertices.getLocation(minStartVertexTag);
for (int k=0; k<startVerticesSize; k++)
if (k != loc)
copyStart(k) = 0;
int numFromStart = 1;
while (numFromStart < startVerticesSize) {
// get the current vertex and its adjacency
vertexPtr = theGraph.getVertexPtr((*theRefResult)(currentMark));
const ID &adjacency = vertexPtr->getAdjacency();
// go through the vertices adjacency and add vertices which
// have not yet been Tmp'ed to the (*theRefResult)
int size = adjacency.Size();
for (int i=0; i<size; i++) {
int vertexTag = adjacency(i);
int loc =startVertices.getLocation(vertexTag);
if (loc >= 0) {
vertexPtr = theGraph.getVertexPtr(vertexTag);
if ((vertexPtr->getTmp()) == -1) {
vertexPtr->setTmp(nextMark);
copyStart(loc) = 1;
numFromStart++;
(*theRefResult)(nextMark--) = vertexTag;
}
}
}
// go to the next vertex
// we decrement because we are doing reverse Cuthill-McKee
currentMark--;
// check to see if graph is disconneted
if (currentMark == nextMark && numFromStart < startVerticesSize) {
// loop over iter till we get a vertex not yet included
for (int l=0; l<startVerticesSize; l++)
if (copyStart(l) == 0) {
int vertexTag = startVertices(l);
vertexPtr = theGraph.getVertexPtr(vertexTag);
nextMark--;
copyStart(l) = 1;
vertexPtr->setTmp(currentMark);
numFromStart++;
(*theRefResult)(currentMark) = vertexPtr->getTag();
l =startVerticesSize;
}
}
}
currentMark = numVertex-1; // set current to the first again
nextMark = numVertex - startVerticesSize -1;
currentMark = numVertex-1; // set current to the first again
// we continue till the ID is full
while (nextMark >= 0) {
// get the current vertex and its adjacency
vertexPtr = theGraph.getVertexPtr((*theRefResult)(currentMark));
const ID &adjacency = vertexPtr->getAdjacency();
// go through the vertices adjacency and add vertices which
// have not yet been Tmp'ed to the (*theRefResult)
int size = adjacency.Size();
for (int i=0; i<size; i++) {
int vertexTag = adjacency(i);
vertexPtr = theGraph.getVertexPtr(vertexTag);
if ((vertexPtr->getTmp()) == -1) {
vertexPtr->setTmp(nextMark);
(*theRefResult)(nextMark--) = vertexTag;
}
}
// go to the next vertex
// we decrement because we are doing reverse Cuthill-McKee
currentMark--;
// check to see if graph is disconneted
if ((currentMark == nextMark) && (currentMark >= 0)) {
opserr << "WARNING: MyRCM::number - Disconnected graph ";
// loop over iter till we get a vertex not yet Tmped
while (((vertexPtr = vertexIter2()) != 0) &&
(vertexPtr->getTmp() != -1))
;
nextMark--;
vertexPtr->setTmp(currentMark);
(*theRefResult)(currentMark) = vertexPtr->getTag();
}
}
// now set the vertex references instead of the vertex tags
// in the result, we change the Tmp to indicate number and return
for (int m=0; m<numVertex; m++) {
int vertexTag = (*theRefResult)(m);
vertexPtr = theGraph.getVertexPtr(vertexTag);
vertexPtr->setTmp(m+1); // 1 through numVertex
(*theRefResult)(m) = vertexPtr->getTag();
}
return *theRefResult;
}
