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;
}
