Browse code

Second version bump after creating 2.14 release branch.

git-svn-id: https://hedgehog.fhcrc.org/bioconductor/trunk/madman/Rpacks/Rgraphviz@88840 bc3139a8-67e5-0310-9ffc-ced21a209358

d.tenenbaum authored on 11/04/2014 21:21:21
Showing1 changed files
1 1
new file mode 100644
... ...
@@ -0,0 +1,999 @@
1
+/* $Id: post_process.c,v 1.9 2011/02/28 16:19:43 erg Exp $ $Revision: 1.9 $ */
2
+/* vim:set shiftwidth=4 ts=8: */
3
+
4
+/*************************************************************************
5
+ * Copyright (c) 2011 AT&T Intellectual Property 
6
+ * All rights reserved. This program and the accompanying materials
7
+ * are made available under the terms of the Eclipse Public License v1.0
8
+ * which accompanies this distribution, and is available at
9
+ * http://www.eclipse.org/legal/epl-v10.html
10
+ *
11
+ * Contributors: See CVS logs. Details at http://www.graphviz.org/
12
+ *************************************************************************/
13
+
14
+#ifdef HAVE_CONFIG_H
15
+#include "config.h"
16
+#endif
17
+
18
+#include <time.h>
19
+#include <string.h>
20
+#include <math.h>
21
+
22
+#include "types.h"
23
+#include "memory.h"
24
+#include "globals.h"
25
+
26
+#include "sparse_solve.h"
27
+#include "post_process.h"
28
+#include "overlap.h"
29
+#include "spring_electrical.h"
30
+#include "call_tri.h"
31
+#include "sfdpinternal.h"
32
+
33
+#define node_degree(i) (ia[(i)+1] - ia[(i)])
34
+
35
+SparseMatrix ideal_distance_matrix(SparseMatrix A, int dim, real *x){
36
+  /* find the ideal distance between edges, either 1, or |N[i] \Union N[j]| - |N[i] \Intersection N[j]|
37
+   */
38
+  SparseMatrix D;
39
+  int *ia, *ja, i, j, k, l, nz;
40
+  real *d;
41
+  int *mask = NULL;
42
+  real len, di, sum, sumd;
43
+
44
+  assert(SparseMatrix_is_symmetric(A, FALSE));
45
+
46
+  D = SparseMatrix_copy(A);
47
+  ia = D->ia;
48
+  ja = D->ja;
49
+  if (D->type != MATRIX_TYPE_REAL){
50
+    FREE(D->a);
51
+    D->type = MATRIX_TYPE_REAL;
52
+    D->a = N_GNEW(D->nz,real);
53
+  }
54
+  d = (real*) D->a;
55
+
56
+  mask = N_GNEW(D->m,int);
57
+  for (i = 0; i < D->m; i++) mask[i] = -1;
58
+
59
+  for (i = 0; i < D->m; i++){
60
+    di = node_degree(i);
61
+    mask[i] = i;
62
+    for (j = ia[i]; j < ia[i+1]; j++){
63
+      if (i == ja[j]) continue;
64
+      mask[ja[j]] = i;
65
+    }
66
+    for (j = ia[i]; j < ia[i+1]; j++){
67
+      k = ja[j];
68
+      if (i == k) continue;
69
+      len = di + node_degree(k);
70
+      for (l = ia[k]; l < ia[k+1]; l++){
71
+	if (mask[ja[l]] == i) len--;
72
+      }
73
+      d[j] = len;
74
+      assert(len > 0);
75
+    }
76
+    
77
+  }
78
+
79
+  sum = 0; sumd = 0;
80
+  nz = 0;
81
+  for (i = 0; i < D->m; i++){
82
+    for (j = ia[i]; j < ia[i+1]; j++){
83
+      if (i == ja[j]) continue;
84
+      nz++;
85
+      sum += distance(x, dim, i, ja[j]);
86
+      sumd += d[j];
87
+    }
88
+  }
89
+  sum /= nz; sumd /= nz;
90
+  sum = sum/sumd;
91
+
92
+  for (i = 0; i < D->m; i++){
93
+    for (j = ia[i]; j < ia[i+1]; j++){
94
+      if (i == ja[j]) continue;
95
+      d[j] = sum*d[j];
96
+    }
97
+  }
98
+
99
+
100
+  return D;
101
+}
102
+
103
+StressMajorizationSmoother StressMajorizationSmoother2_new(SparseMatrix A, int dim, real lambda0, real *x, 
104
+							  int ideal_dist_scheme){
105
+  /* use up to dist 2 neighbor */
106
+  StressMajorizationSmoother sm;
107
+  int i, j, k, l, m = A->m, *ia = A->ia, *ja = A->ja, *iw, *jw, *id, *jd;
108
+  int *mask, nz;
109
+  real *d, *w, *lambda;
110
+  real *avg_dist, diag_d, diag_w, *dd, dist, s = 0, stop = 0, sbot = 0;
111
+  SparseMatrix ID;
112
+
113
+  assert(SparseMatrix_is_symmetric(A, FALSE));
114
+
115
+  ID = ideal_distance_matrix(A, dim, x);
116
+  dd = (real*) ID->a;
117
+
118
+  sm = N_GNEW(1,struct StressMajorizationSmoother_struct);
119
+  sm->scaling = 1.;
120
+  sm->data = NULL;
121
+  sm->scheme = SM_SCHEME_NORMAL;
122
+
123
+  lambda = sm->lambda = N_GNEW(m,real);
124
+  for (i = 0; i < m; i++) sm->lambda[i] = lambda0;
125
+  mask = N_GNEW(m,int);
126
+
127
+  avg_dist = N_GNEW(m,real);
128
+
129
+  for (i = 0; i < m ;i++){
130
+    avg_dist[i] = 0;
131
+    nz = 0;
132
+    for (j = ia[i]; j < ia[i+1]; j++){
133
+      if (i == ja[j]) continue;
134
+      avg_dist[i] += distance(x, dim, i, ja[j]);
135
+      nz++;
136
+    }
137
+    assert(nz > 0);
138
+    avg_dist[i] /= nz;
139
+  }
140
+
141
+
142
+  for (i = 0; i < m; i++) mask[i] = -1;
143
+
144
+  nz = 0;
145
+  for (i = 0; i < m; i++){
146
+    mask[i] = i;
147
+    for (j = ia[i]; j < ia[i+1]; j++){
148
+      k = ja[j];
149
+      if (mask[k] != i){
150
+	mask[k] = i;
151
+	nz++;
152
+      }
153
+    }
154
+    for (j = ia[i]; j < ia[i+1]; j++){
155
+      k = ja[j];
156
+      for (l = ia[k]; l < ia[k+1]; l++){
157
+	if (mask[ja[l]] != i){
158
+	  mask[ja[l]] = i;
159
+	  nz++;
160
+	}
161
+      }
162
+    }
163
+  }
164
+
165
+  sm->Lw = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
166
+  sm->Lwd = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
167
+  if (!(sm->Lw) || !(sm->Lwd)) {
168
+    StressMajorizationSmoother_delete(sm);
169
+    return NULL;
170
+  }
171
+
172
+  iw = sm->Lw->ia; jw = sm->Lw->ja;
173
+  id = sm->Lwd->ia; jd = sm->Lwd->ja;
174
+  w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;
175
+  iw[0] = id[0] = 0;
176
+
177
+  nz = 0;
178
+  for (i = 0; i < m; i++){
179
+    mask[i] = i+m;
180
+    diag_d = diag_w = 0;
181
+    for (j = ia[i]; j < ia[i+1]; j++){
182
+      k = ja[j];
183
+      if (mask[k] != i+m){
184
+	mask[k] = i+m;
185
+
186
+	jw[nz] = k;
187
+	if (ideal_dist_scheme == IDEAL_GRAPH_DIST){
188
+	  dist = 1;
189
+	} else if (ideal_dist_scheme == IDEAL_AVG_DIST){
190
+	  dist = (avg_dist[i] + avg_dist[k])*0.5;
191
+	} else if (ideal_dist_scheme == IDEAL_POWER_DIST){
192
+	  dist = pow(distance_cropped(x,dim,i,k),.4);
193
+	} else {
194
+	  fprintf(stderr,"ideal_dist_scheme value wrong");
195
+	  assert(0);
196
+	  exit(1);
197
+	}
198
+
199
+	/*	
200
+	  w[nz] = -1./(ia[i+1]-ia[i]+ia[ja[j]+1]-ia[ja[j]]);
201
+	  w[nz] = -2./(avg_dist[i]+avg_dist[k]);*/
202
+	/* w[nz] = -1.;*//* use unit weight for now, later can try 1/(deg(i)+deg(k)) */
203
+	w[nz] = -1/(dist*dist);
204
+
205
+	diag_w += w[nz];
206
+
207
+	jd[nz] = k;
208
+	/*
209
+	  d[nz] = w[nz]*distance(x,dim,i,k);
210
+	  d[nz] = w[nz]*dd[j];
211
+	  d[nz] = w[nz]*(avg_dist[i] + avg_dist[k])*0.5;
212
+	*/
213
+	d[nz] = w[nz]*dist;
214
+	stop += d[nz]*distance(x,dim,i,k);
215
+	sbot += d[nz]*dist;
216
+	diag_d += d[nz];
217
+
218
+	nz++;
219
+      }
220
+    }
221
+
222
+    /* distance 2 neighbors */
223
+    for (j = ia[i]; j < ia[i+1]; j++){
224
+      k = ja[j];
225
+      for (l = ia[k]; l < ia[k+1]; l++){
226
+	if (mask[ja[l]] != i+m){
227
+	  mask[ja[l]] = i+m;
228
+
229
+	  if (ideal_dist_scheme == IDEAL_GRAPH_DIST){
230
+	    dist = 2;
231
+	  } else if (ideal_dist_scheme == IDEAL_AVG_DIST){
232
+	    dist = (avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
233
+	  } else if (ideal_dist_scheme == IDEAL_POWER_DIST){
234
+	    dist = pow(distance_cropped(x,dim,i,ja[l]),.4);
235
+	  } else {
236
+	    fprintf(stderr,"ideal_dist_scheme value wrong");
237
+	    assert(0);
238
+	    exit(1);
239
+	  }
240
+
241
+	  jw[nz] = ja[l];
242
+	  /*
243
+	    w[nz] = -1/(ia[i+1]-ia[i]+ia[ja[l]+1]-ia[ja[l]]);
244
+	    w[nz] = -2/(avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]]);*/
245
+	  /* w[nz] = -1.;*//* use unit weight for now, later can try 1/(deg(i)+deg(k)) */
246
+
247
+	  w[nz] = -1/(dist*dist);
248
+
249
+	  diag_w += w[nz];
250
+
251
+	  jd[nz] = ja[l];
252
+	  /*
253
+	    d[nz] = w[nz]*(distance(x,dim,i,k)+distance(x,dim,k,ja[l]));
254
+	    d[nz] = w[nz]*(dd[j]+dd[l]);
255
+	    d[nz] = w[nz]*(avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
256
+	  */
257
+	  d[nz] = w[nz]*dist;
258
+	  stop += d[nz]*distance(x,dim,ja[l],k);
259
+	  sbot += d[nz]*dist;
260
+	  diag_d += d[nz];
261
+
262
+	  nz++;
263
+	}
264
+      }
265
+    }
266
+    jw[nz] = i;
267
+    lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */
268
+
269
+    w[nz] = -diag_w + lambda[i];
270
+    jd[nz] = i;
271
+    d[nz] = -diag_d;
272
+    nz++;
273
+
274
+    iw[i+1] = nz;
275
+    id[i+1] = nz;
276
+  }
277
+  s = stop/sbot;
278
+  for (i = 0; i < nz; i++) d[i] *= s;
279
+
280
+  sm->scaling = s;
281
+  sm->Lw->nz = nz;
282
+  sm->Lwd->nz = nz;
283
+
284
+  FREE(mask);
285
+  FREE(avg_dist);
286
+  SparseMatrix_delete(ID);
287
+  return sm;
288
+}
289
+	
290
+StressMajorizationSmoother SparseStressMajorizationSmoother_new(SparseMatrix A, int dim, real lambda0, real *x, int weighting_scheme){
291
+  /* solve a stress model to achieve the ideal distance among a sparse set of edges recorded in A.
292
+     A must be a real matrix.
293
+   */
294
+  StressMajorizationSmoother sm;
295
+  int i, j, k, m = A->m, *ia, *ja, *iw, *jw, *id, *jd;
296
+  int nz;
297
+  real *d, *w, *lambda;
298
+  real diag_d, diag_w, *a, dist, s = 0, stop = 0, sbot = 0;
299
+  real xdot = 0;
300
+
301
+  assert(SparseMatrix_is_symmetric(A, FALSE) && A->type == MATRIX_TYPE_REAL);
302
+
303
+  /* if x is all zero, make it random */
304
+  for (i = 0; i < m*dim; i++) xdot += x[i]*x[i];
305
+  if (xdot == 0){
306
+    for (i = 0; i < m*dim; i++) x[i] = 72*drand();
307
+  }
308
+
309
+  ia = A->ia;
310
+  ja = A->ja;
311
+  a = (real*) A->a;
312
+
313
+
314
+  sm = MALLOC(sizeof(struct StressMajorizationSmoother_struct));
315
+  sm->scaling = 1.;
316
+  sm->data = NULL;
317
+  sm->scheme = SM_SCHEME_NORMAL;
318
+
319
+  lambda = sm->lambda = MALLOC(sizeof(real)*m);
320
+  for (i = 0; i < m; i++) sm->lambda[i] = lambda0;
321
+
322
+  nz = A->nz;
323
+
324
+  sm->Lw = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
325
+  sm->Lwd = SparseMatrix_new(m, m, nz + m, MATRIX_TYPE_REAL, FORMAT_CSR);
326
+  if (!(sm->Lw) || !(sm->Lwd)) {
327
+    StressMajorizationSmoother_delete(sm);
328
+    return NULL;
329
+  }
330
+
331
+  iw = sm->Lw->ia; jw = sm->Lw->ja;
332
+  id = sm->Lwd->ia; jd = sm->Lwd->ja;
333
+  w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;
334
+  iw[0] = id[0] = 0;
335
+
336
+  nz = 0;
337
+  for (i = 0; i < m; i++){
338
+    diag_d = diag_w = 0;
339
+    for (j = ia[i]; j < ia[i+1]; j++){
340
+      k = ja[j];
341
+      if (k != i){
342
+
343
+	jw[nz] = k;
344
+	dist = a[j];
345
+	switch (weighting_scheme){
346
+	case WEIGHTING_SCHEME_SQR_DIST:
347
+	  if (dist*dist == 0){
348
+	    w[nz] = -100000;
349
+	  } else {
350
+	    w[nz] = -1/(dist*dist);
351
+	  }
352
+	  break;
353
+	case WEIGHTING_SCHEME_NONE:
354
+	  w[nz] = -1;
355
+	  break;
356
+	default:
357
+	  assert(0);
358
+	  return NULL;
359
+	}
360
+	diag_w += w[nz];
361
+	jd[nz] = k;
362
+	d[nz] = w[nz]*dist;
363
+
364
+	stop += d[nz]*distance(x,dim,i,k);
365
+	sbot += d[nz]*dist;
366
+	diag_d += d[nz];
367
+
368
+	nz++;
369
+      }
370
+    }
371
+
372
+    jw[nz] = i;
373
+    lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */
374
+    w[nz] = -diag_w + lambda[i];
375
+
376
+    jd[nz] = i;
377
+    d[nz] = -diag_d;
378
+    nz++;
379
+
380
+    iw[i+1] = nz;
381
+    id[i+1] = nz;
382
+  }
383
+  s = stop/sbot;
384
+  if (s == 0) {
385
+    return NULL;
386
+  }
387
+  for (i = 0; i < nz; i++) d[i] *= s;
388
+
389
+
390
+  sm->scaling = s;
391
+  sm->Lw->nz = nz;
392
+  sm->Lwd->nz = nz;
393
+
394
+  return sm;
395
+}
396
+
397
+static real total_distance(int m, int dim, real* x, real* y){
398
+  real total = 0, dist = 0;
399
+  int i, j;
400
+
401
+  for (i = 0; i < m; i++){
402
+    dist = 0.;
403
+    for (j = 0; j < dim; j++){
404
+      dist += (y[i*dim+j] - x[i*dim+j])*(y[i*dim+j] - x[i*dim+j]);
405
+    }
406
+    total += sqrt(dist);
407
+  }
408
+  return total;
409
+
410
+}
411
+
412
+
413
+
414
+void SparseStressMajorizationSmoother_delete(SparseStressMajorizationSmoother sm){
415
+  return StressMajorizationSmoother_delete(sm);
416
+}
417
+
418
+
419
+real SparseStressMajorizationSmoother_smooth(SparseStressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol){
420
+
421
+  return StressMajorizationSmoother_smooth(sm, dim, x, maxit_sm, tol);
422
+
423
+
424
+}
425
+static void get_edge_label_matrix(relative_position_constraints data, int m, int dim, real *x, SparseMatrix *LL, real **rhs){
426
+  int edge_labeling_scheme = data->edge_labeling_scheme;
427
+  int n_constr_nodes = data->n_constr_nodes;
428
+  int *constr_nodes = data->constr_nodes;
429
+  SparseMatrix A_constr = data->A_constr;
430
+  int *ia = A_constr->ia, *ja = A_constr->ja, ii, jj, nz, l, ll, i, j;
431
+  int *irn = data->irn, *jcn = data->jcn;
432
+  real *val = data->val, dist, kk, k;
433
+  real *x00 = NULL;
434
+  SparseMatrix Lc = NULL;
435
+  real constr_penalty = data->constr_penalty;
436
+
437
+  if (edge_labeling_scheme == ELSCHEME_PENALTY || edge_labeling_scheme == ELSCHEME_STRAIGHTLINE_PENALTY){
438
+    /* for an node with two neighbors j--i--k, and assume i needs to be between j and k, then the contribution to P is
439
+       .   i    j     k
440
+       i   1   -0.5  -0.5
441
+       j  -0.5 0.25  0.25
442
+       k  -0.5 0.25  0.25
443
+       in general, if i has m neighbors j, k, ..., then
444
+       p_ii = 1
445
+       p_jj = 1/m^2
446
+       p_ij = -1/m
447
+       p jk = 1/m^2
448
+       .    i     j    k
449
+       i    1    -1/m  -1/m ...
450
+       j   -1/m  1/m^2 1/m^2 ...
451
+       k   -1/m  1/m^2 1/m^2 ...
452
+    */
453
+    if (!irn){
454
+      assert((!jcn) && (!val));
455
+      nz = 0;
456
+      for (i = 0; i < n_constr_nodes; i++){
457
+	ii = constr_nodes[i];
458
+	k = ia[ii+1] - ia[ii];/*usually k = 2 */
459
+	nz += (k+1)*(k+1);
460
+	
461
+      }
462
+      irn = data->irn = MALLOC(sizeof(int)*nz);
463
+      jcn = data->jcn = MALLOC(sizeof(int)*nz);
464
+      val = data->val = MALLOC(sizeof(double)*nz);
465
+    }
466
+    nz = 0;
467
+    for (i = 0; i < n_constr_nodes; i++){
468
+      ii = constr_nodes[i];
469
+      jj = ja[ia[ii]]; ll = ja[ia[ii] + 1];
470
+      if (jj == ll) continue; /* do not do loops */
471
+      dist = distance_cropped(x, dim, jj, ll);
472
+      dist *= dist;
473
+
474
+      k = ia[ii+1] - ia[ii];/* usually k = 2 */
475
+      kk = k*k;
476
+      irn[nz] = ii; jcn[nz] = ii; val[nz++] = constr_penalty/(dist);
477
+      k = constr_penalty/(k*dist); kk = constr_penalty/(kk*dist);
478
+      for (j = ia[ii]; j < ia[ii+1]; j++){
479
+	irn[nz] = ii; jcn[nz] = ja[j]; val[nz++] = -k;
480
+      }
481
+      for (j = ia[ii]; j < ia[ii+1]; j++){
482
+	jj = ja[j];
483
+	irn[nz] = jj; jcn[nz] = ii; val[nz++] = -k;
484
+	for (l = ia[ii]; l < ia[ii+1]; l++){
485
+	  ll = ja[l];
486
+	  irn[nz] = jj; jcn[nz] = ll; val[nz++] = kk;
487
+	}
488
+      }
489
+    }
490
+    Lc = SparseMatrix_from_coordinate_arrays(nz, m, m, irn, jcn, val, MATRIX_TYPE_REAL);
491
+  } else if (edge_labeling_scheme == ELSCHEME_PENALTY2 || edge_labeling_scheme == ELSCHEME_STRAIGHTLINE_PENALTY2){
492
+    /* for an node with two neighbors j--i--k, and assume i needs to be between the old position of j and k, then the contribution to P is
493
+       1/d_jk, and to the right hand side: {0,...,average_position_of_i's neighbor if i is an edge node,...}
494
+    */
495
+    if (!irn){
496
+      assert((!jcn) && (!val));
497
+      nz = n_constr_nodes;
498
+      irn = data->irn = MALLOC(sizeof(int)*nz);
499
+      jcn = data->jcn = MALLOC(sizeof(int)*nz);
500
+      val = data->val = MALLOC(sizeof(double)*nz);
501
+    }
502
+    x00 = MALLOC(sizeof(real)*m*dim);
503
+    for (i = 0; i < m*dim; i++) x00[i] = 0;
504
+    nz = 0;
505
+    for (i = 0; i < n_constr_nodes; i++){
506
+      ii = constr_nodes[i];
507
+      jj = ja[ia[ii]]; ll = ja[ia[ii] + 1];
508
+      dist = distance_cropped(x, dim, jj, ll);
509
+      irn[nz] = ii; jcn[nz] = ii; val[nz++] = constr_penalty/(dist);
510
+      for (j = ia[ii]; j < ia[ii+1]; j++){
511
+	jj = ja[j];
512
+	for (l = 0; l < dim; l++){
513
+	  x00[ii*dim+l] += x[jj*dim+l];
514
+	}
515
+      }
516
+      for (l = 0; l < dim; l++) {
517
+	x00[ii*dim+l] *= constr_penalty/(dist)/(ia[ii+1] - ia[ii]);
518
+      }
519
+    }
520
+    Lc = SparseMatrix_from_coordinate_arrays(nz, m, m, irn, jcn, val, MATRIX_TYPE_REAL);
521
+    
522
+  }
523
+  *LL = Lc;
524
+  *rhs = x00;
525
+}
526
+
527
+real get_stress(int m, int dim, int *iw, int *jw, real *w, real *d, real *x, real scaling, void *data, int weighted){
528
+  int i, j;
529
+  real res = 0., dist;
530
+  /* we use the fact that d_ij = w_ij*dist(i,j). Also, d_ij and x are scalinged by *scaling, so divide by it to get actual unscaled streee. */
531
+  for (i = 0; i < m; i++){
532
+    for (j = iw[i]; j < iw[i+1]; j++){
533
+      if (i == jw[j]) {
534
+	continue;
535
+      }
536
+      dist = d[j]/w[j];/* both negative*/
537
+      if (weighted){
538
+	res += -w[j]*(dist - distance(x, dim, i, jw[j]))*(dist - distance(x, dim, i, jw[j]));
539
+      } else {
540
+	res += (dist - distance(x, dim, i, jw[j]))*(dist - distance(x, dim, i, jw[j]));
541
+      }
542
+    }
543
+  }
544
+  return res/scaling/scaling;
545
+
546
+}
547
+
548
+static void uniform_stress_augment_rhs(int m, int dim, real *x, real *y, real alpha, real M){
549
+  int i, j, k;
550
+  real dist, distij;
551
+  for (i = 0; i < m; i++){
552
+    for (j = i+1; j < m; j++){
553
+      dist = distance_cropped(x, dim, i, j);
554
+      for (k = 0; k < dim; k++){
555
+	distij = (x[i*dim+k] - x[j*dim+k])/dist;
556
+	y[i*dim+k] += alpha*M*distij;
557
+	y[j*dim+k] += alpha*M*(-distij);
558
+      }
559
+    }
560
+  }
561
+}
562
+
563
+static real uniform_stress_solve(SparseMatrix Lw, real alpha, int dim, real *x0, real *rhs, real tol, int maxit, int *flag){
564
+  Operator Ax;
565
+  Operator Precon;
566
+
567
+  Ax = Operator_uniform_stress_matmul(Lw, alpha);
568
+  Precon = Operator_uniform_stress_diag_precon_new(Lw, alpha);
569
+
570
+  return cg(Ax, Precon, Lw->m, dim, x0, rhs, tol, maxit, flag);
571
+
572
+}
573
+
574
+
575
+real StressMajorizationSmoother_smooth(StressMajorizationSmoother sm, int dim, real *x, int maxit_sm, real tol) {
576
+  SparseMatrix Lw = sm->Lw, Lwd = sm->Lwd, Lwdd = NULL;
577
+  int i, j, m, *id, *jd, *iw, *jw, idiag, flag = 0, iter = 0;
578
+  real *w, *dd, *d, *y = NULL, *x0 = NULL, *x00 = NULL, diag, diff = 1, *lambda = sm->lambda, maxit, res, alpha = 0., M = 0.;
579
+  SparseMatrix Lc = NULL;
580
+
581
+  Lwdd = SparseMatrix_copy(Lwd);
582
+  m = Lw->m;
583
+  x0 = N_GNEW(dim*m,real);
584
+  if (!x0) goto RETURN;
585
+
586
+  x0 = MEMCPY(x0, x, sizeof(real)*dim*m);
587
+  y = N_GNEW(dim*m,real);
588
+  if (!y) goto RETURN;
589
+
590
+  id = Lwd->ia; jd = Lwd->ja;
591
+  d = (real*) Lwd->a;
592
+  dd = (real*) Lwdd->a;
593
+  w = (real*) Lw->a;
594
+  iw = Lw->ia; jw = Lw->ja;
595
+
596
+  /* for the additional matrix L due to the position constraints */
597
+  if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
598
+    get_edge_label_matrix(sm->data, m, dim, x, &Lc, &x00);
599
+    if (Lc) Lw = SparseMatrix_add(Lw, Lc);
600
+  } else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
601
+    alpha = ((real*) (sm->data))[0];
602
+    M = ((real*) (sm->data))[1];
603
+  }
604
+
605
+  while (iter++ < maxit_sm && diff > tol){
606
+
607
+    for (i = 0; i < m; i++){
608
+      idiag = -1;
609
+      diag = 0.;
610
+      for (j = id[i]; j < id[i+1]; j++){
611
+	if (i == jd[j]) {
612
+	  idiag = j;
613
+	  continue;
614
+	}
615
+	dd[j] = d[j]/distance_cropped(x, dim, i, jd[j]);
616
+	diag += dd[j];
617
+      }
618
+      assert(idiag >= 0);
619
+      dd[idiag] = -diag;
620
+    }
621
+
622
+    /* solve (Lw+lambda*I) x = Lwdd y + lambda x0 */
623
+
624
+    SparseMatrix_multiply_dense(Lwdd, FALSE, x, FALSE, &y, FALSE, dim);
625
+ 
626
+    if (lambda){/* is there a penalty term? */
627
+      for (i = 0; i < m; i++){
628
+	for (j = 0; j < dim; j++){
629
+	  y[i*dim+j] += lambda[i]*x0[i*dim+j];
630
+	}
631
+      }
632
+    }
633
+
634
+    if (sm->scheme == SM_SCHEME_NORMAL_ELABEL){
635
+      for (i = 0; i < m; i++){
636
+	for (j = 0; j < dim; j++){
637
+	  y[i*dim+j] += x00[i*dim+j];
638
+	}
639
+      }
640
+    } else if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){/* this part can be done more efficiently using octree approximation */
641
+      uniform_stress_augment_rhs(m, dim, x, y, alpha, M);
642
+    }
643
+
644
+#ifdef DEBUG_PRINT
645
+    if (Verbose) fprintf(stderr, "stress1 = %f\n",get_stress(m, dim, iw, jw, w, d, x, sm->scaling, sm->data, 0));
646
+#endif
647
+
648
+    maxit = sqrt((double) m);
649
+    if (sm->scheme == SM_SCHEME_UNIFORM_STRESS){
650
+      res = uniform_stress_solve(Lw, alpha, dim, x, y, 0.01, maxit, &flag);
651
+    } else {
652
+      res = SparseMatrix_solve(Lw, dim, x, y, 0.01, maxit, SOLVE_METHOD_CG, &flag);
653
+    }
654
+    if (flag) goto RETURN;
655
+#ifdef DEBUG_PRINT
656
+    if (Verbose) fprintf(stderr, "stress2 = %f\n",get_stress(m, dim, iw, jw, w, d, y, sm->scaling, sm->data, 0));
657
+#endif
658
+
659
+    diff = total_distance(m, dim, x, y)/sqrt(vector_product(m*dim, x, x));
660
+#ifdef DEBUG_PRINT
661
+    if (Verbose){
662
+      fprintf(stderr, "Outer iter = %d, res = %g Stress Majorization diff = %g\n",iter, res, diff);
663
+    }
664
+#endif
665
+    MEMCPY(x, y, sizeof(real)*m*dim);
666
+  }
667
+
668
+#ifdef DEBUG
669
+  _statistics[1] += iter-1;
670
+#endif
671
+
672
+ RETURN:
673
+  SparseMatrix_delete(Lwdd);
674
+  if (Lc) {
675
+    SparseMatrix_delete(Lc);
676
+    SparseMatrix_delete(Lw);
677
+  }
678
+
679
+  if (x0) FREE(x0);
680
+  if (y) FREE(y);
681
+  if (x00) FREE(x00);
682
+  return diff;
683
+  
684
+}
685
+
686
+void StressMajorizationSmoother_delete(StressMajorizationSmoother sm){
687
+  if (!sm) return;
688
+  if (sm->Lw) SparseMatrix_delete(sm->Lw);
689
+  if (sm->Lwd) SparseMatrix_delete(sm->Lwd);
690
+  if (sm->lambda) FREE(sm->lambda);
691
+  if (sm->data) sm->data_deallocator(sm->data);
692
+  FREE(sm);
693
+}
694
+
695
+
696
+TriangleSmoother TriangleSmoother_new(SparseMatrix A, int dim, real lambda0, real *x, int use_triangularization){
697
+  TriangleSmoother sm;
698
+  int i, j, k, m = A->m, *ia = A->ia, *ja = A->ja, *iw, *jw, *id, *jd, jdiag, nz;
699
+  SparseMatrix B;
700
+  real *avg_dist, *lambda, *d, *w, diag_d, diag_w, dist;
701
+  real s = 0, stop = 0, sbot = 0;
702
+
703
+  assert(SparseMatrix_is_symmetric(A, FALSE));
704
+
705
+  avg_dist = N_GNEW(m,real);
706
+
707
+  for (i = 0; i < m ;i++){
708
+    avg_dist[i] = 0;
709
+    nz = 0;
710
+    for (j = ia[i]; j < ia[i+1]; j++){
711
+      if (i == ja[j]) continue;
712
+      avg_dist[i] += distance(x, dim, i, ja[j]);
713
+      nz++;
714
+    }
715
+    assert(nz > 0);
716
+    avg_dist[i] /= nz;
717
+  }
718
+
719
+  sm = N_GNEW(1,struct TriangleSmoother_struct);
720
+  sm->scaling = 1;
721
+  sm->data = NULL;
722
+  sm->scheme = SM_SCHEME_NORMAL;
723
+
724
+  lambda = sm->lambda = N_GNEW(m,real);
725
+  for (i = 0; i < m; i++) sm->lambda[i] = lambda0;
726
+  
727
+  if (m > 2){
728
+    if (use_triangularization){
729
+      B= call_tri(m, dim, x);
730
+    } else {
731
+      B= call_tri2(m, dim, x);
732
+    }
733
+  } else {
734
+    B = SparseMatrix_copy(A);
735
+  }
736
+
737
+
738
+
739
+  sm->Lw = SparseMatrix_add(A, B);
740
+
741
+  SparseMatrix_delete(B);
742
+  sm->Lwd = SparseMatrix_copy(sm->Lw);
743
+  if (!(sm->Lw) || !(sm->Lwd)) {
744
+    TriangleSmoother_delete(sm);
745
+    return NULL;
746
+  }
747
+
748
+  iw = sm->Lw->ia; jw = sm->Lw->ja;
749
+  id = sm->Lwd->ia; jd = sm->Lwd->ja;
750
+  w = (real*) sm->Lw->a; d = (real*) sm->Lwd->a;
751
+
752
+  for (i = 0; i < m; i++){
753
+    diag_d = diag_w = 0;
754
+    jdiag = -1;
755
+    for (j = iw[i]; j < iw[i+1]; j++){
756
+      k = jw[j];
757
+      if (k == i){
758
+	jdiag = j;
759
+	continue;
760
+      }
761
+      /*      w[j] = -1./(ia[i+1]-ia[i]+ia[ja[j]+1]-ia[ja[j]]);
762
+	      w[j] = -2./(avg_dist[i]+avg_dist[k]);
763
+	      w[j] = -1.*/;/* use unit weight for now, later can try 1/(deg(i)+deg(k)) */
764
+      dist = pow(distance_cropped(x,dim,i,k),0.6);
765
+      w[j] = 1/(dist*dist);
766
+      diag_w += w[j];
767
+
768
+      /*      d[j] = w[j]*distance(x,dim,i,k);
769
+	      d[j] = w[j]*(avg_dist[i] + avg_dist[k])*0.5;*/
770
+      d[j] = w[j]*dist;
771
+      stop += d[j]*distance(x,dim,i,k);
772
+      sbot += d[j]*dist;
773
+      diag_d += d[j];
774
+
775
+    }
776
+
777
+    lambda[i] *= (-diag_w);/* alternatively don't do that then we have a constant penalty term scaled by lambda0 */
778
+
779
+    assert(jdiag >= 0);
780
+    w[jdiag] = -diag_w + lambda[i];
781
+    d[jdiag] = -diag_d;
782
+  }
783
+
784
+  s = stop/sbot;
785
+  for (i = 0; i < iw[m]; i++) d[i] *= s;
786
+  sm->scaling = s;
787
+
788
+  FREE(avg_dist);
789
+
790
+  return sm;
791
+}
792
+
793
+void TriangleSmoother_delete(TriangleSmoother sm){
794
+
795
+  StressMajorizationSmoother_delete(sm);
796
+
797
+}
798
+
799
+void TriangleSmoother_smooth(TriangleSmoother sm, int dim, real *x){
800
+
801
+  StressMajorizationSmoother_smooth(sm, dim, x, 50, 0.001);
802
+}
803
+
804
+
805
+
806
+
807
+/* ================================ spring and spring-electrical based smoother ================ */
808
+SpringSmoother SpringSmoother_new(SparseMatrix A, int dim, spring_electrical_control ctrl, real *x){
809
+  SpringSmoother sm;
810
+  int i, j, k, l, m = A->m, *ia = A->ia, *ja = A->ja, *id, *jd;
811
+  int *mask, nz;
812
+  real *d, *dd;
813
+  real *avg_dist;
814
+  SparseMatrix ID = NULL;
815
+
816
+  assert(SparseMatrix_is_symmetric(A, FALSE));
817
+
818
+  ID = ideal_distance_matrix(A, dim, x);
819
+  dd = (real*) ID->a;
820
+
821
+  sm = N_GNEW(1,struct SpringSmoother_struct);
822
+  mask = N_GNEW(m,int);
823
+
824
+  avg_dist = N_GNEW(m,real);
825
+
826
+  for (i = 0; i < m ;i++){
827
+    avg_dist[i] = 0;
828
+    nz = 0;
829
+    for (j = ia[i]; j < ia[i+1]; j++){
830
+      if (i == ja[j]) continue;
831
+      avg_dist[i] += distance(x, dim, i, ja[j]);
832
+      nz++;
833
+    }
834
+    assert(nz > 0);
835
+    avg_dist[i] /= nz;
836
+  }
837
+
838
+
839
+  for (i = 0; i < m; i++) mask[i] = -1;
840
+
841
+  nz = 0;
842
+  for (i = 0; i < m; i++){
843
+    mask[i] = i;
844
+    for (j = ia[i]; j < ia[i+1]; j++){
845
+      k = ja[j];
846
+      if (mask[k] != i){
847
+	mask[k] = i;
848
+	nz++;
849
+      }
850
+    }
851
+    for (j = ia[i]; j < ia[i+1]; j++){
852
+      k = ja[j];
853
+      for (l = ia[k]; l < ia[k+1]; l++){
854
+	if (mask[ja[l]] != i){
855
+	  mask[ja[l]] = i;
856
+	  nz++;
857
+	}
858
+      }
859
+    }
860
+  }
861
+
862
+  sm->D = SparseMatrix_new(m, m, nz, MATRIX_TYPE_REAL, FORMAT_CSR);
863
+  if (!(sm->D)){
864
+    SpringSmoother_delete(sm);
865
+    return NULL;
866
+  }
867
+
868
+  id = sm->D->ia; jd = sm->D->ja;
869
+  d = (real*) sm->D->a;
870
+  id[0] = 0;
871
+
872
+  nz = 0;
873
+  for (i = 0; i < m; i++){
874
+    mask[i] = i+m;
875
+    for (j = ia[i]; j < ia[i+1]; j++){
876
+      k = ja[j];
877
+      if (mask[k] != i+m){
878
+	mask[k] = i+m;
879
+	jd[nz] = k;
880
+	d[nz] = (avg_dist[i] + avg_dist[k])*0.5;
881
+	d[nz] = dd[j];
882
+	nz++;
883
+      }
884
+    }
885
+
886
+    for (j = ia[i]; j < ia[i+1]; j++){
887
+      k = ja[j];
888
+      for (l = ia[k]; l < ia[k+1]; l++){
889
+	if (mask[ja[l]] != i+m){
890
+	  mask[ja[l]] = i+m;
891
+	  jd[nz] = ja[l];
892
+	  d[nz] = (avg_dist[i] + 2*avg_dist[k] + avg_dist[ja[l]])*0.5;
893
+	  d[nz] = dd[j]+dd[l];
894
+	  nz++;
895
+	}
896
+      }
897
+    }
898
+    id[i+1] = nz;
899
+  }
900
+  sm->D->nz = nz;
901
+  sm->ctrl = spring_electrical_control_new();
902
+  *(sm->ctrl) = *ctrl;
903
+  sm->ctrl->random_start = FALSE;
904
+  sm->ctrl->multilevels = 1;
905
+  sm->ctrl->step /= 2;
906
+  sm->ctrl->maxiter = 20;
907
+
908
+  FREE(mask);
909
+  FREE(avg_dist);
910
+  SparseMatrix_delete(ID);
911
+
912
+  return sm;
913
+}
914
+
915
+
916
+void SpringSmoother_delete(SpringSmoother sm){
917
+  if (!sm) return;
918
+  if (sm->D) SparseMatrix_delete(sm->D);
919
+  if (sm->ctrl) spring_electrical_control_delete(sm->ctrl);
920
+}
921
+
922
+
923
+
924
+
925
+void SpringSmoother_smooth(SpringSmoother sm, SparseMatrix A, real *node_weights, int dim, real *x){
926
+  int flag = 0;
927
+
928
+  spring_electrical_spring_embedding(dim, A, sm->D, sm->ctrl, node_weights, x, &flag);
929
+  assert(!flag);
930
+
931
+}
932
+
933
+/*=============================== end of spring and spring-electrical based smoother =========== */
934
+
935
+void post_process_smoothing(int dim, SparseMatrix A, spring_electrical_control ctrl, real *node_weights, real *x, int *flag){
936
+#ifdef TIME
937
+  clock_t  cpu;
938
+#endif
939
+
940
+#ifdef TIME
941
+  cpu = clock();
942
+#endif
943
+  *flag = 0;
944
+
945
+  switch (ctrl->smoothing){
946
+  case SMOOTHING_RNG:
947
+  case SMOOTHING_TRIANGLE:{
948
+    TriangleSmoother sm;
949
+    
950
+    if (ctrl->smoothing == SMOOTHING_RNG){
951
+      sm = TriangleSmoother_new(A, dim, 0, x, FALSE);
952
+    } else {
953
+      sm = TriangleSmoother_new(A, dim, 0, x, TRUE);
954
+    }
955
+    TriangleSmoother_smooth(sm, dim, x);
956
+    TriangleSmoother_delete(sm);
957
+    break;
958
+  }
959
+  case SMOOTHING_STRESS_MAJORIZATION_GRAPH_DIST:
960
+  case SMOOTHING_STRESS_MAJORIZATION_POWER_DIST:
961
+  case SMOOTHING_STRESS_MAJORIZATION_AVG_DIST:
962
+    {
963
+      StressMajorizationSmoother sm;
964
+      int k, dist_scheme = IDEAL_AVG_DIST;
965
+
966
+      if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_GRAPH_DIST){
967
+	dist_scheme = IDEAL_GRAPH_DIST;
968
+      } else if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_AVG_DIST){
969
+	dist_scheme = IDEAL_AVG_DIST;
970
+      } else if (ctrl->smoothing == SMOOTHING_STRESS_MAJORIZATION_POWER_DIST){
971
+	dist_scheme = IDEAL_POWER_DIST;
972
+      }
973
+
974
+      for (k = 0; k < 1; k++){
975
+	sm = StressMajorizationSmoother2_new(A, dim, 0.05, x, dist_scheme);
976
+	StressMajorizationSmoother_smooth(sm, dim, x, 50, 0.001);
977
+	StressMajorizationSmoother_delete(sm);
978
+      }
979
+      break;
980
+    }
981
+  case SMOOTHING_SPRING:{
982
+    SpringSmoother sm;
983
+    int k;
984
+
985
+    for (k = 0; k < 1; k++){
986
+      sm = SpringSmoother_new(A, dim, ctrl, x);
987
+      SpringSmoother_smooth(sm, A, node_weights, dim, x);
988
+      SpringSmoother_delete(sm);
989
+    }
990
+
991
+    break;
992
+  }
993
+
994
+  }/* end switch between smoothing methods */
995
+
996
+#ifdef TIME
997
+  if (Verbose) fprintf(stderr, "post processing %f\n",((real) (clock() - cpu)) / CLOCKS_PER_SEC);
998
+#endif
999
+}