float HalfEdgeMesh::FaceCurvature(unsigned int faceIndex) const { // NB Assumes vertex curvature already computed unsigned int indx = f(faceIndex).edge; const EdgeIterator it = GetEdgeIterator(indx); const Vertex& v1 = v(it.GetEdgeVertexIndex()); const Vertex &v2 = v(it.Next().GetEdgeVertexIndex()); const Vertex &v3 = v(it.Next().GetEdgeVertexIndex()); return (v1.curvature + v2.curvature + v3.curvature) / 3.f; }
bool EdgeIterator::operator ==(const EdgeIterator &other) const { if(isAtEnd() && other.isAtEnd()) return true; if(isAtEnd() || other.isAtEnd()) return false; return (*static_cast<const Inherited *>(this) == other ) && _actPrimIndex == other._actPrimIndex; }
Vector3<float> HalfEdgeMesh::FaceNormal(unsigned int faceIndex) const { unsigned int indx = f(faceIndex).edge; const EdgeIterator it = GetEdgeIterator(indx); const Vector3<float> &p1 = v(it.GetEdgeVertexIndex()).pos; const Vector3<float> &p2 = v(it.Next().GetEdgeVertexIndex()).pos; const Vector3<float> &p3 = v(it.Next().GetEdgeVertexIndex()).pos; const Vector3<float> e1 = p2-p1; const Vector3<float> e2 = p3-p1; return Cross(e1, e2).Normalize(); }
/*! Subdivides the face at faceIndex given 3 not subdividable neighbors or if subdividable for this face is false */ std::vector< std::vector<Vector3<float> > > AdaptiveLoopSubdivisionMesh::Subdivide3(unsigned int faceIndex){ EdgeIterator eit = GetEdgeIterator( f(faceIndex).edge ); Vector3<float> v1 = VertexRule(eit.GetEdgeVertexIndex()); Vector3<float> v2 = VertexRule(eit.Next().GetEdgeVertexIndex()); Vector3<float> v3 = VertexRule(eit.Next().GetEdgeVertexIndex()); std::vector<Vector3<float> > face; face.push_back(v1); face.push_back(v2); face.push_back(v3); std::vector< std::vector<Vector3<float> > > faces; faces.push_back(face); return faces; }
void ShortestPathVertex::compute (MessageIterator<int>* msgs) { int mindist = (superstep() == 0 && vertex_id() == 0) ? 0 : getValue(); for (; !msgs->done(); msgs->next()) mindist = min (mindist, msgs->getValue()); if (mindist < getValue()) { *mutableValue() = mindist; EdgeIterator<int> iter = getOutEdgeIterator(); for (; !iter.done(); iter.next()) sendMessageTo(iter.dest()->getDest(), mindist + iter.dest()->getValue()); } voteToHalt(); }
bool Graph::Node::reachableBy(Node *node, Node *term) { Stack stack; Node *pos; const int seq = graph->nextSequence(); stack.push(node); while (stack.getSize()) { pos = reinterpret_cast<Node *>(stack.pop().u.p); if (pos == this) return true; if (pos == term) continue; for (EdgeIterator ei = pos->outgoing(); !ei.end(); ei.next()) { if (ei.getType() == Edge::BACK || ei.getType() == Edge::DUMMY) continue; if (ei.getNode()->visit(seq)) stack.push(ei.getNode()); } } return pos == this; }
bool Graph::Node::reachableBy(const Node *node, const Node *term) const { std::stack<const Node *> stack; const Node *pos = NULL; const int seq = graph->nextSequence(); stack.push(node); while (!stack.empty()) { pos = stack.top(); stack.pop(); if (pos == this) return true; if (pos == term) continue; for (EdgeIterator ei = pos->outgoing(); !ei.end(); ei.next()) { if (ei.getType() == Edge::BACK || ei.getType() == Edge::DUMMY) continue; if (ei.getNode()->visit(seq)) stack.push(ei.getNode()); } } return pos == this; }
std::ostream& operator<<(std::ostream &o, const Sawyer::Container::Graph<V, E> &graph) { typedef const typename Sawyer::Container::Graph<V, E> Graph; typedef typename Graph::ConstVertexIterator VertexIterator; typedef typename Graph::ConstEdgeIterator EdgeIterator; typedef typename Graph::Vertex Vertex; typedef typename Graph::Edge Edge; o <<" vertices:\n"; for (size_t id=0; id<graph.nVertices(); ++id) { VertexIterator vertex = graph.findVertex(id); o <<" [" <<vertex->id() <<"] = " <<vertex->value() <<"\n"; BOOST_FOREACH (const Edge &edge, vertex->outEdges()) o <<" out edge #" <<edge.id() <<" to node #" <<edge.target()->id() <<" = " <<edge.value() <<"\n"; BOOST_FOREACH (const Edge &edge, vertex->inEdges()) o <<" in edge #" <<edge.id() <<" from node #" <<edge.source()->id() <<" = " <<edge.value() <<"\n"; } o <<" edges:\n"; for (size_t id=0; id<graph.nEdges(); ++id) { EdgeIterator edge = graph.findEdge(id); o <<" [" <<edge->id() <<"] = " <<edge->value() <<"\n"; o <<" from vertex [" <<edge->source()->id() <<"] = " <<edge->source()->value() <<"\n"; o <<" to vertex [" <<edge->target()->id() <<"] = " <<edge->target()->value() <<"\n"; } return o; }
/*! Subdivides the face at faceindex into a vector of faces */ std::vector< std::vector<Vector3<float> > > LoopSubdivisionMesh::Subdivide(unsigned int faceIndex) { std::vector< std::vector<Vector3<float> > > faces; EdgeIterator eit = GetEdgeIterator( f(faceIndex).edge ); // get the inner halfedges unsigned int e0, e1, e2; // and their vertex indices unsigned int v0, v1, v2; e0 = eit.GetEdgeIndex(); v0 = eit.GetEdgeVertexIndex(); eit.Next(); e1 = eit.GetEdgeIndex(); v1 = eit.GetEdgeVertexIndex(); eit.Next(); e2 = eit.GetEdgeIndex(); v2 = eit.GetEdgeVertexIndex(); // Compute positions of the vertices Vector3<float> pn0 = VertexRule(v0); Vector3<float> pn1 = VertexRule(v1); Vector3<float> pn2 = VertexRule(v2); // Compute positions of the edge vertices Vector3<float> pn3 = EdgeRule(e0); Vector3<float> pn4 = EdgeRule(e1); Vector3<float> pn5 = EdgeRule(e2); // add the four new triangles to new mesh std::vector<Vector3<float> > verts; verts.push_back(pn0); verts.push_back(pn3); verts.push_back(pn5); faces.push_back(verts); verts.clear(); verts.push_back(pn3); verts.push_back(pn4); verts.push_back(pn5); faces.push_back(verts); verts.clear(); verts.push_back(pn3); verts.push_back(pn1); verts.push_back(pn4); faces.push_back(verts); verts.clear(); verts.push_back(pn5); verts.push_back(pn4); verts.push_back(pn2); faces.push_back(verts); return faces; }
// @dist is indexed by Node::tag, returns -1 if no path found int Graph::findLightestPathWeight(Node *a, Node *b, const std::vector<int> &weight) { std::vector<int> path(weight.size(), std::numeric_limits<int>::max()); std::list<Node *> nodeList; const int seq = nextSequence(); path[a->tag] = 0; for (Node *c = a; c && c != b;) { const int p = path[c->tag] + weight[c->tag]; for (EdgeIterator ei = c->outgoing(); !ei.end(); ei.next()) { Node *t = ei.getNode(); if (t->getSequence() < seq) { if (path[t->tag] == std::numeric_limits<int>::max()) nodeList.push_front(t); if (p < path[t->tag]) path[t->tag] = p; } } c->visit(seq); Node *next = NULL; for (std::list<Node *>::iterator n = nodeList.begin(); n != nodeList.end(); ++n) { if (!next || path[(*n)->tag] < path[next->tag]) next = *n; if ((*n) == c) { // erase visited n = nodeList.erase(n); --n; } } c = next; } if (path[b->tag] == std::numeric_limits<int>::max()) return -1; return path[b->tag]; }
bool ContractionHierarchiesClient::GetRoute( double* distance, QVector< Node>* pathNodes, QVector< Edge >* pathEdges, const IGPSLookup::Result& source, const IGPSLookup::Result& target ) { assert( distance != NULL ); m_heapForward->Clear(); m_heapBackward->Clear(); *distance = computeRoute( source, target, pathNodes, pathEdges ); if ( *distance == std::numeric_limits< int >::max() ) return false; // is it shorter to drive along the edge? if ( target.source == source.source && target.target == source.target && source.edgeID == target.edgeID ) { EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID ); double onEdgeDistance = fabs( target.percentage - source.percentage ) * targetEdge.distance(); if ( onEdgeDistance < *distance ) { if ( ( targetEdge.forward() && targetEdge.backward() ) || source.percentage < target.percentage ) { if ( pathNodes != NULL && pathEdges != NULL ) { pathNodes->clear(); pathEdges->clear(); pathNodes->push_back( source.nearestPoint ); QVector< Node > tempNodes; if ( targetEdge.unpacked() ) m_graph.path( targetEdge, &tempNodes, pathEdges, target.target == targetEdge.target() ); else pathEdges->push_back( targetEdge.description() ); if ( target.previousWayCoordinates < source.previousWayCoordinates ) { for ( unsigned pathID = target.previousWayCoordinates; pathID < source.previousWayCoordinates; pathID++ ) pathNodes->push_back( tempNodes[pathID - 1] ); std::reverse( pathNodes->begin() + 1, pathNodes->end() ); } else { for ( unsigned pathID = source.previousWayCoordinates; pathID < target.previousWayCoordinates; pathID++ ) pathNodes->push_back( tempNodes[pathID - 1] ); } pathNodes->push_back( target.nearestPoint ); pathEdges->front().length = pathNodes->size() - 1; } *distance = onEdgeDistance; } } } *distance /= 10; return true; }
bool Graph::Node::detach(Graph::Node *node) { EdgeIterator ei = this->outgoing(); for (; !ei.end(); ei.next()) if (ei.getNode() == node) break; if (ei.end()) { ERROR("no such node attached\n"); return false; } delete ei.getEdge(); return true; }
/*! Subdivides the face at faceIndex given 2 not subdividable neighbors */ std::vector< std::vector<Vector3<float> > > AdaptiveLoopSubdivisionMesh::Subdivide2(unsigned int faceIndex){ // We know we have otwo false faces // 1. Start by orienting the triangle so that we always have the same case // We find the start edge as the edge who shares face with the _subdividable_ neighbor EdgeIterator eit = GetEdgeIterator( f(faceIndex).edge ); unsigned int num = 0; while( !Subdividable(eit.Pair().GetEdgeFaceIndex()) ){ eit.Pair().Next(); assert(num++ < 3); } // go back to inner edge eit.Pair(); // 2. Now find the vertices std::vector< std::vector<Vector3<float> > > faces; // corner vertices, labeled from current edge's origin vertex Vector3<float> v1 = VertexRule(eit.GetEdgeVertexIndex()); Vector3<float> v2 = VertexRule(eit.Next().GetEdgeVertexIndex()); Vector3<float> v3 = VertexRule(eit.Next().GetEdgeVertexIndex()); // edge vertices, labeled from start edge Vector3<float> e1v = EdgeRule( eit.Next().GetEdgeIndex() ); // 3. Create the 2 faces and push them on the vector std::vector<Vector3<float> > face; face.push_back(v1); face.push_back(e1v); face.push_back(v3); faces.push_back(face); face.clear(); face.push_back(e1v); face.push_back(v2); face.push_back(v3); faces.push_back(face); face.clear(); // 4. Return return faces; }
void ContractionHierarchiesClient::computeStep( Heap* heapForward, Heap* heapBackward, const EdgeAllowed& edgeAllowed, const StallEdgeAllowed& stallEdgeAllowed, NodeIterator* middle, int* targetDistance ) { const NodeIterator node = heapForward->DeleteMin(); const int distance = heapForward->GetKey( node ); if ( heapForward->GetData( node ).stalled ) return; if ( heapBackward->WasInserted( node ) && !heapBackward->GetData( node ).stalled ) { const int newDistance = heapBackward->GetKey( node ) + distance; if ( newDistance < *targetDistance ) { *middle = node; *targetDistance = newDistance; } } if ( distance > *targetDistance ) { heapForward->DeleteAll(); return; } for ( EdgeIterator edge = m_graph.edges( node ); edge.hasEdgesLeft(); ) { m_graph.unpackNextEdge( &edge ); const NodeIterator to = edge.target(); const int edgeWeight = edge.distance(); assert( edgeWeight > 0 ); const int toDistance = distance + edgeWeight; if ( stallEdgeAllowed( edge.forward(), edge.backward() ) && heapForward->WasInserted( to ) ) { const int shorterDistance = heapForward->GetKey( to ) + edgeWeight; if ( shorterDistance < distance ) { //perform a bfs starting at node //only insert nodes when a sub-optimal path can be proven //insert node into the stall queue heapForward->GetKey( node ) = shorterDistance; heapForward->GetData( node ).stalled = true; m_stallQueue.push( node ); while ( !m_stallQueue.empty() ) { //get node from the queue const NodeIterator stallNode = m_stallQueue.front(); m_stallQueue.pop(); const int stallDistance = heapForward->GetKey( stallNode ); //iterate over outgoing edges for ( EdgeIterator stallEdge = m_graph.edges( stallNode ); stallEdge.hasEdgesLeft(); ) { m_graph.unpackNextEdge( &stallEdge ); //is edge outgoing/reached/stalled? if ( !edgeAllowed( stallEdge.forward(), stallEdge.backward() ) ) continue; const NodeIterator stallTo = stallEdge.target(); if ( !heapForward->WasInserted( stallTo ) ) continue; if ( heapForward->GetData( stallTo ).stalled == true ) continue; const int stallToDistance = stallDistance + stallEdge.distance(); //sub-optimal path found -> insert stallTo if ( stallToDistance < heapForward->GetKey( stallTo ) ) { if ( heapForward->WasRemoved( stallTo ) ) heapForward->GetKey( stallTo ) = stallToDistance; else heapForward->DecreaseKey( stallTo, stallToDistance ); m_stallQueue.push( stallTo ); heapForward->GetData( stallTo ).stalled = true; } } } break; } } if ( edgeAllowed( edge.forward(), edge.backward() ) ) { //New Node discovered -> Add to Heap + Node Info Storage if ( !heapForward->WasInserted( to ) ) heapForward->Insert( to, toDistance, node ); //Found a shorter Path -> Update distance else if ( toDistance <= heapForward->GetKey( to ) ) { heapForward->DecreaseKey( to, toDistance ); //new parent + unstall heapForward->GetData( to ).parent = node; heapForward->GetData( to ).stalled = false; } } } }
/*! Subdivides the mesh one step, depending on subdividability */ void AdaptiveLoopSubdivisionMesh::Subdivide() { // Create new mesh and copy all the attributes HalfEdgeMesh subDivMesh; subDivMesh.SetTransform(GetTransform()); subDivMesh.SetName(GetName()); subDivMesh.SetColorMap(GetColorMap()); subDivMesh.SetWireframe(GetWireframe()); subDivMesh.SetShowNormals(GetShowNormals()); subDivMesh.SetOpacity(GetOpacity()); if (IsHovering()) subDivMesh.Hover(); if (IsSelected()) subDivMesh.Select(); subDivMesh.mMinCMap = mMinCMap; subDivMesh.mMaxCMap = mMaxCMap; subDivMesh.mAutoMinMax = mAutoMinMax; // loop over each face and create new ones for(unsigned int i=0; i<GetNumFaces(); i++){ // find neighbor faces unsigned int f1, f2, f3; EdgeIterator eit = GetEdgeIterator( f(i).edge ); f1 = eit.Pair().GetEdgeFaceIndex(); eit.Pair(); f2 = eit.Next().Pair().GetEdgeFaceIndex(); eit.Pair(); f3 = eit.Next().Pair().GetEdgeFaceIndex(); unsigned int numNotSubdividable = !Subdividable(f1) + !Subdividable(f2) + !Subdividable(f3); // Do not subdivide if "self" is not subdividable if(!Subdividable(i)){ numNotSubdividable = 3; } std::vector< std::vector <Vector3<float> > > faces; switch(numNotSubdividable){ case 0: // normal subdivision (from LoopSubdivisionMesh) faces = LoopSubdivisionMesh::Subdivide(i); break; case 1: // special case 1 faces = Subdivide1(i); break; case 2: // special case 2 faces = Subdivide2(i); break; case 3: // trivial case, no subdivision, same as if subdividable(fi) == false faces = Subdivide3(i); break; } // add the faces (if any) to subDivMesh for(unsigned int j=0; j<faces.size(); j++){ subDivMesh.AddFace(faces.at(j)); } } // Assign the new mesh *this = AdaptiveLoopSubdivisionMesh(subDivMesh, ++mNumSubDivs); Update(); }
bool EdgeIterator::operator==(const EdgeIterator &rhs) const { return eit().pos == rhs.eit().pos; }
bool ContractionHierarchiesClient::unpackEdge( const NodeIterator source, const NodeIterator target, bool forward, QVector< Node >* pathNodes, QVector< Edge >* pathEdges ) { EdgeIterator shortestEdge; unsigned distance = std::numeric_limits< unsigned >::max(); for ( EdgeIterator edge = m_graph.edges( source ); edge.hasEdgesLeft(); ) { m_graph.unpackNextEdge( &edge ); if ( edge.target() != target ) continue; if ( forward && !edge.forward() ) continue; if ( !forward && !edge.backward() ) continue; if ( edge.distance() > distance ) continue; distance = edge.distance(); shortestEdge = edge; } if ( shortestEdge.unpacked() ) { m_graph.path( shortestEdge, pathNodes, pathEdges, forward ); return true; } if ( !shortestEdge.shortcut() ) { pathEdges->push_back( shortestEdge.description() ); if ( forward ) pathNodes->push_back( m_graph.node( target ).coordinate ); else pathNodes->push_back( m_graph.node( source ).coordinate ); return true; } const NodeIterator middle = shortestEdge.middle(); if ( forward ) { unpackEdge( middle, source, false, pathNodes, pathEdges ); unpackEdge( middle, target, true, pathNodes, pathEdges ); return true; } else { unpackEdge( middle, target, false, pathNodes, pathEdges ); unpackEdge( middle, source, true, pathNodes, pathEdges ); return true; } }
int ContractionHierarchiesClient::computeRoute( const IGPSLookup::Result& source, const IGPSLookup::Result& target, QVector< Node>* pathNodes, QVector< Edge >* pathEdges ) { EdgeIterator sourceEdge = m_graph.findEdge( source.source, source.target, source.edgeID ); unsigned sourceWeight = sourceEdge.distance(); EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID ); unsigned targetWeight = targetEdge.distance(); //insert source into heap m_heapForward->Insert( source.target, sourceWeight - sourceWeight * source.percentage, source.target ); if ( sourceEdge.backward() && sourceEdge.forward() && source.target != source.source ) m_heapForward->Insert( source.source, sourceWeight * source.percentage, source.source ); //insert target into heap m_heapBackward->Insert( target.source, targetWeight * target.percentage, target.source ); if ( targetEdge.backward() && targetEdge.forward() && target.target != target.source ) m_heapBackward->Insert( target.target, targetWeight - targetWeight * target.percentage, target.target ); int targetDistance = std::numeric_limits< int >::max(); NodeIterator middle = ( NodeIterator ) 0; AllowForwardEdge forward; AllowBackwardEdge backward; while ( m_heapForward->Size() + m_heapBackward->Size() > 0 ) { if ( m_heapForward->Size() > 0 ) computeStep( m_heapForward, m_heapBackward, forward, backward, &middle, &targetDistance ); if ( m_heapBackward->Size() > 0 ) computeStep( m_heapBackward, m_heapForward, backward, forward, &middle, &targetDistance ); } if ( targetDistance == std::numeric_limits< int >::max() ) return std::numeric_limits< int >::max(); // abort early if the path description is not requested if ( pathNodes == NULL || pathEdges == NULL ) return targetDistance; std::stack< NodeIterator > stack; NodeIterator pathNode = middle; while ( true ) { NodeIterator parent = m_heapForward->GetData( pathNode ).parent; stack.push( pathNode ); if ( parent == pathNode ) break; pathNode = parent; } pathNodes->push_back( source.nearestPoint ); bool reverseSourceDescription = pathNode != source.target; if ( source.source == source.target && sourceEdge.backward() && sourceEdge.forward() && source.percentage < 0.5 ) reverseSourceDescription = !reverseSourceDescription; if ( sourceEdge.unpacked() ) { bool unpackSourceForward = source.target != sourceEdge.target() ? reverseSourceDescription : !reverseSourceDescription; m_graph.path( sourceEdge, pathNodes, pathEdges, unpackSourceForward ); if ( reverseSourceDescription ) { pathNodes->remove( 1, pathNodes->size() - 1 - source.previousWayCoordinates ); } else { pathNodes->remove( 1, source.previousWayCoordinates - 1 ); } } else { pathNodes->push_back( m_graph.node( pathNode ) ); pathEdges->push_back( sourceEdge.description() ); } pathEdges->front().length = pathNodes->size() - 1; pathEdges->front().seconds *= reverseSourceDescription ? source.percentage : 1 - source.percentage; while ( stack.size() > 1 ) { const NodeIterator node = stack.top(); stack.pop(); unpackEdge( node, stack.top(), true, pathNodes, pathEdges ); } pathNode = middle; while ( true ) { NodeIterator parent = m_heapBackward->GetData( pathNode ).parent; if ( parent == pathNode ) break; unpackEdge( parent, pathNode, false, pathNodes, pathEdges ); pathNode = parent; } int begin = pathNodes->size(); bool reverseTargetDescription = pathNode != target.source; if ( target.source == target.target && targetEdge.backward() && targetEdge.forward() && target.percentage > 0.5 ) reverseTargetDescription = !reverseTargetDescription; if ( targetEdge.unpacked() ) { bool unpackTargetForward = target.target != targetEdge.target() ? reverseTargetDescription : !reverseTargetDescription; m_graph.path( targetEdge, pathNodes, pathEdges, unpackTargetForward ); if ( reverseTargetDescription ) { pathNodes->resize( pathNodes->size() - target.previousWayCoordinates ); } else { pathNodes->resize( begin + target.previousWayCoordinates - 1 ); } } else { pathEdges->push_back( targetEdge.description() ); } pathNodes->push_back( target.nearestPoint ); pathEdges->back().length = pathNodes->size() - begin; pathEdges->back().seconds *= reverseTargetDescription ? 1 - target.percentage : target.percentage; return targetDistance; }