janwas
5bf9bca9ef
there are too many W4 and "potentially uninitialized", so those are disabled by 0ad_warning_disable.h. the silly "int x = strlen" and very dangerous "int x = (void*)p" (and vice versa) problems are fixed. This was SVN commit r5526.
665 lines
17 KiB
C++
665 lines
17 KiB
C++
#include "precompiled.h"
|
|
#include "0ad_warning_disable.h"
|
|
# include <stdlib.h>
|
|
# include "sr_graph.h"
|
|
# include "sr_heap.h"
|
|
|
|
//============================== SrGraphLink ===============================================
|
|
|
|
|
|
|
|
|
|
//============================== SrGraphNode ===============================================
|
|
|
|
SrGraphNode::~SrGraphNode ()
|
|
{
|
|
SrClassManagerBase* lman = _graph->link_class_manager();
|
|
if ( _links.size()>0 )
|
|
lman->free ( _links.pop() );
|
|
}
|
|
|
|
SrGraphLink* SrGraphNode::linkto ( SrGraphNode* n, float cost )
|
|
{
|
|
SrGraphLink* l = (SrGraphLink*) _graph->link_class_manager()->alloc();
|
|
l->_node = n;
|
|
l->_index = 0;
|
|
l->_cost = cost;
|
|
|
|
_links.push () = l;
|
|
|
|
return l;
|
|
}
|
|
|
|
void SrGraphNode::unlink ( int ni )
|
|
{
|
|
SrClassManagerBase* lman = _graph->link_class_manager();
|
|
lman->free ( _links[ni] );
|
|
_links[ni] = _links.pop();
|
|
}
|
|
|
|
int SrGraphNode::search_link ( SrGraphNode* n ) const
|
|
{
|
|
int i;
|
|
for ( i=0; i<_links.size(); i++ )
|
|
if ( _links[i]->node()==n ) return i;
|
|
return -1;
|
|
}
|
|
|
|
void SrGraphNode::output ( SrOutput& o ) const
|
|
{
|
|
float cost;
|
|
int i, max;
|
|
max = _links.size()-1;
|
|
if ( max<0 ) return;
|
|
|
|
SrClassManagerBase* lman = _graph->link_class_manager();
|
|
|
|
o<<'(';
|
|
for ( i=0; i<=max; i++ )
|
|
{ // output blocked status
|
|
o << _links[i]->_blocked << srspc;
|
|
|
|
// output index to target node
|
|
o << _links[i]->_node->_index << srspc;
|
|
|
|
// output link cost
|
|
cost = _links[i]->_cost;
|
|
if ( cost==SR_TRUNC(cost) ) o << int(cost);
|
|
else o << cost;
|
|
|
|
// output user data
|
|
o << srspc;
|
|
lman->output(o,_links[i]);
|
|
|
|
if ( i<max ) o<<srspc;
|
|
}
|
|
o<<')';
|
|
}
|
|
|
|
//============================== SrGraphPathTree ===============================================
|
|
|
|
class SrGraphPathTree
|
|
{ public :
|
|
struct Node { int parent; float cost; SrGraphNode* node; };
|
|
struct Leaf { int l; int d; }; // the leaf index in nodes array, and its depth
|
|
SrArray<Node> nodes;
|
|
SrHeap<Leaf,float> leafs;
|
|
SrGraphBase* graph;
|
|
SrGraphNode* closest;
|
|
int iclosest;
|
|
float cdist;
|
|
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata );
|
|
void *udata;
|
|
bool bidirectional_block;
|
|
|
|
public :
|
|
SrGraphPathTree ()
|
|
{ bidirectional_block = false;
|
|
}
|
|
|
|
void init ( SrGraphBase* g, SrGraphNode* n )
|
|
{ nodes.size(1);
|
|
nodes[0].parent = -1;
|
|
nodes[0].cost = 0;
|
|
nodes[0].node = n;
|
|
Leaf l;
|
|
l.l = l.d = 0;
|
|
leafs.insert ( l, 0 );
|
|
graph = g;
|
|
distfunc=0;
|
|
udata = 0;
|
|
closest = 0;
|
|
iclosest = 0;
|
|
cdist = 0;
|
|
}
|
|
|
|
bool has_leaf ()
|
|
{ return leafs.size()==0? false:true;
|
|
}
|
|
|
|
bool expand_lowest_cost_leaf ( SrGraphNode* goalnode )
|
|
{ int i;
|
|
int n = leafs.top().l;
|
|
int d = leafs.top().d;
|
|
leafs.remove ();
|
|
SrGraphNode* node = nodes[n].node;
|
|
Leaf leaf;
|
|
const SrArray<SrGraphLink*>& a = node->links();
|
|
for ( i=0; i<a.size(); i++ )
|
|
{ //sr_out<<a.size()<<srnl;
|
|
if ( graph->marked(a[i]) ||
|
|
a[i]->blocked() ||
|
|
a[i]->node()->blocked() ) continue;
|
|
if ( bidirectional_block )
|
|
{ if ( a[i]->node()->link(node)->blocked() ) continue; }
|
|
nodes.push();
|
|
nodes.top().parent = n;
|
|
nodes.top().cost = nodes[n].cost + a[i]->cost();
|
|
nodes.top().node = a[i]->node();
|
|
leaf.l = nodes.size()-1;
|
|
leaf.d = d+1;
|
|
leafs.insert ( leaf, nodes.top().cost );
|
|
graph->mark ( a[i] );
|
|
if ( distfunc )
|
|
{ float d = distfunc ( nodes.top().node, goalnode, udata );
|
|
if ( !closest || d<cdist )
|
|
{ closest=nodes.top().node; iclosest=nodes.size()-1; cdist=d; }
|
|
}
|
|
if ( a[i]->node()==goalnode ) return true;
|
|
}
|
|
return false;
|
|
}
|
|
};
|
|
|
|
//============================== SrGraphBase ===============================================
|
|
|
|
# define MARKFREE 0
|
|
# define MARKING 1
|
|
# define INDEXING 2
|
|
|
|
SrGraphBase::SrGraphBase ( SrClassManagerBase* nm, SrClassManagerBase* lm )
|
|
:_nodes(nm)
|
|
{
|
|
_curmark = 1;
|
|
_mark_status = MARKFREE;
|
|
_pt = 0; // allocated only if shortest path is called
|
|
_lman = lm;
|
|
_lman->ref(); // nm is managed by the list _nodes
|
|
_leave_indices_after_save = 0;
|
|
}
|
|
|
|
SrGraphBase::~SrGraphBase ()
|
|
{
|
|
_nodes.init (); // Important: this ensures that _lman is used before _lman->unref()
|
|
delete _pt;
|
|
_lman->unref();
|
|
}
|
|
|
|
void SrGraphBase::init ()
|
|
{
|
|
_nodes.init();
|
|
_curmark = 1;
|
|
_mark_status = MARKFREE;
|
|
}
|
|
|
|
void SrGraphBase::compress ()
|
|
{
|
|
if ( _nodes.empty() ) return;
|
|
_nodes.gofirst();
|
|
do { _nodes.cur()->compress();
|
|
} while ( _nodes.notlast() );
|
|
}
|
|
|
|
int SrGraphBase::num_links () const
|
|
{
|
|
if ( _nodes.empty() ) return 0;
|
|
|
|
int n=0;
|
|
|
|
SrGraphNode* first = _nodes.first();
|
|
SrGraphNode* cur = first;
|
|
do { n += cur->num_links();
|
|
cur = cur->next();
|
|
} while ( cur!=first );
|
|
|
|
return n;
|
|
}
|
|
|
|
//----------------------------------- marking --------------------------------
|
|
|
|
void SrGraphBase::begin_marking ()
|
|
{
|
|
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_mark() is locked!");
|
|
_mark_status = MARKING;
|
|
if ( _curmark==sruintmax ) _normalize_mark ();
|
|
else _curmark++;
|
|
}
|
|
|
|
void SrGraphBase::end_marking ()
|
|
{
|
|
_mark_status=MARKFREE;
|
|
}
|
|
|
|
bool SrGraphBase::marked ( SrGraphNode* n )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(n): marking is not active!\n" );
|
|
return n->_index==_curmark? true:false;
|
|
}
|
|
|
|
void SrGraphBase::mark ( SrGraphNode* n )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(n): marking is not active!\n" );
|
|
n->_index = _curmark;
|
|
}
|
|
|
|
void SrGraphBase::unmark ( SrGraphNode* n )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(n): marking is not active!\n");
|
|
n->_index = _curmark-1;
|
|
}
|
|
|
|
bool SrGraphBase::marked ( SrGraphLink* l )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::marked(l): marking is not active!\n" );
|
|
return l->_index==_curmark? true:false;
|
|
}
|
|
|
|
void SrGraphBase::mark ( SrGraphLink* l )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::mark(l): marking is not active!\n" );
|
|
l->_index = _curmark;
|
|
}
|
|
|
|
void SrGraphBase::unmark ( SrGraphLink* l )
|
|
{
|
|
if ( _mark_status!=MARKING ) sr_out.fatal_error ( "SrGraphBase::unmark(l): marking is not active!\n");
|
|
l->_index = _curmark-1;
|
|
}
|
|
|
|
//----------------------------------- indexing --------------------------------
|
|
|
|
void SrGraphBase::begin_indexing ()
|
|
{
|
|
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::begin_indexing() is locked!");
|
|
_mark_status = INDEXING;
|
|
}
|
|
|
|
void SrGraphBase::end_indexing ()
|
|
{
|
|
_normalize_mark ();
|
|
_mark_status=MARKFREE;
|
|
}
|
|
|
|
sruint SrGraphBase::index ( SrGraphNode* n )
|
|
{
|
|
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n): indexing is not active!");
|
|
return n->_index;
|
|
}
|
|
|
|
void SrGraphBase::index ( SrGraphNode* n, sruint i )
|
|
{
|
|
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(n,i): indexing is not active!");
|
|
n->_index = i;
|
|
}
|
|
|
|
sruint SrGraphBase::index ( SrGraphLink* l )
|
|
{
|
|
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l): indexing is not active!");
|
|
return l->_index;
|
|
}
|
|
|
|
void SrGraphBase::index ( SrGraphLink* l, sruint i )
|
|
{
|
|
if ( _mark_status!=INDEXING ) sr_out.fatal_error ("SrGraphBase::index(l,i): indexing is not active!");
|
|
l->_index = i;
|
|
}
|
|
|
|
//----------------------------------- construction --------------------------------
|
|
|
|
SrGraphNode* SrGraphBase::insert ( SrGraphNode* n )
|
|
{
|
|
_nodes.insert_next ( n );
|
|
n->_graph = this;
|
|
return n;
|
|
}
|
|
|
|
SrGraphNode* SrGraphBase::extract ( SrGraphNode* n )
|
|
{
|
|
_nodes.cur(n);
|
|
return _nodes.extract();
|
|
}
|
|
|
|
void SrGraphBase::remove_node ( SrGraphNode* n )
|
|
{
|
|
_nodes.cur(n);
|
|
_nodes.remove();
|
|
}
|
|
|
|
int SrGraphBase::remove_link ( SrGraphNode* n1, SrGraphNode* n2 )
|
|
{
|
|
int i;
|
|
int n=0;
|
|
|
|
while ( true )
|
|
{ i = n1->search_link(n2);
|
|
if ( i<0 ) break;
|
|
n++;
|
|
n1->unlink(i);
|
|
}
|
|
|
|
while ( true )
|
|
{ i = n2->search_link(n1);
|
|
if ( i<0 ) break;
|
|
n++;
|
|
n2->unlink(i);
|
|
}
|
|
|
|
return n;
|
|
}
|
|
|
|
void SrGraphBase::link ( SrGraphNode* n1, SrGraphNode* n2, float c )
|
|
{
|
|
n1->linkto(n2,c);
|
|
n2->linkto(n1,c);
|
|
}
|
|
|
|
//----------------------------------- get edges ----------------------------------
|
|
|
|
void SrGraphBase::get_directed_edges ( SrArray<SrGraphNode*>& edges )
|
|
{
|
|
edges.size ( num_nodes() );
|
|
edges.size ( 0 );
|
|
|
|
int i;
|
|
SrGraphNode* n;
|
|
SrListIterator<SrGraphNode> it(_nodes);
|
|
|
|
for ( it.first(); it.inrange(); it.next() )
|
|
{ n = it.get();
|
|
for ( i=0; i<n->num_links(); i++ )
|
|
{ edges.push() = n;
|
|
edges.push() = n->link(i)->node();
|
|
}
|
|
}
|
|
}
|
|
|
|
void SrGraphBase::get_undirected_edges ( SrArray<SrGraphNode*>& edges )
|
|
{
|
|
edges.size ( num_nodes() );
|
|
edges.size ( 0 );
|
|
|
|
int i, li;
|
|
SrGraphNode* n;
|
|
SrListIterator<SrGraphNode> it(_nodes);
|
|
|
|
begin_marking();
|
|
|
|
for ( it.first(); it.inrange(); it.next() )
|
|
{ n = it.get();
|
|
for ( i=0; i<n->links().size(); i++ )
|
|
{ if ( !marked(n->link(i)) )
|
|
{ edges.push() = n;
|
|
edges.push() = n->link(i)->node();
|
|
mark ( n->link(i) );
|
|
li = edges.top()->search_link(n);
|
|
if ( li>=0 ) mark ( edges.top()->link(li) );
|
|
}
|
|
}
|
|
}
|
|
|
|
end_marking();
|
|
}
|
|
|
|
//----------------------------------- components ----------------------------------
|
|
|
|
static void _traverse ( SrGraphBase* graph,
|
|
SrArray<SrGraphNode*>& stack,
|
|
SrArray<SrGraphNode*>& nodes )
|
|
{
|
|
int i;
|
|
SrGraphNode *n, *ln;
|
|
while ( stack.size()>0 )
|
|
{ n = stack.pop();
|
|
graph->mark ( n );
|
|
nodes.push() = n;
|
|
for ( i=0; i<n->num_links(); i++ )
|
|
{ ln = n->link(i)->node();
|
|
if ( !graph->marked(ln) ) stack.push()=ln;
|
|
}
|
|
}
|
|
}
|
|
|
|
void SrGraphBase::get_connected_nodes ( SrGraphNode* source, SrArray<SrGraphNode*>& nodes )
|
|
{
|
|
nodes.size ( num_nodes() );
|
|
nodes.size ( 0 );
|
|
|
|
SrArray<SrGraphNode*>& stack = _buffer;
|
|
stack.size ( 0 );
|
|
|
|
begin_marking();
|
|
stack.push() = source;
|
|
_traverse ( this, stack, nodes );
|
|
end_marking();
|
|
}
|
|
|
|
void SrGraphBase::get_disconnected_components ( SrArray<int>& components, SrArray<SrGraphNode*>& nodes )
|
|
{
|
|
nodes.size ( num_nodes() );
|
|
nodes.size ( 0 );
|
|
|
|
components.size ( 0 );
|
|
|
|
SrArray<SrGraphNode*>& stack = _buffer;
|
|
stack.size ( 0 );
|
|
|
|
SrGraphNode* n;
|
|
SrListIterator<SrGraphNode> it(_nodes);
|
|
|
|
begin_marking();
|
|
|
|
for ( it.first(); it.inrange(); it.next() )
|
|
{ n = it.get();
|
|
if ( !marked(n) )
|
|
{ components.push() = nodes.size();
|
|
stack.push() = n;
|
|
_traverse ( this, stack, nodes );
|
|
components.push() = nodes.size()-1;
|
|
}
|
|
}
|
|
|
|
end_marking();
|
|
}
|
|
|
|
//----------------------------------- shortest path ----------------------------------
|
|
|
|
float SrGraphBase::get_shortest_path ( SrGraphNode* n1,
|
|
SrGraphNode* n2,
|
|
SrArray<SrGraphNode*>& path,
|
|
float (*distfunc) ( const SrGraphNode*, const SrGraphNode*, void* udata ),
|
|
void* udata )
|
|
{
|
|
path.size(0);
|
|
if ( n1==n2 ) { path.push()=n1; return 0; }
|
|
|
|
begin_marking ();
|
|
|
|
if ( !_pt ) _pt = new SrGraphPathTree;
|
|
_pt->init ( this, n2 );
|
|
|
|
if ( distfunc )
|
|
{ _pt->distfunc = distfunc;
|
|
_pt->udata = udata;
|
|
}
|
|
|
|
int i;
|
|
bool end = false;
|
|
|
|
while ( !end )
|
|
{ if ( !_pt->has_leaf() )
|
|
{ end_marking();
|
|
if ( !distfunc ) return 0;
|
|
i = _pt->iclosest;
|
|
float cost = _pt->nodes[i].cost;
|
|
while ( i>=0 )
|
|
{ path.push () = _pt->nodes[i].node;
|
|
i = _pt->nodes[i].parent;
|
|
}
|
|
return cost;
|
|
}
|
|
|
|
end = _pt->expand_lowest_cost_leaf ( n1 );
|
|
}
|
|
|
|
end_marking ();
|
|
|
|
i = _pt->nodes.size()-1; // last element is n1
|
|
float cost = _pt->nodes[i].cost;
|
|
while ( i>=0 )
|
|
{ path.push () = _pt->nodes[i].node;
|
|
i = _pt->nodes[i].parent;
|
|
}
|
|
|
|
return cost;
|
|
}
|
|
|
|
bool SrGraphBase::local_search
|
|
( SrGraphNode* startn,
|
|
SrGraphNode* endn,
|
|
int maxdepth,
|
|
float maxdist,
|
|
int& depth,
|
|
float& dist )
|
|
{
|
|
depth=0;
|
|
dist=0;
|
|
|
|
if ( startn==endn ) return true;
|
|
|
|
begin_marking ();
|
|
|
|
if ( !_pt ) _pt = new SrGraphPathTree;
|
|
_pt->init ( this, startn );
|
|
|
|
int i;
|
|
bool not_found = false;
|
|
bool end = false;
|
|
|
|
while ( !end )
|
|
{ if ( !_pt->has_leaf() ) { not_found=true; break; } // not found!
|
|
|
|
dist = _pt->nodes[_pt->leafs.top().l].cost;
|
|
depth = _pt->leafs.top().d;
|
|
|
|
if ( maxdepth>0 && depth>maxdepth ) { i=-1; break; } // max depth reached
|
|
if ( maxdist>0 && dist>maxdist ) { i=-1; break; } // max dist reached
|
|
|
|
end = _pt->expand_lowest_cost_leaf ( endn );
|
|
}
|
|
|
|
end_marking ();
|
|
|
|
if ( not_found ) return false; // not found!
|
|
return true;
|
|
}
|
|
|
|
void SrGraphBase::bidirectional_block_test ( bool b )
|
|
{
|
|
if ( !_pt ) _pt = new SrGraphPathTree;
|
|
_pt->bidirectional_block = b;
|
|
}
|
|
|
|
//------------------------------------- I/O --------------------------------
|
|
|
|
void SrGraphBase::output ( SrOutput& o )
|
|
{
|
|
SrListIterator<SrGraphNode> it(_nodes);
|
|
SrClassManagerBase* nman = node_class_manager();
|
|
|
|
// set indices
|
|
if ( _mark_status!=MARKFREE ) sr_out.fatal_error("SrGraphBase::operator<<(): begin_indexing() is locked!");
|
|
sruint i=0;
|
|
begin_indexing();
|
|
for ( it.first(); it.inrange(); it.next() ) index(it.get(),i++);
|
|
|
|
// print
|
|
o<<'[';
|
|
for ( it.first(); it.inrange(); it.next() )
|
|
{ o << it->index() << srspc; // output node index
|
|
o << it->_blocked << srspc; // output node blocked status
|
|
nman->output ( o, it.get() ); // output user data
|
|
if ( it.get()->num_links()>0 ) // output node links (blocked, ids, cost, and udata)
|
|
{ o<<srspc;
|
|
it->SrGraphNode::output ( o );
|
|
}
|
|
if ( !it.inlast() ) o << srnl;
|
|
}
|
|
o<<']';
|
|
|
|
if ( !_leave_indices_after_save ) end_indexing();
|
|
_leave_indices_after_save = 0;
|
|
}
|
|
|
|
static void set_blocked ( int& blocked, const char* last_token )
|
|
{
|
|
if ( last_token[0]=='b' ) // back-compatibility
|
|
blocked = 1;
|
|
else if ( last_token[0]=='f' ) // back-compatibility
|
|
blocked = 0;
|
|
else // should be an integer
|
|
blocked = atoi(last_token);
|
|
}
|
|
|
|
void SrGraphBase::input ( SrInput& inp )
|
|
{
|
|
SrArray<SrGraphNode*>& nodes = _buffer;
|
|
nodes.size(128);
|
|
nodes.size(0);
|
|
|
|
SrClassManagerBase* nman = node_class_manager();
|
|
SrClassManagerBase* lman = link_class_manager();
|
|
|
|
init ();
|
|
|
|
inp.getd(); // [
|
|
inp.get_token(); // get node counter (which is not needed)
|
|
|
|
while ( inp.last_token()[0]!=']' )
|
|
{
|
|
nodes.push() = _nodes.insert_next(); // allocate one node
|
|
nodes.top()->_graph = this;
|
|
|
|
inp.get_token(); // get node blocked status
|
|
set_blocked ( nodes.top()->_blocked, inp.last_token() );
|
|
|
|
nman->input ( inp, nodes.top() ); // read node user data
|
|
|
|
inp.get_token(); // new node counter, or '(', or ']'
|
|
|
|
if ( inp.last_token()[0]=='(')
|
|
while ( true )
|
|
{ inp.get_token();
|
|
if ( inp.last_token()[0]==')' ) { inp.get_token(); break; }
|
|
SrArray<SrGraphLink*>& la = nodes.top()->_links;
|
|
la.push() = (SrGraphLink*) lman->alloc();
|
|
set_blocked ( la.top()->_blocked, inp.last_token() ); // get link blocked status
|
|
|
|
inp.get_token(); // get id
|
|
la.top()->_index = atoi(inp.last_token()); // store id
|
|
inp.getn(); // get cost
|
|
la.top()->_cost = (float) atof(inp.last_token()); // store cost
|
|
lman->input ( inp, la.top() );
|
|
}
|
|
}
|
|
|
|
// now convert indices to pointers:
|
|
int i, j;
|
|
for ( i=0; i<nodes.size(); i++ )
|
|
{ SrArray<SrGraphLink*>& la = nodes[i]->_links;
|
|
for ( j=0; j<la.size(); j++ )
|
|
{ la[j]->_node = nodes[ int(la[j]->_index) ];
|
|
}
|
|
}
|
|
}
|
|
|
|
//---------------------------- private methods --------------------------------
|
|
|
|
// set all indices (nodes and links) to 0 and curmark to 1
|
|
void SrGraphBase::_normalize_mark()
|
|
{
|
|
int i;
|
|
SrGraphNode* n;
|
|
|
|
if ( _nodes.empty() ) return;
|
|
|
|
_nodes.gofirst();
|
|
do { n = _nodes.cur();
|
|
n->_index = 0;
|
|
for ( i=0; i<n->num_links(); i++ ) n->link(i)->_index=0;
|
|
} while ( _nodes.notlast() );
|
|
_curmark = 1;
|
|
}
|
|
|
|
//============================== end of file ===============================
|
|
|