bool Graph::visit(int v, std::list<int> & sortedList)
{
VertexIterator iter = vertices.find(v);
if (iter != vertices.end())
{
if (iter->second.visited == false)
{
iter->second.visited = true;
std::list<Edge>::const_iterator liter = iter->second.adjList.begin();
for (; liter != iter->second.adjList.end(); ++liter)
{
if (!visit(liter->dest, sortedList))
{
return false;
}
}
sortedList.push_front(iter->first);
}
else
{
return false;
}
}
return true;
}
void Graph::TopologicalSort()
{
VertexConstIterator iter = vertices.begin();
std::list<int> sortedList;
bool cycle = false;
for (; iter != vertices.end(); ++iter)
{
if (iter->second.indegree == 0)
{
if (!visit(iter->first, sortedList))
{
cycle = true;
break;
}
}
}
if (cycle || sortedList.size() != vertices.size())
{
cout << "CYCLE";
return;
}
std::list<int>::const_iterator liter = sortedList.begin();
for (; liter != sortedList.end(); ++liter)
{
cout << *liter << " ";
}
}
void Graph::resetStates()
{
VertexIterator iter = vertices.begin();
for (; iter != vertices.end(); ++iter)
{
iter->second.visited = false;
}
}
void Graph::print()
{
VertexConstIterator iter = vertices.begin();
for (; iter != vertices.end(); ++iter)
{
cout << iter->second << "\n";
}
}
VertexMap* MeshBuilder::addVertexMap( int dimensions, const String& name,
const VertexMapFormat& format )
{
VertexMap* item = allocateVertexMap();
item->setDimensions( dimensions );
item->setName( name );
item->setFormat( format );
m_this->vertexMaps.add( item );
return item;
}
VertexMap* MeshBuilder::getVertexMapByName( const String& name )
{
int i;
for ( i = 0 ; i < (int)m_this->vertexMaps.size() ; ++i )
{
VertexMap* item = m_this->vertexMaps[i];
if ( item->name() == name )
return item;
}
return 0;
}
void Graph::addEdge(int v1, int v2)
{
VertexIterator iter = vertices.find(v1);
if (iter != vertices.end())
{
Edge edge(v2);
iter->second.adjList.push_back(edge);
}
iter = vertices.find(v2);
iter->second.indegree++;
}
bool testMapToVertexMap()
{
typedef Z2i::Point Point;
typedef Z2i::Point Vertex;
typedef int Value;
typedef map<Vertex, Value> Map;
typedef STLMapToVertexMapAdapter<Map> VertexMap;
VertexMap myMap;
BOOST_CONCEPT_ASSERT((CVertexMap<VertexMap>));
myMap.setValue(Point(1,1), 10);
myMap.setValue(Point(2,3), 2);
return (myMap(Point(1,1)) == 10 && myMap(Point(2,3)) == 2);
}
std::vector<Vertex> ConstructPath( VertexMap& aFrom, const Vertex& aCurrentNode)
{
VertexMap::iterator i = aFrom.find(aCurrentNode);
if( i != aFrom.end() )
{
std::vector<Vertex> path = ConstructPath(aFrom,(*i).second);
path.push_back(aCurrentNode);
return path;
}else
{
std::vector<Vertex> path;
path.push_back(aCurrentNode);
return path;
}
}
// input is data from mesh.Append(append, TMap, VMap) - copy/rewrite faces from appendpolys
void MeshPolygons::AppendPolygons( VFTriangleMesh & mesh,
VFTriangleMesh & append, MeshPolygons & appendpolys,
VertexMap & VMap, TriangleMap & TMap, UVMap * pUVMap )
{
std::vector<IMesh::VertexID> vBoundary;
std::vector<unsigned int> vBoundaryUV;
id_iterator curp(appendpolys.begin()), endp(appendpolys.end());
while ( curp != endp ) {
PolygonID sID = *curp++;
const std::vector<IMesh::TriangleID> & vTris = appendpolys.GetTriangles(sID);
size_t nCount = vTris.size();
PolygonID sNewID = CreatePolygon((unsigned int)nCount);
for ( unsigned int k = 0; k < nCount; ++k )
AppendTriangle( sNewID, TMap.GetNew(vTris[k]) );
const std::vector<IMesh::VertexID> & vOldBoundary = appendpolys.GetBoundary(sID);
const std::vector<unsigned int> & vOldBoundaryUV = appendpolys.GetBoundaryUV(sID);
nCount = vOldBoundary.size();
vBoundary.resize(nCount);
for ( unsigned int k = 0; k < nCount; ++k )
vBoundary[k] = VMap.GetNew( vOldBoundary[k] );
if ( pUVMap ) {
nCount = vOldBoundaryUV.size();
vBoundaryUV.resize(nCount);
for ( unsigned int k = 0; k < nCount; ++k )
vBoundaryUV[k] = pUVMap->GetNew( vOldBoundaryUV[k] );
} else
vBoundaryUV = vOldBoundaryUV;
SetBoundary(sNewID, vBoundary, &vBoundaryUV);
}
}
void Graph::BreadthFirstTour(int v)
{
cout << "BFS BEGIN\n";
std::queue<int> q;
VertexConstIterator iter = vertices.find(v);
if (iter != vertices.end())
{
q.push(iter->first);
}
else
{
cout << "BFS END\n";
return;
}
VertexIterator iter1;
while (!q.empty())
{
int vtx = q.front();
q.pop();
Vertex & v = vertices[vtx];
if (v.visited == false )
{
v.visited = true;
cout << v.orig << " ";
std::list<Edge>::const_iterator iter = v.adjList.begin();
while (iter != v.adjList.end())
{
//cout << "dest " << iter->dest << endl;
VertexConstIterator citer = vertices.find(iter->dest);
if (citer != vertices.end())
{
q.push(citer->first);
//cout << "pushing " << citer->orig << endl;
}
++iter;
}
}
}
cout << "\nBFS END" << endl;
resetStates();
}
void Graph::DepthFirstTour(int v)
{
VertexIterator iter = vertices.find(v);
if (iter != vertices.end())
{
if (iter->second.visited == false)
{
iter->second.visited = true;
std::list<Edge>::iterator liter = iter->second.adjList.begin();
for (; liter != iter->second.adjList.end(); ++liter)
{
DepthFirstTour(liter->dest);
}
cout << iter->second.orig << " ";
}
}
}
void IConnectionLoader::addConnection(
VertexMap& vertices, SystemTypeGetter* info,
int from, int to, TypeSet tags)
{
TypeSet toType, fromType;
toType.insert(info->getType(to));
toType.insert(tags.begin(), tags.end());
fromType.insert(info->getType(from));
fromType.insert(tags.begin(), tags.end());
//Add vertices if they dont exists already
//this will fail if it already exists
vertices.insert(std::make_pair(from, Vertex(from)));
vertices.insert(std::make_pair(to, Vertex(to)));
//Add edges to the vertices
vertices.find(from)->second.addEdge(to, toType);
vertices.find(to)->second.addEdge(from, fromType);
}
void MeshPolygons::RewriteBoundaries( const VertexMap & VMap )
{
std::set<Polygon>::iterator curt(m_vPolygons.begin()), endt(m_vPolygons.end());
while ( curt != endt ) {
Polygon & t = const_cast<Polygon&>(*curt++);
size_t nVerts = t.vBoundary.size();
for ( unsigned int k = 0; k < nVerts; ++k ) {
IMesh::VertexID vNew = VMap.GetNew(t.vBoundary[k]);
if ( vNew != IMesh::InvalidID )
t.vBoundary[k] = vNew;
}
}
}
bool vtkPolyData_to_polygon_mesh(vtkPolyData* poly_data,
TM& tmesh,
VertexMap& vertex_map,
FaceMap& face_map)
{
typedef typename boost::property_map<TM, CGAL::vertex_point_t>::type VPMap;
typedef typename boost::property_map_value<TM, CGAL::vertex_point_t>::type Point_3;
typedef typename boost::graph_traits<TM>::vertex_descriptor vertex_descriptor;
typedef typename boost::graph_traits<TM>::face_descriptor face_descriptor;
VPMap vpmap = get(CGAL::vertex_point, tmesh);
// get nb of points and cells
vtkIdType nb_points = poly_data->GetNumberOfPoints();
vtkIdType nb_cells = poly_data->GetNumberOfCells();
//extract points
for (vtkIdType i = 0; i<nb_points; ++i)
{
double coords[3];
poly_data->GetPoint(i, coords);
vertex_descriptor v = add_vertex(tmesh);
put(vpmap, v, Point_3(coords[0], coords[1], coords[2]));
vertex_map.insert(std::make_pair(i, v));
}
//extract cells
for (vtkIdType i = 0; i<nb_cells; ++i)
{
vtkCell* cell_ptr = poly_data->GetCell(i);
vtkIdType nb_vertices = cell_ptr->GetNumberOfPoints();
if (nb_vertices != 3){
std::cerr << "Error a cell with " << nb_vertices << " found\n";
return false;
}
std::vector<vertex_descriptor> vr;
vr.push_back(vertex_map[cell_ptr->GetPointId(0)]);
vr.push_back(vertex_map[cell_ptr->GetPointId(1)]);
vr.push_back(vertex_map[cell_ptr->GetPointId(2)]);
face_descriptor f = CGAL::Euler::add_face(vr, tmesh);
face_map.insert(std::make_pair(i, f));
}
return true;
}
void MeshPolygons::Rewrite( const VertexMap & VMap, const TriangleMap & TMap )
{
m_PolygonMap.clear();
std::set<Polygon>::iterator curt(m_vPolygons.begin()), endt(m_vPolygons.end());
while ( curt != endt ) {
Polygon & t = const_cast<Polygon&>(*curt++);
size_t nTris = t.vTris.size();
for ( unsigned int k = 0 ; k < nTris; ++k ) {
IMesh::TriangleID tNew = TMap.GetNew( t.vTris[k] );
if ( tNew != IMesh::InvalidID )
t.vTris[k] = tNew;
m_PolygonMap.insert( std::pair<IMesh::TriangleID, PolygonID>( t.vTris[k], t.id ) );
}
size_t nVerts = t.vBoundary.size();
for ( unsigned int k = 0; k < nVerts; ++k ) {
IMesh::VertexID vNew = VMap.GetNew( t.vBoundary[k] );
if ( vNew != IMesh::InvalidID )
t.vBoundary[k] = vNew;
}
}
}
typename boost::graph_traits<Graph>::vertex_descriptor
add_named_vertex(Graph& g, NameMap nm, const std::string& name, VertexMap& vm)
{
typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef typename VertexMap::iterator Iterator;
Vertex v;
Iterator iter;
bool inserted;
tie(iter, inserted) = vm.insert(make_pair(name, Vertex()));
if(inserted) {
// The name was unique so we need to add a vertex to the graph
v = add_vertex(g);
iter->second = v;
put(nm, v, name); // store the name in the name map
}
else {
// We had alread inserted this name so we can return the
// associated vertex.
v = iter->second;
}
return v;
}
static int test()
{
MeshBuilder mesh;
Vertex* v[4];
int i;
for ( i = 0 ; i < 4 ; ++i ) // 0 1
v[i] = mesh.addVertex(); // 3 2
Polygon* a = mesh.addPolygon();
a->addVertex( v[0] );
a->addVertex( v[1] );
a->addVertex( v[2] );
Polygon* b = mesh.addPolygon();
b->addVertex( v[0] );
b->addVertex( v[2] );
b->addVertex( v[3] );
VertexMap* vmap = mesh.addVertexMap( 2, "TXUV", VertexMapFormat::VERTEXMAP_TEXCOORD );
float uv[4][2] = { {0, 0}, {1,0}, {1,1}, {0,1} };
vmap->addValue( v[2]->index(), uv[2], 2 );
vmap->addValue( v[1]->index(), uv[1], 2 );
vmap->addValue( v[3]->index(), uv[3], 2 );
vmap->addValue( v[0]->index(), uv[0], 2 );
v[1] = a->getVertex(1)->clone();
a->setVertex( 1, v[1] );
assert( mesh.vertices() == 5 );
float uv1[4][2];
memset( uv1, 0, sizeof(uv1) );
for ( i = 0 ; i < 4 ; ++i )
{
vmap->getValue( v[i]->index(), uv1[i], 2 );
assert( !memcmp(uv[i],uv1[i],sizeof(uv[i])) );
}
assert( vmap->dimensions() == 2 );
assert( vmap->name() == "TXUV" );
return 0;
}
void Graph::addVertex(int v)
{
vertices.insert( std::make_pair(v, Vertex(v)) );
}
/**
* This is a straight forward C++ implementation of Wikipedia's A* pseudo code.
* As proof of this the pseudo code is embedded in the source, just above the
* C++ statements.
*
* See http://en.wikipedia.org/wiki/A*#Pseudocode for the complete pseudo code
*
* But also see the discussion page: it explains why the algorithm is only
* correct if the cost function is monotone.This algorithm does not give the
* correct solution in our case. The routes and hence costs differ whether we
* go from (0,0) to (10,10) or vice versa:
*
* 0-0 -> 3-2 -> 4-5 -> 4-8 -> 6-9 -> 10-10, cost = 15
*
* 10-10 -> 10-7 -> 7-5 -> 6-3 -> 3-2 -> 0-0, cost = 14
*
* Use this piece of code as inspiration only.
*
*/
std::vector<Vertex> WikiAStar( Vertex start, Vertex goal)
{
ClosedSet closedSet; // The set of nodes already evaluated.
OpenSet openSet; // The set of tentative nodes to be evaluated, initially containing the start node
VertexMap predecessorMap; // The map of navigated nodes.
start.actualCost = 0.0; // Cost from start along best known path.
start.heuristicCost = start.actualCost + HeuristicCost(start, goal); // Estimated total cost from start to goal through y.
openSet.insert(start);
//while openset is not empty
while(!openSet.empty())
{
// current := the node in openset having the lowest f_score[] value
Vertex current = *openSet.begin();
// if current = goal
if(current == goal)
{
// return reconstruct_path(came_from, goal)
return ConstructPath(predecessorMap,current);
}
// remove current from openset
openSet.erase(current);
// add current to closedset
closedSet.insert(current);
//for each neighbour in neighbour_nodes(current)
std::vector< Vertex > neighbours = GetNeighbours(current);
for(Vertex neighbour : neighbours)
{
// if neighbor in closedset continue (with next neighbour)
if(closedSet.find(neighbour) != closedSet.end())
{
continue;
}
// tentative_g_score := g_score[current] + dist_between(current,neighbour)
double calculatedActualNeighbourCost = current.actualCost + ActualCost(current,neighbour);
// if neighbour not in openset or tentative_g_score < g_score[neighbour]
if(openSet.find(neighbour) == openSet.end() || calculatedActualNeighbourCost < neighbour.actualCost)
{
// Here we deviate from the Wikipedia article, because of the map semantics:
// we cannot change the object's key values once it in the map so we first
// set the vales and then put it into the map.
// g_score[neighbor] := tentative_g_score
neighbour.actualCost = calculatedActualNeighbourCost;
// f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
neighbour.heuristicCost = neighbour.actualCost + HeuristicCost(neighbour, goal);
// came_from[neighbor] := current
std::pair<VertexMap::iterator,bool> result = predecessorMap.insert(std::make_pair(neighbour,current));
// The following if-statement is not part of the pseudo code but a partial fix for a semantic difference
// in the map of the pseudo code and the c++ std::map
if(result.second==false)
(*result.first).second = current;
// if neighbor not in openset
if(openSet.find(neighbour) == openSet.end())
{
// add neighbor to openset
openSet.insert(neighbour);
}
}
}
}
//return failure
return std::vector<Vertex>();
}
void RigidBody::DrawMesh()
{
/*
//Cube!
glBegin(GL_QUADS);
glColor3f(0.0f,1.0f,0.0f);
glVertex3f( 1.0f, 1.0f,-1.0f);
glVertex3f(-1.0f, 1.0f,-1.0f);
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f( 1.0f, 1.0f, 1.0f);
glColor3f(1.0f,0.5f,0.0f);
glVertex3f( 1.0f,-1.0f, 1.0f);
glVertex3f(-1.0f,-1.0f, 1.0f);
glVertex3f(-1.0f,-1.0f,-1.0f);
glVertex3f( 1.0f,-1.0f,-1.0f);
glColor3f(1.0f,0.0f,0.0f);
glVertex3f( 1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f,-1.0f, 1.0f);
glVertex3f( 1.0f,-1.0f, 1.0f);
glColor3f(1.0f,1.0f,0.0f);
glVertex3f( 1.0f,-1.0f,-1.0f);
glVertex3f(-1.0f,-1.0f,-1.0f);
glVertex3f(-1.0f, 1.0f,-1.0f);
glVertex3f( 1.0f, 1.0f,-1.0f);
glColor3f(0.0f,0.0f,1.0f);
glVertex3f(-1.0f, 1.0f, 1.0f);
glVertex3f(-1.0f, 1.0f,-1.0f);
glVertex3f(-1.0f,-1.0f,-1.0f);
glVertex3f(-1.0f,-1.0f, 1.0f);
glColor3f(1.0f,0.0f,1.0f);
glVertex3f( 1.0f, 1.0f,-1.0f);
glVertex3f( 1.0f, 1.0f, 1.0f);
glVertex3f( 1.0f,-1.0f, 1.0f);
glVertex3f( 1.0f,-1.0f,-1.0f);
glEnd();
*/
typedef map<VertexId, Vertex> VertexMap;
VertexMap vertices = m_Geom.GetVertices();
VertexMap::const_iterator end = vertices.end();
/*
glColor3f(1.0,0.0,0.0);
glBegin(GL_POINTS);
for (VertexMap::const_iterator it = vertices.begin(); it != end; ++it)
{
glPushMatrix();
glVertex3f((*it).second.GetPos().GetX(),
(*it).second.GetPos().GetY(),
(*it).second.GetPos().GetZ());
glPopMatrix();
}
glEnd();
*/
glCallList(m_RigidBodyList);
/*
glColor3f(1.0,1.0,0.0);
typedef map<FaceId, Face> FaceMap;
FaceMap faces = m_Geom.GetFaces();
FaceMap::const_iterator itFaceEnd = faces.end();
for (FaceMap::const_iterator itFace = faces.begin(); itFace != itFaceEnd; ++itFace)
{
glBegin(GL_POLYGON);
vector<VertexId> vertexIds = (*itFace).second.GetVertexIds();
vector<VertexId>::iterator vItEnd = vertexIds.end();
for(vector<VertexId>::iterator vIt = vertexIds.begin() ;
vIt != vItEnd ;
vIt++)
{
Vertex vertex = m_Geom.GetVertex((*vIt));
glPushMatrix();
glVertex3f(vertex.GetPos().GetX(),
vertex.GetPos().GetY(),
vertex.GetPos().GetZ());
glPopMatrix();
}
glEnd();
}
*/
}
