最短路徑算法實現(xiàn)！


#ifndef _topo_h__
#define _topo_h__
//////////////////////////////////////////////////////////////////////////
#define MAXWEIGHT 1000000000
#define DIFFVAL(v1, v2) (((v1) > (v2))? (v1)-(v2) : (v2)-(v1))

#pragma warning(disable:4786)
#include <vector>
using namespace std;


struct topo_node
{
union {
struct {
long x;
long y;
};

long nod[2];
};

topo_node(){}

topo_node(long lx, long ly)
{
x = lx;
y = ly;
}
};

struct topo_arc
{
union {
struct {
long id;// arc id links explict attrlist. aid identifies direction of an arc
long wt;// weight of arc
long na;// from node point
long nb;// to node point
};

long arc[4];
};

topo_arc(){}

topo_arc(long arcid, long weight, long from, long to)
{
id = arcid;
wt = weight;
na = from;
nb = to;
}
};

// A topo_path consistes of multiple topo_arc(s)
class topo_path
{
private:
vector<long> ipath;

public:
topo_path(){}

~topo_path(){
ipath.clear();
}

inline long& operator [](int i)
{
return ipath;
}

inline void pushback(long val)
{
ipath.push_back(val);
}

inline static void link(topo_path& pa, topo_path& pb, topo_path& res)
{
int sa = pa.size();
int sb = pb.size();
int size = sa+sb;
res.ipath.resize(size);

for (int i=0; i<sa; i++)
res.ipath = pa.ipath;

int j=0;
for (i=sa; i<size; i++)
res.ipath = pb.ipath[j++];
}

inline int size(){
return ipath.size();
}
};


class topo_network
{
typedef vector<topo_arc> arc_list;
typedef vector<topo_node> node_list;

// Data
unsigned short diff;// Tolerance units of network
arc_list arcs; // A vector of topo_arc(s)
node_list nodes; // A vector of topo_node(s)

// Adjacence matrix
long* arcmat;// Adjacence matrix of arc ids
long* wtmat;// Adjacence matrix of weights of arcs

// Floyd & Dijkstra
topo_path* pathmat;

// Dijkstra
long* sel;
long* dist;

public:

topo_network(unsigned short difference=1)
{
diff = difference;
arcmat = NULL;
wtmat = NULL;
pathmat = NULL;

sel = NULL;
dist = NULL;
}

~topo_network(){
delete[] arcmat;
delete[] wtmat;
delete[] pathmat;
delete[] sel;
}

void initDiff(unsigned short difference) {
diff = difference;
}

// Add a edge to arcs, if have a reverse edge simultaneously, add it as well.
void addArc(long arcid, long weight, 
long xfrom, long yfrom, 
long xto, long yto,
bool bHaveRev = false)
{
// traverse nodes firstly
topo_arc arc(arcid, weight, 0, 0);
long nod;
long size = nodes.size();

bool bAddFrom = false;
bool bAddTo = false;
for (long i=0; i<size; i++) {
if ( !bAddFrom && DIFFVAL(nodes.x, xfrom) < diff && DIFFVAL(nodes.y, yfrom) < diff) {
arc.na = i;
bAddFrom = true;
}

if ( !bAddTo && DIFFVAL(nodes.x, xto) < diff && DIFFVAL(nodes.y, yto) < diff) {
arc.nb = i;
bAddTo = true;
}
}

if (!bAddFrom) {
topo_node node(xfrom, yfrom);
nodes.push_back(node);
arc.na = nodes.size()-1;
}

if (!bAddTo) {
topo_node node(xto, yto);
nodes.push_back(node);
arc.nb = nodes.size()-1;
}

arcs.push_back(arc);
if (bHaveRev) {
nod = arc.nb;
arc.nb = arc.na;
arc.na = nod;
arc.id = -arc.id;

arcs.push_back(arc);
}
}

long sizeArcs(){
return arcs.size();
}

long sizeNodes(){
return nodes.size();
}

// Create adjacency matrix for storing topo_network.
bool createAdjMatrix() {
long size = nodes.size();
long length = size*size;
try{
if (arcmat)
delete[] arcmat;
if (wtmat)
delete[] wtmat;
if (pathmat)
delete[] pathmat;

arcmat = new long [length];
wtmat = new long [length];
pathmat = new topo_path [length];
}
catch(...){
arcmat = NULL;
wtmat = NULL;
pathmat = NULL;
return false;
}

if (!arcmat || !wtmat || !pathmat)
return false;

// Initialize with sensely value.
for (long row=0; row<size; row++) {
for (long col=0; col<size; col++) {
long q = row*size+col;
arcmat[q] = -1;
if (row == col) 
wtmat[q] = 0;
else
wtmat[q] = MAXWEIGHT;
}
}

// Filling weight in it
long count = arcs.size();
for (long k=0; k<count; k++) {
long i = arcs[k].na;
long j = arcs[k].nb;
long q = i*size+j;
arcmat[q] = k;
wtmat[q] = arcs[k].wt;
pathmat[q].pushback(k);
}

return true;
}

void Floyd()
{
long v = 0;
long size = nodes.size();

for (int k=0; k<size; k++) {
for (int i=0; i<size; i++) {
for (int j=0; j<size; j++) {
long ij = i*size+j;
long ik = i*size+k;
long kj = k*size+j;

v = wtmat[i*size+k] + wtmat[k*size+j];

if (v < wtmat[ij]) {
wtmat[ij] = v;
topo_path::link(pathmat[ik], pathmat[kj], pathmat[ij]);
}
}
}
}
}

// Get path from isrc to idst using Dijkstra Algorithm.
void Dijkstra (long isrc)
{
long size = nodes.size();

if (sel)
delete[] sel;
sel = new long [size];

// Dist is a reference to weight matrix, so we NEED NOT allocate and free it.
dist = wtmat + isrc*size;

// Initialize sel
for (int i=0; i<size; i++)
sel = 0;
sel[isrc] = 1;// Select isrc to selection

// Select the min dist j to selection.
for (long k=0; k<size-1; k++) {

long wt = MAXWEIGHT;
long j = isrc;

for (i=0; i<size; i++) {
if (sel!=1 && dist < wt) {// (v!=1) is faster than (v==0)
j = i;
wt = dist;
}
}
sel[j] = 1;// Add j to sel which dist[j] is min.
//if (j==isrc)
//break;

// Modify dist[]
for (i=0; i<size; i++) {
long w = dist[j] + wtmat[j*size+i]; // weight from j to i plus from isrc to j.

if (sel != 1 && w < dist ) {
dist = w;
// Desc: path[isrc, i] = path[isrc, j] + path[j,i];
topo_path::link(pathmat[isrc*size+j], pathmat[j*size+i], pathmat[isrc*size+i]);
}
}
}
}

topo_path& getPath(long isrc, long idst, long* length=NULL)
{
int q = isrc*sizeNodes() + idst;

if (length)
*length = wtmat[q];

return pathmat[q];
}

// i must be a valid index of arcs
topo_arc getArc(long i) {
topo_arc a = arcs;
return a;
}

// Get index of node in nodes
long getNodeIndex(long lx, long ly) {
long size = nodes.size();

for (long i=0; i<size; i++) {
if ( DIFFVAL(nodes.x, lx) < diff && DIFFVAL(nodes.y, ly) < diff)
return i;
}

return -1;// Invalid index
}
};