int dijkstra(
s_env& st_env,
float af_maxAllowedCost,
bool ab_surfaceCostVoid)
{
int i, j;
int vno_c = -1;
int vno_n = -1;
int vno_i, vno_f;
VERTEX *v_c, *v_n;
float cost, f_pathCost;
struct d_node *dn, *dn_next;
int rv;
// s_iterInfo st_iterInfo;
MRIS* surf = st_env.pMS_active;
// bool b_relNextReference = true;
// If we aren't going to preserve cost history in the environment, then we
// will by default always be able to write path costs
bool b_canWriteCostVal = !st_env.b_costHistoryPreserve;
static int calls = 0;
int marked = 0;
int totalLoops = -1;
/* --- sanity checks --- */
vno_i = st_env.startVertex;
vno_f = st_env.endVertex;
assert(vno_i >= 0);
assert(vno_i < surf->nvertices);
assert(vno_f >= 0);
assert(vno_f < surf->nvertices);
if (!st_env.b_costHistoryPreserve) {
assert(!surf->vertices[vno_i].ripflag);
assert(!surf->vertices[vno_f].ripflag);
}
/* --- initialize --- */
for (i = 0; i < surf->nvertices; i++) {
bool b_overwrite = true;
if (mark(surf, i, DIJK_VIRGIN, b_overwrite) != NO_ERROR)
goto error;
// Set all vertex values to -1 - only the very first time
// that this function is called, or if explicitly
// specified in the calling parameters.
if (!calls || ab_surfaceCostVoid) surf->vertices[i].val = -1;
}
calls++;
surf->vertices[vno_i].val = 0.0;
surf->vertices[vno_i].old_undefval = vno_f;
if (mark(surf, vno_i, DIJK_IN_PLAY) != NO_ERROR)
goto error;
// If the start and end vertices are coincident in the problem environment,
// we should loop through at least once, and ignore the vno_c!=vno_f
// condition, otherwise the while() will terminate after one loop.
while ( vno_c!=vno_f || !totalLoops) {
totalLoops++;
if(totalLoops >= st_env.pMS_curvature->nvertices-1) {
// If this condition is true, we have processed all available vertices
// in the mesh -- typically only occurs if the startVertex == endVertex
// and is used for 'autodijk' type calculations.
rv = TRUE;
goto clean;
}
/* set vno_c (find min) */
if (d_list == NULL) {
ErrorPrintf(ERROR_BADPARM, "dijkstra(): out of vertices");
colprintf(st_env.lw, st_env.rw, "start:stop", "[ %d:%d ]\n",
st_env.startVertex,
st_env.endVertex);
goto error;
}
vno_c = d_list->vno;
v_c = &surf->vertices[vno_c];
/* mark it */
if (mark(surf, vno_c, DIJK_DONE) != NO_ERROR)
goto error;
/* update neighbors */
//cout << "neighbors = " << (int) v_c->num << endl;
for (j = 0; j < (int) v_c->vnum; j++) {
//cout << "neighbor = " << j << endl;
vno_n = v_c->v[j];
v_n = &surf->vertices[vno_n];
//if(v_n->ripflag) continue;
if (v_n->marked == DIJK_DONE) continue; // for "circular" path searches
// cost = st_env.costFunc_do(st_env, &st_iterInfo, vno_c, j,
// b_relNextReference);
cost = s_env_edgeCostFind(st_env, vno_c, vno_n);
f_pathCost = v_c->val + cost;
// Break out of while if af_maxAllowedCost is violated.
if (af_maxAllowedCost && (f_pathCost > af_maxAllowedCost)) continue;
if ( (v_n->marked == DIJK_VIRGIN) || (f_pathCost < v_n->val) ) {
// The check in the <if> statement preserves pathCost values in the MRIS
// from previous calls to this function. This history is important
// in determing ply distances from a given target path. A pathCost
// is only written to a new vertex iff that vertex has never been
// visited, or if the cost is less than an older value.
if (st_env.b_costHistoryPreserve) {
b_canWriteCostVal = (f_pathCost<v_n->val||v_n->val==-1);
}
if (b_canWriteCostVal) {
marked++;
v_n->val = f_pathCost;
v_n->old_undefval = vno_c;
//cout << vno_c << "<---" << vno_n << endl;
}
}
// cout << "v->marked in dijkstra " << v_n->marked << endl;
if (v_n->marked == DIJK_VIRGIN)
if (mark(surf, vno_n, DIJK_IN_PLAY) != NO_ERROR) goto error;
}
}
rv = TRUE;
goto clean;
error:
rv = FALSE;
clean:
for (dn = d_list;dn != NULL;dn = dn_next) {
dn_next = dn->next;
free(dn);
}
d_list = NULL;
return(rv);
} /* end dijkstra() */
float
surface_ripMark(
s_env& st_env
) {
//
// ARGS
// st_env in environment data
//
// DESCRIPTION
// Adds a TRUE to the ripflag of each vertex that is part of the
// Dijskstra path. Also saves the route itself and cost information
// to a text file.
//
// PRECONDITIONS
// o Rips on the "active" surface are marked.
// o The cumulative cost of traveling along the marked path is returned.
//
// HISTORY
// 18 November 2004
// o Initial design and coding.
//
// 10 March 2005
// o Added "active" surface.
//
// 30 October 2009
// o Added cumulative cost value to result channel and stdout.
//
// s_iterInfo st_iterInfo;
string str_costFile = st_env.str_workingDir +
st_env.str_costFileName;
int i;
int ii, jj;
float f_cost = 0.;
float f_costSum = 0.;
// bool b_relNextReference = false;
ofstream ofs(str_costFile.c_str(), ios::out);
ofs.flags(ios::fixed );
for (i = st_env.endVertex; i != st_env.startVertex;
i = st_env.pMS_active->vertices[i].old_undefval) {
ii = st_env.pMS_active->vertices[i].old_undefval;
jj = i;
// f_cost = st_env.costFunc_do(st_env, &st_iterInfo, ii, jj,
// b_relNextReference);
f_cost = s_env_edgeCostFind(st_env, ii, jj);
st_env.pMS_active->vertices[i].ripflag = TRUE;
f_costSum += f_cost;
if(st_env.b_costPathSave) {
ofs << ii << "\t" << jj;
ofs << "\t" << f_cost;
ofs << "\t" << st_env.pst_iterInfo->iter;
ofs << "\t" << st_env.pst_iterInfo->f_distance;
ofs << "\t" << st_env.pst_iterInfo->f_curvature;
ofs << "\t" << st_env.pst_iterInfo->f_sulcalHeight;
ofs << "\t" << st_env.pst_iterInfo->f_dir;
ofs << endl;
}
}
st_env.pMS_active->vertices[i].ripflag = TRUE;
if(st_env.b_costPathSave) ofs.close();
return f_costSum;
}
