35 idx_t i, ii,
j, jj, k, me=0, nvtxs,
first,
last, nleft, ncmps;
38 idx_t mustfree_ccsr=0, mustfree_where=0;
46 cptr =
imalloc(nvtxs+1,
"FindPartitionInducedComponents: cptr");
47 cind =
imalloc(nvtxs,
"FindPartitionInducedComponents: cind");
53 where =
ismalloc(nvtxs, 0,
"FindPartitionInducedComponents: where");
58 perm =
iincset(nvtxs, 0,
imalloc(nvtxs,
"FindPartitionInducedComponents: perm"));
59 todo =
iincset(nvtxs, 0,
imalloc(nvtxs,
"FindPartitionInducedComponents: todo"));
60 touched =
ismalloc(nvtxs, 0,
"FindPartitionInducedComponents: touched");
69 cptr[++ncmps] =
first;
70 ASSERT(touched[todo[0]] == 0);
79 j = todo[k] = todo[--nleft];
82 for (j=xadj[i]; j<xadj[i+1]; j++) {
84 if (where[k] == me && !touched[k]) {
90 cptr[++ncmps] =
first;
122 nvtxs = graph->
nvtxs;
134 while (first < nvtxs) {
142 i = bfsperm[first++];
143 for (j=xadj[i]; j<xadj[i+1]; j++) {
150 bfsperm[perm[k]] = bfsperm[
last];
151 perm[bfsperm[
last]] = perm[k];
173 if (ncmps != 1 && report)
174 printf(
"The graph is not connected. It has %"PRIDX" connected components.\n", ncmps);
190 nvtxs = graph->
nvtxs;
193 where = graph->
where;
195 touched =
ismalloc(nvtxs, 0,
"IsConnected: touched");
196 queue =
imalloc(nvtxs,
"IsConnected: queue");
197 cptr =
imalloc(nvtxs+1,
"IsConnected: cptr");
200 for (i=0; i<nvtxs; i++) {
205 for (i=0; i<nvtxs; i++) {
216 while (first != nleft) {
218 cptr[++ncmps] =
first;
219 for (i=0; i<nvtxs; i++) {
220 if (where[i] == pid && !touched[i])
228 for (j=xadj[i]; j<xadj[i+1]; j++) {
230 if (where[k] == pid && !touched[k]) {
236 cptr[++ncmps] =
first;
238 if (ncmps > 1 && report) {
239 printf(
"The graph has %"PRIDX" connected components in partition %"PRIDX":\t", ncmps, pid);
240 for (i=0; i<ncmps; i++) {
242 for (j=cptr[i]; j<cptr[i+1]; j++)
243 wgt += graph->
vwgt[queue[j]];
244 printf(
"[%5"PRIDX
" %5"PRIDX
"] ", cptr[i+1]-cptr[i], wgt);
255 return (ncmps == 1 ? 1 : 0);
273 nvtxs = graph->
nvtxs;
276 where = graph->
where;
278 touched =
ismalloc(nvtxs, 0,
"IsConnected: queue");
280 for (i=0; i<graph->
nbnd; i++)
281 touched[graph->
bndind[i]] = 1;
286 for (i=0; i<nvtxs; i++) {
291 for (i=0; i<nvtxs; i++) {
303 while (first != nleft) {
305 cptr[++ncmps] =
first;
306 for (i=0; i<nvtxs; i++) {
315 for (j=xadj[i]; j<xadj[i+1]; j++) {
323 cptr[++ncmps] =
first;
338 idx_t i, ii,
j, jj, k, me,
nparts, nvtxs,
ncon, ncmps,
other,
341 idx_t *cptr, *cind, *cpvec, *pcptr, *pcind, *cwhere;
342 idx_t cid, bestcid, *cwgt, *bestcwgt;
343 idx_t ntodo, oldntodo, *todo;
350 nvtxs = graph->
nvtxs;
357 where = graph->
where;
358 pwgts = graph->
pwgts;
369 printf(
"I found %"PRIDX" components, for this %"PRIDX"-way partition\n",
373 if (ncmps > nparts) {
381 cand = (rkv_t *)
wspacemalloc(ctrl, nparts*
sizeof(rkv_t));
392 for (i=0; i<ncmps; i++)
393 pcptr[where[cind[cptr[i]]]]++;
395 for (i=0; i<ncmps; i++)
396 pcind[pcptr[where[cind[cptr[i]]]]++] = i;
400 for (ntodo=0, i=0; i<
nparts; i++) {
401 if (pcptr[i+1]-pcptr[i] == 1)
402 bestcid = pcind[pcptr[
i]];
404 for (bestcid=-1, j=pcptr[i]; j<pcptr[i+1]; j++) {
407 for (ii=cptr[cid]; ii<cptr[cid+1]; ii++)
408 iaxpy(ncon, 1, vwgt+cind[ii]*ncon, 1, cwgt, 1);
409 if (bestcid == -1 ||
isum(ncon, bestcwgt, 1) <
isum(ncon, cwgt, 1)) {
411 icopy(ncon, cwgt, bestcwgt);
415 for (j=pcptr[i]; j<pcptr[i+1]; j++) {
416 if (pcind[j] != bestcid)
417 todo[ntodo++] = pcind[
j];
421 for (j=cptr[bestcid]; j<cptr[bestcid+1]; j++) {
422 ASSERT(where[cind[j]] == i);
430 for (i=0; i<ntodo; i++) {
432 me = where[cind[cptr[cid]]];
436 for (j=cptr[cid]; j<cptr[cid+1]; j++)
437 iaxpy(ncon, 1, vwgt+cind[j]*ncon, 1, cwgt, 1);
440 printf(
"Trying to move %"PRIDX
" [%"PRIDX
"] from %"PRIDX
"\n",
441 cid,
isum(ncon, cwgt, 1), me));
444 iset(nparts, 0, cpvec);
445 for (j=cptr[cid]; j<cptr[cid+1]; j++) {
447 for (jj=xadj[ii]; jj<xadj[ii+1]; jj++)
448 if (cwhere[adjncy[jj]] != -1)
449 cpvec[cwhere[adjncy[jj]]] += (adjwgt ? adjwgt[jj] : 1);
453 for (ncand=0, j=0; j<
nparts; j++) {
455 cand[ncand].key = cpvec[
j];
456 cand[ncand++].val =
j;
469 for (j=1; j<ncand; j++) {
470 if (cand[j].
key < .5*cand[0].
key)
477 target = cand[0].val;
478 for (j=1; j<ncand; j++) {
480 1, pwgts+target*ncon, ctrl->
pijbm+target*ncon,
481 1, pwgts+cand[j].val*ncon, ctrl->
pijbm+cand[j].val*ncon))
482 target = cand[
j].val;
486 printf(
"\tMoving it to %"PRIDX
" [%"PRIDX
"] [%"PRIDX
"]\n", target, cpvec[target], ncand));
498 vmarker, pmarker, modind);
507 for (j=cptr[cid]; j<cptr[cid+1]; j++)
508 cwhere[cind[j]] = target;
510 todo[
i] = todo[--ntodo];
512 if (oldntodo == ntodo) {
518 for (i=0; i<nvtxs; i++)
519 ASSERT(where[i] == cwhere[i]);
534 idx_t i, ii, iii,
j, jj, k,
l, nvtxs, nbnd, from, me;
539 nvtxs = graph->
nvtxs;
544 where = graph->
where;
550 for (iii=ptr[gid]; iii<ptr[gid+1]; iii++) {
555 if (myrinfo->
inbr == -1) {
562 for (k=0; k<myrinfo->
nnbrs; k++) {
563 if (mynbrs[k].pid == to)
566 if (k == myrinfo->
nnbrs) {
581 for (j=xadj[i]; j<xadj[i+1]; j++) {
587 from, to, myrinfo, adjwgt[j], nbnd, bndptr, bndind,
BNDTYPE_REFINE);
605 idx_t i, ii, iii,
j, jj, k,
l, nvtxs, from, me,
other, xgain;
610 nvtxs = graph->
nvtxs;
612 vsize = graph->
vsize;
614 where = graph->
where;
616 for (iii=ptr[gid]; iii<ptr[gid+1]; iii++) {
621 if (myrinfo->
inbr == -1) {
627 xgain = (myrinfo->
nid == 0 && myrinfo->
ned > 0 ? vsize[
i] : 0);
630 for (k=0; k<myrinfo->
nnbrs; k++) {
631 if (mynbrs[k].pid == to)
634 if (k == myrinfo->
nnbrs) {
635 if (myrinfo->
nid > 0)
639 for (j=xadj[i]; j<xadj[i+1]; j++) {
648 for (l=0; l<orinfo->
nnbrs; l++) {
649 if (onbrs[l].pid == to)
652 if (l == orinfo->
nnbrs)
657 for (l=0; l<orinfo->
nnbrs; l++) {
658 if (onbrs[l].pid == to)
661 if (l == orinfo->
nnbrs)
665 for (l=0; l<orinfo->
nnbrs; l++) {
666 if (onbrs[l].pid == from && onbrs[l].ned == 1) {
677 graph->
minvol -= (xgain + mynbrs[k].
gv);
const gtsam::Symbol key('X', 0)
idx_t idx_t idx_t idx_t * vwgt
idx_t FindSepInducedComponents(ctrl_t *ctrl, graph_t *graph, idx_t *cptr, idx_t *cind)
for(size_t i=1;i< poses.size();++i)
#define ASSERTP(expr, msg)
idx_t idx_t idx_t idx_t idx_t idx_t idx_t * nparts
void gk_errexit(int signum, char *f_str,...)
EIGEN_CONSTEXPR Index first(const T &x) EIGEN_NOEXCEPT
NonlinearFactorGraph graph
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
idx_t idx_t idx_t idx_t idx_t * vsize
#define ASSERT(expression)
static const Line3 l(Rot3(), 1, 1)
void MoveGroupContigForVol(ctrl_t *ctrl, graph_t *graph, idx_t to, idx_t gid, idx_t *ptr, idx_t *ind, idx_t *vmarker, idx_t *pmarker, idx_t *modind)
idx_t IsConnected(graph_t *graph, idx_t report)
idx_t idx_t idx_t idx_t idx_t * perm
void EliminateComponents(ctrl_t *ctrl, graph_t *graph)
idx_t IsConnectedSubdomain(ctrl_t *ctrl, graph_t *graph, idx_t pid, idx_t report)
idx_t idx_t idx_t idx_t idx_t idx_t idx_t real_t * tpwgts
#define SHIFTCSR(i, n, a)
idx_t idx_t idx_t idx_t idx_t idx_t * adjwgt
idx_t FindPartitionInducedComponents(graph_t *graph, idx_t *where, idx_t *cptr, idx_t *cind)
void gk_free(void **ptr1,...)
idx_t idx_t idx_t idx_t * where
idx_t idx_t idx_t * adjncy
#define UpdateAdjacentVertexInfoAndBND(ctrl, vid, adjlen, me, from, to, myrinfo, ewgt, nbnd, bndptr, bndind, bndtype)
#define UpdateMovedVertexInfoAndBND(i, from, k, to, myrinfo, mynbrs, where, nbnd, bndptr, bndind, bndtype)
void MoveGroupContigForCut(ctrl_t *ctrl, graph_t *graph, idx_t to, idx_t gid, idx_t *ptr, idx_t *ind)
#define BetterBalanceKWay
#define IFSET(a, flag, cmd)
void ComputeBFSOrdering(ctrl_t *ctrl, graph_t *graph, idx_t *bfsperm)