1 #include "SUMA_suma.h"
2 #include "matrix.h"
3 #include "SUMA_SurfWarp.h"
4
5 extern SUMA_SurfaceViewer *SUMAg_SVv;
6
7 /******************* Begin optimizing functions here ************************************************/
8 /* DON'T FORGET TO ADD FUNCTIONS TO THE SUMA_SURFWARP.H FILE!!! */
9
Matrix_Condition_Num(matrix M,FILE * condition_num)10 double Matrix_Condition_Num (matrix M, FILE *condition_num)
11 {
12 static char FuncName[]={"Matrix_Condition_Num"};
13 double *ev, ebot, emin, emax, cond = 0.0;
14 int i, nsmall=0 ;
15 SUMA_Boolean LocalHead = NOPE;
16 SUMA_ENTRY;
17
18 #ifndef PSINV_EPS
19 #define PSINV_EPS 1.e-12
20 #endif
21
22 ev = matrix_singvals( M ) ;
23 emax = 1.e-38 ;
24 for( i=0 ; i < M.cols ; i++ ){
25 if( ev[i] > emax ) emax = ev[i] ;
26 }
27
28 fprintf(condition_num, "E = [\n");
29 for (i=0 ; i < M.cols ; i++ ){
30 fprintf(condition_num, "%f\n", ev[i]);
31 }
32 fprintf(condition_num, "];\n \n");
33
34 ebot = sqrt(PSINV_EPS)*emax ; emin = 1.e+38 ;
35 for( i=0 ; i < M.cols ; i++ ){
36 if( ev[i] >= ebot && ev[i] < emin ) emin = ev[i] ;
37 if( ev[i] < ebot ) nsmall++ ;
38 }
39 if( nsmall > 0 ){
40 fprintf(stderr,
41 "** WARNING: Largest singular value=%g;"
42 " Implies strong collinearity in the input regressors\n", emax ) ;
43 }
44
45 if(LocalHead) fprintf(stderr,"min ev=%g max ev=%g\n",emin,emax ) ;
46
47 free((void *)ev) ;
48 if( emin <= 0.0 || emax <= 0.0 ){
49 fprintf(condition_num,"** Matrix condition: UNDEFINED: "
50 "min ev=%g max ev=%g ** VERY BAD **\n",emin,emax ) ;
51 } else {
52 cond = emax/emin ;
53 fprintf(condition_num,"Matrix condition : %f\n", cond) ;
54 }
55 SUMA_RETURN(cond);
56 }
57
Optimization_Kernel(MyCircleOpt * opt,double theta)58 double Optimization_Kernel(MyCircleOpt *opt, double theta)
59 {
60 static char FuncName[]={"Optimization_Kernel"};
61 double expand = 0.0;
62 SUMA_Boolean LocalHead = NOPE;
63 SUMA_ENTRY;
64
65 if(opt->sin_kern) {
66 if(theta > opt->Zero) { expand = SUMA_POW2((sin(theta)/theta))*exp(-0.5*theta*theta);
67 } else { expand = 1.0; }
68 }
69
70 expand = exp(-0.5*theta*theta);
71
72 SUMA_RETURN(expand);
73 }
74
Deformation_Kernel(MyCircleOpt * opt,double theta)75 double Deformation_Kernel(MyCircleOpt *opt, double theta)
76 {
77 static char FuncName[]={"Deformation_Kernel"};
78 double expand = 0.0;
79 SUMA_Boolean LocalHead = NOPE;
80 SUMA_ENTRY;
81
82 if(opt->sin_kern) {
83 if(theta > opt->Zero) { expand = SUMA_POW2((sin(theta)/theta))*exp(-0.5*theta*theta);
84 } else { expand = 1.0; }
85 }
86
87 SUMA_RETURN(expand);
88 }
89
Set_up_Control_Curve(MyCircleOpt * opt,SUMA_MX_VEC * ControlCurve)90 SUMA_Boolean Set_up_Control_Curve( MyCircleOpt *opt, SUMA_MX_VEC *ControlCurve )
91 {
92 static char FuncName[]={"Set_up_Control_Curve"};
93 int i = 0, j, k, i3, j3, k2;
94 double arc_theta, arc_nrm[3], mag_arc_nrm;
95 double *dp = NULL, *dp2 = NULL;
96 double theta = 0.0, nrm[3] = {0.0, 0.0, 0.0};
97
98 SUMA_Boolean LocalHead = NOPE;
99 SUMA_ENTRY;
100
101 for (j=0; j<opt->N_ctrl_points; ++j) {
102 j3 = 3*j;
103
104 /* Store intial location of control point. */
105 k=0;
106 dp = mxvdp3(ControlCurve, i, j, k);
107 dp[0] = opt->CtrlPts_i[j3 ];
108 dp[1] = opt->CtrlPts_i[j3+1];
109 dp[2] = opt->CtrlPts_i[j3+2];
110 dp = NULL;
111
112 /* Calculate and store intermediate steps of control point path. */
113 SUMA_ANGLE_DIST_NC((&(opt->CtrlPts_f[j3])), (&(opt->CtrlPts_i[j3])), arc_theta, arc_nrm);
114 arc_theta = arc_theta/opt->M_time_steps;
115 mag_arc_nrm = sqrt( SUMA_POW2(arc_nrm[0]) + SUMA_POW2(arc_nrm[1]) + SUMA_POW2(arc_nrm[2]) );
116 if(mag_arc_nrm > opt->Zero) {
117 arc_nrm[0] *= (1.0/mag_arc_nrm);
118 arc_nrm[1] *= (1.0/mag_arc_nrm);
119 arc_nrm[2] *= (1.0/mag_arc_nrm);
120 }
121 dp = mxvdp3(ControlCurve, i, j, k);
122
123 for (k=1; k<opt->M_time_steps; ++k) {
124 dp2 = mxvdp3(ControlCurve, i, j, k);
125 SUMA_ROTATE_ABOUT_AXIS( (&(dp[0])), arc_nrm, arc_theta, (&(dp2[0])) );
126 if(LocalHead) fprintf(SUMA_STDERR, "Rotated to k=1 location: %f %f %f\n", dp2[0], dp2[1], dp2[2]);
127 dp = dp2;
128 }
129 dp = NULL; dp2 = NULL;
130
131 /* Store final location of control point. */
132 k=opt->M_time_steps;
133 dp = mxvdp3(ControlCurve, i, j, k);
134 dp[0] = opt->CtrlPts_f[j3 ];
135 dp[1] = opt->CtrlPts_f[j3+1];
136 dp[2] = opt->CtrlPts_f[j3+2];
137 dp = NULL;
138 }
139
140 #if 0
141 /* For comparing the angular distance between the initial and final control points. */
142 FILE *angle_dist = NULL;
143 angle_dist = fopen( "angle_dist_check.1D", "w" );
144 for (j=0; j<opt->N_ctrl_points; ++j) {
145 j3 = 3*j;
146 k=0; k2=opt->M_time_steps;
147 dp = mxvdp3(ControlCurve, i, j, k);
148 dp2 = mxvdp3(ControlCurve, i, j, k2);
149
150 theta = 0.0; nrm[0] = 0.0; nrm[1] = 0.0; nrm[2] = 0.0;
151 SUMA_ANGLE_DIST_NC((&(dp2[0])), (&(dp[0])), theta, nrm)
152 fprintf(angle_dist, "%d %f\n", j, theta);
153 }
154 fclose (angle_dist); angle_dist = NULL;
155 #endif
156
157 SUMA_RETURN(YUP);
158 }
SUMA_Perturbation2SDO(SUMA_MX_VEC * ControlCurve,SUMA_MX_VEC * Perturb_Vec,char * Label,int p,double scale)159 SUMA_SegmentDO *SUMA_Perturbation2SDO(SUMA_MX_VEC *ControlCurve, SUMA_MX_VEC *Perturb_Vec, char *Label, int p, double scale)
160 {
161 static char FuncName[]={"SUMA_Perturbation2SDO"};
162 SUMA_SegmentDO *SDO=NULL;
163 int N_n, oriented, NodeBased, Stipple, i, m, d, kcc;
164 char *idcode_str=NULL, *Parent_idcode_str=NULL;
165 float LineWidth;
166 double *dp=NULL, *dp_new=NULL;
167 int *NodeId=NULL;
168 float *n0=NULL, *n1=NULL;
169 float *colv=NULL, *thickv=NULL;
170 float acol[4], LineCol[4] = { 1.000000, 0.300000, 1.000000, 1.000000 };
171
172 SUMA_ENTRY;
173
174 oriented = 1;
175 Stipple = 0;
176 NodeBased = 0;
177 LineWidth = 8;
178 SUMA_NEW_ID(idcode_str, Label);
179
180 N_n = ControlCurve->dims[1] * (ControlCurve->dims[2]);
181 n0 = (float *) SUMA_malloc(sizeof(float)*N_n*3);
182 n1 = (float *) SUMA_malloc(sizeof(float)*N_n*3);
183 colv = (float *) SUMA_malloc(sizeof(float)*N_n*4);
184 thickv = (float *) SUMA_malloc(sizeof(float)*N_n);
185
186
187 kcc = 0;
188 for(i=0; i<ControlCurve->dims[1]; ++i) {
189 for (m=0; m<ControlCurve->dims[2]; ++m) {
190 if(1) {
191 dp = mxvdp3(ControlCurve, 0, i, m);
192 dp_new = mxvdp4(Perturb_Vec, 0, i, m, p);
193 for (d=0;d<3;++d) {
194 n0[3*kcc+d] = dp[d];
195 n1[3*kcc+d] = dp[d]+scale*dp_new[d];
196 }
197 }
198 if (p==0) {
199 colv[4*kcc+0] = 1.0;
200 colv[4*kcc+1] = colv[4*kcc+2] = colv[4*kcc+3] = 0.0;
201 } else{
202 colv[4*kcc+1] = 1.0;
203 colv[4*kcc+0] = colv[4*kcc+2] = colv[4*kcc+3] = 0.0;
204 }
205 thickv[kcc] = 2;
206 ++kcc;
207 dp = NULL; dp_new = NULL;
208 }
209 }
210
211 SDO = SUMA_CreateSegmentDO( N_n, oriented, NodeBased, Stipple,
212 Label, idcode_str, Parent_idcode_str,
213 NOT_SET_type, NULL,
214 LineWidth, LineCol,
215 NodeId, NULL, n0, n1,
216 colv, thickv );
217
218 if (idcode_str) SUMA_free(idcode_str); idcode_str = NULL;
219 if (n0) SUMA_free(n0); n0 = NULL;
220 if (n1) SUMA_free(n1); n1 = NULL;
221 if (colv) SUMA_free(colv); colv = NULL;
222 if (thickv) SUMA_free(thickv); thickv = NULL;
223
224 SUMA_RETURN(SDO);
225 }
226
Perturbations(MyCircleOpt * opt,SUMA_MX_VEC * ControlCurve,SUMA_MX_VEC * MaxStep,SUMA_MX_VEC * Perturb_Vec,SUMA_GENERIC_ARGV_PARSE * ps)227 SUMA_Boolean Perturbations( MyCircleOpt *opt, SUMA_MX_VEC *ControlCurve,
228 SUMA_MX_VEC *MaxStep, SUMA_MX_VEC *Perturb_Vec,
229 SUMA_GENERIC_ARGV_PARSE *ps )
230 {
231 static char FuncName[]={"Perturbations"};
232 int i, j, k, p;
233 double theta= 0.0, theta_2 = 0.0, nrm[3], p1[3], p1_mag, nrm_mag, theta_fac;
234 double *dp = NULL, *dp2 = NULL, *dp_m1 = NULL, *dp_m2 = NULL, *dp_p1 = NULL, *dp_p2 = NULL;
235
236 SUMA_Boolean LocalHead = NOPE;
237 SUMA_ENTRY;
238
239 for (k=1; k<opt->M_time_steps; ++k) { /* Compare the ith c.p. to all other c.p., at time k. */
240 for(i=0; i<opt->N_ctrl_points; ++i) {
241 dp = mxvdp3(ControlCurve, 0, i, k); /* Pointer to control point of interest, at time k. */
242 if(LocalHead) fprintf(SUMA_STDERR, "Control Point(i=%d,k= %d) = %f %f %f\n", i, k, dp[0], dp[1], dp[2]);
243
244 /* Compare control point of interest to all other control points. */
245 for (j=0; j<opt->N_ctrl_points; ++j) {
246 dp2 = mxvdp3(ControlCurve, 0, j, k);
247 if(LocalHead) fprintf(SUMA_STDERR, "Control Point(j=%d, k=%d) = %f %f %f\n", j, k, dp2[0], dp2[1], dp2[2]);
248
249 if( j<1 ) SUMA_ANGLE_DIST_NC((&(dp2[0])), (&(dp[0])), theta, nrm);
250 if(j>0) SUMA_ANGLE_DIST_NC((&(dp2[0])), (&(dp[0])), theta_2, nrm);
251 if(LocalHead) {
252 if(theta) fprintf(SUMA_STDERR, "Theta(i=%d,j=%d,k=%d) = %f\n", i, j, k, theta);
253 if(theta_2) fprintf(SUMA_STDERR, "Theta_2(i=%d,j=%d,k=%d) = %f\n", i, j, k, theta_2);
254 }
255 if(theta<opt->Zero) theta = theta_2;
256 else if(theta_2<opt->Zero) theta = theta;
257 else if(theta_2 && theta) {
258 if(theta_2 < theta) theta = theta_2;
259 else theta = theta;
260 }
261 if(LocalHead) fprintf(SUMA_STDERR, "Theta_final(i=%d,j=%d,k=%d) = %f\n", i, j, k, theta);
262 theta_2 = 0.0;
263 }/* Now, theta is smallest angle between the ith cp and all j other cp, at time m. */
264
265 /* Compare location of control point at time k to location of that control point at time k-1. */
266 dp_m1 = mxvdp3(ControlCurve, 0, i, k-1);
267 SUMA_ANGLE_DIST_NC((&(dp[0])), (&(dp_m1[0])), theta_2, nrm);
268 if(LocalHead) if(theta_2) fprintf(SUMA_STDERR, "Theta_k-1(i=%d,j=%d,k=%d) = %f\n", i, j, k, theta_2);
269 if(theta<opt->Zero) theta = theta_2;
270 else if(theta_2<opt->Zero) theta = theta;
271 else if(theta_2 && theta) {
272 if(theta_2 < theta) theta = theta_2;
273 else theta = theta;
274 }
275 nrm[0] = 0.0; nrm[1] = 0.0; nrm[2] = 0.0;
276
277 /* Compare location of control point at time k to location of that control point at time k+1. */
278 dp_m2 = mxvdp3(ControlCurve, 0, i, k+1);
279 SUMA_ANGLE_DIST_NC((&(dp_m2[0])), (&(dp[0])), theta_2, nrm);
280 if(LocalHead) if(theta_2) fprintf(SUMA_STDERR, "Theta_k+1(i=%d,j=%d,k=%d) = %f\n", i, j, k, theta_2);
281 if(theta<opt->Zero) theta = theta_2;
282 else if(theta_2<opt->Zero) theta = theta;
283 else if(theta_2 && theta) {
284 if(theta_2 < theta) theta = theta_2;
285 else theta = theta;
286 }
287 if(LocalHead) fprintf(SUMA_STDERR, "Min Theta (i=%d, k=%d) = %f\n", i, k, theta);
288
289 /* This is the theta that I need to store!! There's one for every i and m. The m's are the "knots", counted by index k.*/
290 mxvd2(MaxStep, i, k) = theta;
291
292 /* Directed along the curve. */
293 dp_p1 = mxvdp4(Perturb_Vec, 0, i, k, 0);
294 SUMA_MT_CROSS(p1, (&(nrm[0])), (&(dp[0])) );
295 p1_mag = sqrt( SUMA_POW2(p1[0]) + SUMA_POW2(p1[1]) + SUMA_POW2(p1[2]) );
296 if(p1_mag > opt->Zero) {
297 p1[0] *= (1.0/p1_mag);
298 p1[1] *= (1.0/p1_mag);
299 p1[2] *= (1.0/p1_mag);
300 }
301 if(LocalHead) fprintf( SUMA_STDERR, "Direction along curve - checking if unit vec. %f %f %f\n", p1[0], p1[1], p1[2]);
302 theta_fac = (1.0/8.0)*theta; /* This factor only affects the perturbation vector's magnitude */
303 dp_p1[0] = theta_fac*p1[0];
304 dp_p1[1] = theta_fac*p1[1];
305 dp_p1[2] = theta_fac*p1[2];
306 p1[0] = 0.0; p1[1] = 0.0; p1[2] = 0.0;
307 if(LocalHead) {
308 fprintf( SUMA_STDERR, "Direction along curve - complete vector. %f %f %f\n", dp_p1[0], dp_p1[1], dp_p1[2]);
309 fprintf(SUMA_STDERR, "Perturb_Vec_along_curve(i=%d, k=%d, p=0) = %f %f %f\n", i, k,
310 mxvd4(Perturb_Vec, 0, i, k, 0), mxvd4(Perturb_Vec, 1, i, k, 0), mxvd4(Perturb_Vec, 2, i, k, 0) );
311 }
312
313 /* Directed transverse to the curve. */
314 dp_p2 = mxvdp4(Perturb_Vec, 0, i, k, 1);
315 nrm_mag = sqrt( SUMA_POW2(nrm[0]) + SUMA_POW2(nrm[1]) + SUMA_POW2(nrm[2]) );
316 if(nrm_mag > opt->Zero) {
317 nrm[0] *= (1.0/nrm_mag);
318 nrm[1] *= (1.0/nrm_mag);
319 nrm[2] *= (1.0/nrm_mag);
320 }
321 if(LocalHead) fprintf( SUMA_STDERR, "Direction perpendicular to curve - checking if unit vec. %f %f %f\n", nrm[0], nrm[1], nrm[2]);
322 dp_p2[0] = theta_fac*nrm[0];
323 dp_p2[1] = theta_fac*nrm[1];
324 dp_p2[2] = theta_fac*nrm[2];
325
326 /* Check perturbation vectors. These vectors should be perpendicular. */
327 if(LocalHead) fprintf( SUMA_STDERR, "Dot product of the two perturbation vectors = %g\n", SUMA_MT_DOT((&(dp_p1[0])), (&(dp_p2[0]))) );
328
329 dp = dp2 = dp_m1 = dp_m2 = dp_p1 = dp_p2 = NULL;
330 theta = theta_2 = 0.0;
331 nrm[0] = 0.0; nrm[1] = 0.0; nrm[2] = 0.0;
332 nrm_mag = p1_mag = 0.0;
333 }
334 }
335
336 if (ps->cs->talk_suma) {
337 SUMA_SegmentDO *sdo=NULL;
338 NI_group *ngr = NULL;
339 int suc;
340 /* Send Perturbation vector to SUMA */
341 sdo = SUMA_Perturbation2SDO(opt->ControlCurve, Perturb_Vec, "PV0", 0, 5.0);
342 /* change that thing to NIML */
343 ngr = SUMA_SDO2niSDO(sdo);
344 #if 0
345 /* write it to diks, for kicks */
346 sprintf(stmp, "file:PV0_%d.niml.SDO", opt->iter_count);
347 NEL_WRITE_TX(ngr, stmp, suc);
348 #endif
349 /* send it to suma */
350 if (!SUMA_SendToSuma (opt->SO, ps->cs, (void *)ngr, SUMA_SEGMENT_OBJECT, 1)) {
351 SUMA_S_Warn("Failed in SUMA_SendToSuma\nCommunication halted.");
352 ps->cs->talk_suma = NOPE;
353 }
354 SUMA_free_SegmentDO(sdo); sdo = NULL;
355 NI_free_element(ngr); ngr = NULL;
356 /* again for direction 2 */
357 sdo = SUMA_Perturbation2SDO(opt->ControlCurve, Perturb_Vec, "PV1", 1, 5.0);
358 /* change that thing to NIML */
359 ngr = SUMA_SDO2niSDO(sdo);
360 #if 0
361 /* write it to diks, for kicks */
362 sprintf(stmp, "file:PV1_%d.niml.SDO", opt->iter_count);
363 NEL_WRITE_TX(ngr, stmp, suc);
364 #endif
365 /* send it to suma */
366 if (!SUMA_SendToSuma (opt->SO, ps->cs, (void *)ngr, SUMA_SEGMENT_OBJECT, 1)) {
367 SUMA_S_Warn("Failed in SUMA_SendToSuma\nCommunication halted.");
368 ps->cs->talk_suma = NOPE;
369 }
370 SUMA_free_SegmentDO(sdo); sdo = NULL;
371 NI_free_element(ngr); ngr = NULL;
372 if ( opt->pause > 1) {
373 SUMA_PAUSE_PROMPT("Pausing after perturbation directions\nDo something to proceed.\n");
374 }
375 }
376 SUMA_RETURN(YUP);
377 }
378
Print_Matrix(MyCircleOpt * opt,matrix M,FILE * fp)379 SUMA_Boolean Print_Matrix( MyCircleOpt *opt, matrix M, FILE *fp ) /* Assumes # of rows and # of columns depend on # of control points. */
380 {
381 static char FuncName[]={"Print_Matrix"};
382 int r, c, r3, c3, k;
383 FILE *fpl = NULL; /* fp local */
384
385 SUMA_Boolean LocalHead = NOPE;
386 SUMA_ENTRY;
387
388 if(fp) fpl = fp;
389 else fpl = SUMA_STDERR;
390
391 fprintf(fpl, "[\n");
392 for (r=0; r<opt->N_ctrl_points; ++r) {
393 for (k=0; k<3; ++k) {
394 for(c=0; c<opt->N_ctrl_points; ++c) {
395 fprintf (fpl,"%11.8f %11.8f %11.8f ",
396 M.elts [ (3*r+k) ][ (3*c ) ],
397 M.elts [ (3*r+k) ][ (3*c+1) ],
398 M.elts [ (3*r+k) ][ (3*c+2) ]);
399 }
400 fprintf(fpl,"\n");
401 }
402 }
403 fprintf(fpl, "];\n\n");
404
405 fpl = NULL;
406 SUMA_RETURN(YUP);
407 }
408
Rotation_Matrix(MyCircleOpt * opt,vector X,matrix M)409 SUMA_Boolean Rotation_Matrix( MyCircleOpt *opt, vector X, matrix M )
410 {
411 static char FuncName[]={"Rotation_Matrix"};
412 int r, c, r3, c3, k;
413 double Mcr[3][3], t_cr = 0.0, nrm_cr[3], expand_cr = 0.0;
414
415 SUMA_Boolean LocalHead = NOPE;
416
417 SUMA_ENTRY;
418
419 for(r=0; r<opt->N_ctrl_points; ++r) { /* r for row. */
420 for(c=0; c<opt->N_ctrl_points; ++c) { /* c for column. */
421 r3 = 3*r; c3 = 3*c;
422
423 t_cr = 0.0; expand_cr = 0.0;
424 /* Create the rotation matrix. */
425 SUMA_3D_Rotation_Matrix( (&(X.elts[c3])), (&(X.elts[r3])), Mcr, t_cr, nrm_cr);
426
427 expand_cr = Optimization_Kernel(opt, t_cr);
428
429 /* Assemble the matrix and include the expansion factor. */
430 M.elts[ (r3 ) ][ (c3 ) ] = expand_cr * Mcr[0][0];
431 M.elts[ (r3 ) ][ (c3+1) ] = expand_cr * Mcr[0][1];
432 M.elts[ (r3 ) ][ (c3+2) ] = expand_cr * Mcr[0][2];
433 M.elts[ (r3+1) ][ (c3 ) ] = expand_cr * Mcr[1][0];
434 M.elts[ (r3+1) ][ (c3+1) ] = expand_cr * Mcr[1][1];
435 M.elts[ (r3+1) ][ (c3+2) ] = expand_cr * Mcr[1][2];
436 M.elts[ (r3+2) ][ (c3 ) ] = expand_cr * Mcr[2][0];
437 M.elts[ (r3+2) ][ (c3+1) ] = expand_cr * Mcr[2][1];
438 M.elts[ (r3+2) ][ (c3+2) ] = expand_cr * Mcr[2][2];
439 }
440 }
441
442 if(LocalHead) Print_Matrix(opt, M, NULL);
443
444 SUMA_RETURN(YUP);
445 }
446
447 /* See Snip_2.c for Sm2SD0 function. Function for plotting Sm, but Sm isn't what we want to plot. */
448
Change_in_Energy(MyCircleOpt * opt,SUMA_MX_VEC * ControlCurve,SUMA_MX_VEC * Perturb_Vec,SUMA_MX_VEC * Del_S,FILE * condition_num,FILE * condition_num_only)449 SUMA_Boolean Change_in_Energy( MyCircleOpt *opt, SUMA_MX_VEC *ControlCurve, SUMA_MX_VEC *Perturb_Vec,
450 SUMA_MX_VEC *Del_S, FILE *condition_num, FILE *condition_num_only)
451 {
452 static char FuncName[]={"Change_in_Energy"};
453 matrix Kern, Kern_p, delKern, KernI, delKernI, Xm_t, A, B;
454 matrix *nullptr = NULL;
455 vector Xm, Xm_mid, Xm_p, Pert, del_S1, del_S2, del_SF;
456 int nr, nc, i, i3, q, m, p, r, c, k, j, j3;
457 double mag_mid, mag_p, cond = 0.0;
458 double *Cp_list = NULL, *dp = NULL, *dp_m1 = NULL, *dp_q = NULL;
459
460 SUMA_Boolean LocalHead = NOPE;
461
462 SUMA_ENTRY;
463
464 if (opt->dbg_flag > 2) LocalHead = YUP;
465
466 nr = 3 * opt->N_ctrl_points; /* number of rows */
467 nc = 3 * opt->N_ctrl_points; /* number of columns */
468
469 vector_initialize(&Xm);
470 vector_create( (3*opt->N_ctrl_points), &Xm );
471 vector_initialize(&Xm_mid);
472 vector_create( (3*opt->N_ctrl_points), &Xm_mid );
473 vector_initialize(&Xm_p);
474 vector_create( (3*opt->N_ctrl_points), &Xm_p );
475 vector_initialize(&Pert);
476 vector_create( (3*opt->N_ctrl_points), &Pert );
477
478 vector_initialize(&del_S1);
479 vector_create(1, &del_S1);
480 vector_initialize(&del_S2);
481 vector_create(1, &del_S2);
482 vector_initialize(&del_SF);
483 vector_create(1, &del_SF);
484
485 if(LocalHead) fprintf( SUMA_STDERR, "%s: Finished allocating matrices and vectors.\n", FuncName);
486
487 for(p=0; p<2; ++p) {
488 for(q=0; q<2; ++q) {
489 for(m=0; m<opt->M_time_steps; ++m) {
490
491 if(LocalHead) fprintf(SUMA_STDERR, "WHAT'S GOING ON? q = %d, p = %d, m = %d\n", q, p, m);
492
493 matrix_initialize(&Xm_t);
494 matrix_create(1, (3*opt->N_ctrl_points), &Xm_t);
495
496 /* Set vectors to zero, so can put in new values. */
497 for(i=0; i<3*opt->N_ctrl_points; ++i) {
498 Xm.elts[i] = 0.0;
499 Xm_mid.elts[i] = 0.0;
500 Xm_p.elts[i] = 0.0;
501 Xm_t.elts[0][i] = 0.0;
502 Pert.elts[i] = 0.0;
503 }
504
505 if(LocalHead) fprintf(SUMA_STDERR, "Control Point Locations:\n" );
506 for(i=0; i<opt->N_ctrl_points; ++i) {
507 i3 = 3*i;
508 dp = mxvdp3(ControlCurve, 0, i, m);
509 dp_m1 = mxvdp3(ControlCurve, 0, i, m+1);
510 Xm_mid.elts[i3 ] = 0.5*(dp_m1[0] + dp[0]);
511 Xm_mid.elts[i3+1] = 0.5*(dp_m1[1] + dp[1]);
512 Xm_mid.elts[i3+2] = 0.5*(dp_m1[2] + dp[2]);
513
514 /* Project onto the sphere. */
515 mag_mid = sqrt( SUMA_POW2(Xm_mid.elts[i3 ]) + SUMA_POW2(Xm_mid.elts[i3+1]) + SUMA_POW2(Xm_mid.elts[i3+2]));
516 if(mag_mid) {
517 Xm_mid.elts[i3 ] = Xm_mid.elts[i3 ]/mag_mid;
518 Xm_mid.elts[i3+1] = Xm_mid.elts[i3+1]/mag_mid;
519 Xm_mid.elts[i3+2] = Xm_mid.elts[i3+2]/mag_mid;
520 }
521
522 Xm.elts[i3 ] = (dp_m1[0] - dp[0]);
523 Xm.elts[i3+1] = (dp_m1[1] - dp[1]);
524 Xm.elts[i3+2] = (dp_m1[2] - dp[2]);
525
526 /* Need transpose of Xm difference. This value is not projected to the sphere, I don't think. */
527 /* Subtracting two locations does not give a location that makes sense here. I think this comes from
528 the derivative estimation of the velocity. */
529 Xm_t.elts[0][i3 ] = (dp_m1[0] - dp[0]);
530 Xm_t.elts[0][i3+1] = (dp_m1[1] - dp[1]);
531 Xm_t.elts[0][i3+2] = (dp_m1[2] - dp[2]);
532
533 if(LocalHead) {
534 fprintf(SUMA_STDERR, "dp = [%f; %f; %f];\n", dp[0], dp[1], dp[2]);
535 fprintf(SUMA_STDERR, "dp_m1 = [%f; %f; %f];\n", dp_m1[0], dp_m1[1], dp_m1[2]);
536 }
537 }
538
539
540 /* Need to perturb one point at a time. */
541 for(i=0; i<opt->N_ctrl_points; ++i) {
542
543 if(LocalHead) fprintf(SUMA_STDERR, "BEGINNING OF LOOP, q=%d, p=%d, m=%d, i=%d\n", q, p, m, i);
544 /* Create temporary matrices each time through loop. */
545 matrix_initialize(&Kern);
546 matrix_create(nr, nc, &Kern);
547 matrix_initialize(&Kern_p); /* p for perturbation */
548 matrix_create(nr, nc, &Kern_p);
549 matrix_initialize(&delKern); /* del for delta */
550 matrix_create(nr, nc, &delKern);
551 matrix_initialize(&KernI); /* I for inverse */
552 matrix_create(nr, nc, &KernI);
553 matrix_initialize(&delKernI);
554 matrix_create(nr, nc, &delKernI);
555
556
557 j3 = 3*j;
558 i3 = 3*i;
559 if(q == 0) dp_q = mxvdp4(Perturb_Vec, 0, i, m, p); /* q=0 */
560 if(q == 1) dp_q = mxvdp4(Perturb_Vec, 0, i, m+1, p); /* q=1 */
561
562 for(j=0; j<3*opt->N_ctrl_points; ++j) {Pert.elts[j] = 0.0;}
563 /* Pert.elts is all zeros except for one perturbation vector. Only three rows have values. x-y-z */
564 Pert.elts[i3 ] = 0.5*dp_q[0];
565 Pert.elts[i3+1] = 0.5*dp_q[1]; /* 0.5 used for calculations, will store perturbation vectors with out this scaler */
566 Pert.elts[i3+2] = 0.5*dp_q[2];
567
568 vector_add( Xm_mid, Pert, &Xm_p); /* Xm_p is the perturbed midpoint vector. */
569
570 if(LocalHead) fprintf(SUMA_STDERR, "Perturbation Vector: [%f; %f; %f]\n", dp_q[0], dp_q[1], dp_q[2]);
571
572 /* Project onto the sphere. */
573 for(j=0; j<opt->N_ctrl_points; ++j) {
574 j3 = 3*j;
575 mag_p = sqrt( SUMA_POW2(Xm_p.elts[j3 ]) + SUMA_POW2(Xm_p.elts[j3+1]) + SUMA_POW2(Xm_p.elts[j3+2]));
576 if(mag_p) {
577 Xm_p.elts[j3 ] = Xm_p.elts[j3 ]/mag_p;
578 Xm_p.elts[j3+1] = Xm_p.elts[j3+1]/mag_p;
579 Xm_p.elts[j3+2] = Xm_p.elts[j3+2]/mag_p;
580 }
581 }
582
583 Pert.elts[i3 ] = dp_q[0]; /* Need perturbation vector for later that is not scaled by 0.5 */
584 Pert.elts[i3+1] = dp_q[1];
585 Pert.elts[i3+2] = dp_q[2];
586
587 if(LocalHead) {
588 fprintf(SUMA_STDERR, "Check outside of loop. m = %d\n", m);
589 for(j=0; j<opt->N_ctrl_points; ++j) {
590 j3 = 3*j;
591 fprintf(SUMA_STDERR, "Xm_mid(%d) = [%f; %f; %f]\n", j, Xm_mid.elts[j3 ], Xm_mid.elts[j3+1], Xm_mid.elts[j3+2]);
592 fprintf(SUMA_STDERR, "Xm_p(%d) = [%f; %f; %f]\n", j, Xm_p.elts[i3 ], Xm_p.elts[j3+1], Xm_p.elts[i3+2]);
593 fprintf(SUMA_STDERR, "Pert(%d) = [%f; %f; %f]\n", j, Pert.elts[j3 ], Pert.elts[j3+1], Pert.elts[j3+2]);
594 }
595 }
596
597 if(LocalHead){
598 fprintf(SUMA_STDERR, "PERT.ELTS:\n");
599 for(j=0; j<3*opt->N_ctrl_points; ++j) {fprintf(SUMA_STDERR, "%f\n", Pert.elts[j]);}
600 }
601
602 Rotation_Matrix(opt, Xm_mid, Kern);
603
604 /* This is the matrix whose condition number should be checked. */
605 if(i<1 && q<1 && p<1) {
606 fprintf(condition_num, "Kern (m=%d) = ", m);
607 Print_Matrix(opt, Kern, condition_num);
608 cond = Matrix_Condition_Num( Kern, condition_num );
609 fprintf(condition_num, "%f\n", cond);
610 fprintf(condition_num_only, "%f\n", cond);
611 }
612
613
614
615 matrix_psinv(Kern, nullptr, &KernI);
616 if(LocalHead) {
617 fprintf(SUMA_STDERR, "\nKern = "); Print_Matrix(opt, Kern, NULL);
618 fprintf(SUMA_STDERR, "\nKernI = "); Print_Matrix(opt, KernI, NULL);
619 }
620
621 Rotation_Matrix(opt, Xm_p, Kern_p);
622 if(LocalHead) { fprintf(SUMA_STDERR, "\nKern_p = "); Print_Matrix(opt, Kern_p, NULL); }
623
624 /* Calculate delta K */
625 matrix_subtract(Kern_p, Kern, &delKern);
626
627 if(LocalHead) { fprintf(SUMA_STDERR, "\ndelKern = "); Print_Matrix(opt, delKern, NULL); }
628 if(LocalHead) { fprintf(SUMA_STDERR, "\nKernI = "); Print_Matrix(opt, KernI, NULL); }
629
630 /*****************************/
631 matrix_initialize(&A);
632 matrix_create(nr, nc, &A);
633 matrix_initialize(&B);
634 matrix_create(nr, nc, &B);
635
636 /* Use K and delta K to calculate inverse delta K */
637 matrix_multiply(KernI, delKern, &A);
638 matrix_multiply(A, KernI, &B);
639 matrix_scale(-1.0, B, &delKernI);
640
641 if(LocalHead) { fprintf(SUMA_STDERR, "\ndelKernI = "); Print_Matrix(opt, delKernI, NULL); }
642
643 matrix_destroy(&A);
644 matrix_destroy(&B);
645
646 /****************************/
647 /* Put it all together in the delta S equation. */
648 matrix_initialize(&A);
649 matrix_create(1, (3*opt->N_ctrl_points), &A);
650 matrix_initialize(&B);
651 matrix_create(1, (3*opt->N_ctrl_points), &B);
652
653 /* JUST CHANGED THIS. 11/07/06. BEFORE HAD q==1 ASSOCIATED WITH A -2 SCALER. */
654 /* q describes which point perturbed. If q = 0, then the m point is perturbed. If q = 1, then the
655 m+1 point is perturbed. */
656 /* MUST BE A +2 SCALER WHEN USING M+1 POINT FOR PERTURBATION. */
657 if(q == 0) matrix_scale(-2.0, Xm_t, &A);
658 if(q == 1) matrix_scale(+2.0, Xm_t, &A);
659
660 matrix_multiply(A, KernI, &B);
661 vector_multiply(B, Pert, &del_S1);
662
663 if(LocalHead) {
664 fprintf(SUMA_STDERR, "Xm_t = [\n");
665 for(j=0; j<(3*opt->N_ctrl_points); ++j) fprintf(SUMA_STDERR, "%f ", (Xm_t.elts[0][j]));
666 fprintf(SUMA_STDERR, "\n];\n");
667 fprintf(SUMA_STDERR, "del_S1 = %f\n", del_S1.elts[0]);
668 }
669
670 matrix_destroy(&A);
671 matrix_destroy(&B);
672
673 /****************************/
674 matrix_initialize(&B);
675 matrix_create(1, (3*opt->N_ctrl_points), &B);
676
677 matrix_multiply(Xm_t, delKernI, &B);
678 vector_multiply(B, Xm, &del_S2);
679
680 vector_add(del_S1, del_S2, &del_SF);
681
682 if(LocalHead) {
683 fprintf(SUMA_STDERR, "Xm = [\n");
684 for(j=0; j<(3*opt->N_ctrl_points); ++j) fprintf(SUMA_STDERR, "%f;\n", (Xm.elts[j]));
685 fprintf(SUMA_STDERR, "\n];\n");
686 fprintf(SUMA_STDERR, "del_S2 = %f\n", del_S2.elts[0]);
687 fprintf(SUMA_STDERR, "del_SF = %f\n", del_SF.elts[0]);
688 }
689
690 if(LocalHead) fprintf(SUMA_STDERR, "CHECK THE INDICIES!!!i = %d, m = %d, q = %d, p = %d\n", i, m, q, p);
691 mxvd4(Del_S, i, m, q, p) = del_SF.elts[0];
692 if(LocalHead) fprintf(SUMA_STDERR, "del_SF = %11.8f\n", del_SF.elts[0]);
693 matrix_destroy(&B);
694
695 /* Clean up */
696 del_S1.elts[0] = 0.0; del_S2.elts[0] = 0.0; del_SF.elts[0] = 0.0;
697
698 matrix_destroy(&Kern);
699 matrix_destroy(&KernI);
700 matrix_destroy(&Kern_p);
701 matrix_destroy(&delKern);
702 matrix_destroy(&delKernI);
703 }
704
705 matrix_destroy(&Xm_t);
706 }
707 }
708 }
709 if(LocalHead) fprintf( SUMA_STDERR, "%s: Finished del_S loop.\n", FuncName);
710
711 vector_destroy(&Xm);
712 vector_destroy(&Xm_mid);
713 vector_destroy(&Xm_p);
714 vector_destroy(&Pert);
715 vector_destroy(&del_S1);
716 vector_destroy(&del_S2);
717 vector_destroy(&del_SF);
718
719 if(LocalHead) fprintf( SUMA_STDERR, "\n\n\n\n\n");
720
721 SUMA_RETURN(YUP);
722 }
723
S_energy(MyCircleOpt * opt,SUMA_MX_VEC * VecX,SUMA_GENERIC_ARGV_PARSE * ps)724 double S_energy( MyCircleOpt *opt, SUMA_MX_VEC *VecX , SUMA_GENERIC_ARGV_PARSE *ps) /* Be very careful! Order matters with index of VecX. */
725 {
726 static char FuncName[]={"S_energy"};
727 int j, i, m, i3;
728 double *dp_m = NULL, *dp_m1 = NULL, S_grad = 0.0, Xm_mag;
729 vector Xm, Xm_mid, Sm;
730 matrix *nullptr = NULL;
731 matrix Xm_t, Kern, KernI, A;
732
733 SUMA_Boolean LocalHead = NOPE;
734
735 SUMA_ENTRY;
736
737 if(LocalHead) {
738 fprintf(SUMA_STDERR, "%s: Control Point Locations:\n", FuncName);
739 for(m=0; m<opt->M_time_steps; ++m) {
740 for(j=0; j<opt->N_ctrl_points; ++j) {
741 dp_m = mxvdp3(VecX, 0, j, m);
742 fprintf(SUMA_STDERR, "%s: m=%d, i=%d, [%f; %f; %f];\n", FuncName, m, j, dp_m[0], dp_m[1], dp_m[2]);
743 dp_m = NULL;
744 }
745 }
746 }
747
748 for(m=0; m<opt->M_time_steps; ++m) {
749 vector_initialize(&Xm);
750 vector_create((3*opt->N_ctrl_points), &Xm);
751 vector_initialize(&Xm_mid);
752 vector_create((3*opt->N_ctrl_points), &Xm_mid);
753 matrix_initialize(&Xm_t);
754 matrix_create(1, (3*opt->N_ctrl_points), &Xm_t);
755 matrix_initialize(&Kern);
756 matrix_create((3*opt->N_ctrl_points), (3*opt->N_ctrl_points), &Kern);
757 matrix_initialize(&KernI);
758 matrix_create((3*opt->N_ctrl_points), (3*opt->N_ctrl_points), &KernI);
759 matrix_initialize(&A);
760 matrix_create(1,(3*opt->N_ctrl_points), &A);
761 vector_initialize(&Sm);
762 vector_create(1, &Sm);
763
764 for(j=0; j<3*opt->N_ctrl_points; ++j) {
765 Xm.elts[j] = 0.0;
766 Xm_mid.elts[j] = 0.0;
767 Xm_t.elts[0][j] = 0.0;
768 }
769
770 for(i=0; i<opt->N_ctrl_points; ++i) {
771 i3 = 3*i;
772 dp_m = mxvdp3(VecX, 0, i, m);
773 dp_m1 = mxvdp3(VecX, 0, i, (m+1));
774
775 Xm.elts[i3 ] = dp_m1[0] - dp_m[0];
776 Xm.elts[i3+1] = dp_m1[1] - dp_m[1];
777 Xm.elts[i3+2] = dp_m1[2] - dp_m[2];
778
779 Xm_t.elts[0][i3 ] = dp_m1[0] - dp_m[0];
780 Xm_t.elts[0][i3+1] = dp_m1[1] - dp_m[1];
781 Xm_t.elts[0][i3+2] = dp_m1[2] - dp_m[2];
782
783 /* This Xm_mid must be projected to the sphere!!! */
784 Xm_mid.elts[i3 ] = 0.5*(dp_m1[0] + dp_m[0]);
785 Xm_mid.elts[i3+1] = 0.5*(dp_m1[1] + dp_m[1]);
786 Xm_mid.elts[i3+2] = 0.5*(dp_m1[2] + dp_m[2]);
787
788 Xm_mag = sqrt(SUMA_POW2(Xm_mid.elts[i3 ]) + SUMA_POW2(Xm_mid.elts[i3+1]) + SUMA_POW2(Xm_mid.elts[i3+2]) );
789 if(Xm_mag) {
790 Xm_mid.elts[i3 ] = Xm_mid.elts[i3 ]/Xm_mag;
791 Xm_mid.elts[i3+1] = Xm_mid.elts[i3+1]/Xm_mag;
792 Xm_mid.elts[i3+2] = Xm_mid.elts[i3+2]/Xm_mag;
793 }
794 }
795
796 if(LocalHead) {
797 fprintf(SUMA_STDERR, "\n %s:\n", FuncName);
798 fprintf(SUMA_STDERR, "Xm.elts = [\n");
799 for(j=0; j<3*opt->N_ctrl_points; ++j) fprintf(SUMA_STDERR, "%f\n", Xm.elts[j]);
800 fprintf(SUMA_STDERR, "]\n");
801 fprintf(SUMA_STDERR, "Xm_mid.elts = [\n");
802 for(j=0; j<3*opt->N_ctrl_points; ++j) fprintf(SUMA_STDERR, "%f\n", Xm_mid.elts[j]);
803 fprintf(SUMA_STDERR, "]\n");
804 fprintf(SUMA_STDERR, "Xm_t.elts = [\n");
805 for(j=0; j<3*opt->N_ctrl_points; ++j) fprintf(SUMA_STDERR, "%f ", Xm_t.elts[0][j]);
806 fprintf(SUMA_STDERR, "]\n");
807 }
808
809 Rotation_Matrix( opt, Xm_mid, Kern );
810 matrix_psinv(Kern, nullptr, &KernI);
811
812 matrix_multiply(Xm_t, KernI, &A);
813 vector_multiply(A, Xm, &Sm);
814
815 #if 0
816 if (ps->cs->talk_suma) {
817 SUMA_SegmentDO *sdo=NULL;
818 NI_group *ngr = NULL;
819 int suc;
820 /* Send Perturbation vector to SUMA */
821 sdo = SUMA_Sm2SDO(opt->ControlCurve, Sm, "Sm", 6.0);
822 /* change that thing to NIML */
823 ngr = SUMA_SDO2niSDO(sdo);
824 #if 0
825 /* write it to diks, for kicks */
826 sprintf(stmp, "file:PV0_%d.niml.SDO", opt->iter_count);
827 NEL_WRITE_TX(ngr, stmp, suc);
828 #endif
829 /* send it to suma */
830 if (!SUMA_SendToSuma (opt->SO, ps->cs, (void *)ngr, SUMA_SEGMENT_OBJECT, 1)) {
831 SUMA_S_Warn("Failed in SUMA_SendToSuma\nCommunication halted.");
832 ps->cs->talk_suma = NOPE;
833 }
834
835 /* SUMA_free_SegmentDO(sdo); sdo = NULL;
836 NI_free_element(ngr); ngr = NULL;
837 if ( opt->pause > 1) {
838 SUMA_PAUSE_PROMPT("Pausing after Sm trial\nDo something to proceed.\n");
839 }*/
840 }
841 #endif
842
843 if(LocalHead) {
844 fprintf(SUMA_STDERR, "\n %s:\n", FuncName);
845 fprintf(SUMA_STDERR, "Kern = \n");
846 Print_Matrix(opt, Kern, NULL);
847 fprintf(SUMA_STDERR, "KernI = \n");
848 Print_Matrix(opt, KernI, NULL);
849 fprintf(SUMA_STDERR, "A = \n");
850 matrix_print(A);
851 fprintf(SUMA_STDERR, "Sm = %f\n", Sm.elts[0]);
852 }
853
854 S_grad = S_grad + Sm.elts[0];
855
856 if(LocalHead) fprintf(SUMA_STDERR, "%s: S_grad = %f\n", FuncName, S_grad);
857
858 vector_destroy(&Xm);
859 matrix_destroy(&Xm_t);
860 matrix_destroy(&Kern);
861 matrix_destroy(&KernI);
862 matrix_destroy(&A);
863 vector_destroy(&Sm);
864 vector_destroy(&Xm_mid);
865 }
866
867 SUMA_RETURN(S_grad);
868 }
869
SUMA_G2SDO(vector G,SUMA_MX_VEC * ControlCurve,char * Label,double scl)870 SUMA_SegmentDO *SUMA_G2SDO(vector G, SUMA_MX_VEC *ControlCurve,
871 char *Label, double scl)
872 {
873 static char FuncName[]={"SUMA_G2SDO"};
874 SUMA_SegmentDO *SDO=NULL;
875 int N_n, oriented, NodeBased, Stipple, i, m, d, kcc;
876 char *idcode_str=NULL, *Parent_idcode_str=NULL;
877 float LineWidth;
878 double *dp=NULL, *dp_new=NULL;
879 int *NodeId=NULL;
880 float *n0=NULL, *n1=NULL;
881 float *colv=NULL, *thickv=NULL;
882 float acol[4], LineCol[4] = { 1.000000, 0.300000, 1.000000, 1.000000 };
883 SUMA_Boolean LocalHead = NOPE;
884
885 SUMA_ENTRY;
886
887 oriented = 0;
888 Stipple = 0;
889 NodeBased = 0;
890 LineWidth = 4;
891 SUMA_NEW_ID(idcode_str, Label);
892
893 N_n = ControlCurve->dims[1] * (ControlCurve->dims[2]-2);
894 n0 = (float *) SUMA_malloc(sizeof(float)*N_n*3);
895 n1 = (float *) SUMA_malloc(sizeof(float)*N_n*3);
896 colv = (float *) SUMA_malloc(sizeof(float)*N_n*4);
897 thickv = (float *) SUMA_malloc(sizeof(float)*N_n);
898
899
900 for(i=0; i<ControlCurve->dims[1]; ++i) { /* ControlCurve->dims[1] =
901 opt->N_ctrl_points */
902 SUMA_a_good_col("ROI_i256", i+2, acol);/* get a decent colormap */
903 /* Add 2 to the color map index to avoid getting purple. */
904 for (m=0; m<ControlCurve->dims[2]-2; ++m) { /* ControlCurve->dims[2] =
905 opt->M_time_steps+1 */
906 kcc = m*ControlCurve->dims[1]+i;
907 if(1) {
908 dp = mxvdp3(ControlCurve, 0, i, m+1);
909 for (d=0;d<3;++d) {
910 n0[3*kcc+d] = dp[d];
911 n1[3*kcc+d] = dp[d]+scl * G.elts[3*kcc+d];
912 if (LocalHead)
913 fprintf( SUMA_STDERR,"%d/%d\n",
914 3*kcc+d,
915 3*ControlCurve->dims[1] * (ControlCurve->dims[2]-2));
916 }
917 }
918 for (d=0;d<4;++d) colv[4*kcc+d] = acol[d];
919 thickv[kcc] = 3;
920 dp = NULL; dp_new = NULL;
921 }
922 }
923
924 SDO = SUMA_CreateSegmentDO( N_n, oriented, NodeBased, Stipple,
925 Label, idcode_str, Parent_idcode_str,
926 NOT_SET_type, NULL,
927 LineWidth, LineCol,
928 NodeId, NULL, n0, n1,
929 colv, thickv);
930
931 if (idcode_str) SUMA_free(idcode_str); idcode_str = NULL;
932 if (n0) SUMA_free(n0); n0 = NULL;
933 if (n1) SUMA_free(n1); n1 = NULL;
934 if (colv) SUMA_free(colv); colv = NULL;
935 if (thickv) SUMA_free(thickv); thickv = NULL;
936 SUMA_RETURN(SDO);
937 }
938
Find_Lamda(MyCircleOpt * opt,SUMA_MX_VEC * ControlCurve,SUMA_MX_VEC * MaxStep,SUMA_MX_VEC * Perturb_Vec,SUMA_MX_VEC * Del_S,SUMA_MX_VEC * X_Lamda,SUMA_GENERIC_ARGV_PARSE * ps)939 double Find_Lamda( MyCircleOpt *opt, SUMA_MX_VEC *ControlCurve, SUMA_MX_VEC *MaxStep,
940 SUMA_MX_VEC *Perturb_Vec, SUMA_MX_VEC *Del_S, SUMA_MX_VEC *X_Lamda,
941 SUMA_GENERIC_ARGV_PARSE *ps)
942 {
943
944 static char FuncName[]={"Find_Lamda"};
945 int m, i, p, j, nr, nc, m3, m2, r, c, i3, repeat = 0, descent_small = 0;
946 int C_N_dims = 3;
947 int C_dims[10] = { opt->N_ctrl_points, (opt->M_time_steps - 1), 2}; /* time steps, p */
948 double *dp_m0 = NULL, *dp_m1 = NULL, *dp = NULL, *dp_LG = NULL;
949 double Sx, SxL, Lda = 0.0, mag_G, Theta_step, Theta_step_min, mag_LG;
950 char stmp[500];
951 vector Change, G;
952 matrix E, Et, EtE, EtEI, R;
953 matrix *nullptr = NULL;
954
955 SUMA_Boolean LocalHead = NOPE;
956
957 SUMA_ENTRY;
958
959 if (opt->dbg_flag > 2) LocalHead = YUP;
960
961 vector_initialize(&Change);
962 vector_create( (2*opt->N_ctrl_points*(opt->M_time_steps-1)), &Change );
963
964 /* Calculate Values for C vector. */
965 SUMA_MX_VEC *Change_S = NULL;
966 Change_S = SUMA_NewMxVec( SUMA_double, C_N_dims, C_dims, 1 );
967
968 for(p=0; p<2; ++p) {
969 for(m=1; m<(opt->M_time_steps); ++m) {
970 for(i=0; i<opt->N_ctrl_points; ++i) {
971 dp_m0 = mxvdp4(Del_S, i, m, 0, p);
972 dp_m1 = mxvdp4(Del_S, i, (m-1), 1, p);
973
974 mxvd3(Change_S, i, m-1, p) = dp_m0[0] + dp_m1[0];
975
976 if(LocalHead) { fprintf(SUMA_STDERR, "p=%d, m=%d, i=%d\n dp_m0 = %f, dp_m1 = %f\n C = %f\n",
977 p, m, i, dp_m0[0], dp_m1[0], mxvd3(Change_S, i, m-1, p)); }
978 dp_m0 = NULL; dp_m1 = NULL;
979 }
980 }
981 }
982 if(LocalHead) fprintf(SUMA_STDERR, "Show Change_S:\n");
983
984 if (opt->dbg_flag > 1) SUMA_ShowMxVec(Change_S, -1, NULL, "\nChange_S\n");
985
986 /* Fill vector C with the values. Index in order, m, i, p*/
987 /* BE CAREFUL, M=0 IS NOT THE ENDPOINT, IT IS ACTUALLY THE FIRST STEP WHICH WAS PREVIOUSLY CALLED M=1. */
988 j=0;
989 for(m=0; m<(opt->M_time_steps-1); ++m) {
990 for(i=0; i<opt->N_ctrl_points; ++i) {
991 for(p=0; p<2; ++p) {
992 Change.elts[j] = mxvd3(Change_S, i, m, p);
993 if(LocalHead) fprintf(SUMA_STDERR, "m=%d, i=%d, p=%d\n %f\n", m, i, p, Change.elts[j]);
994 ++j;
995 }
996 }
997 }
998
999 if(LocalHead) {
1000 fprintf(SUMA_STDERR, "Change:\n");
1001 for(j=0; j<(2*opt->N_ctrl_points*(opt->M_time_steps-1)); ++j) fprintf(SUMA_STDERR, "%f\n", Change.elts[j]);
1002 }
1003
1004 /* Form E matrices that store the perturbation vectors. */
1005 nr = 3*opt->N_ctrl_points*(opt->M_time_steps-1);
1006 fprintf(SUMA_STDERR,"nr = %d\n", nr);
1007 nc = 2*opt->N_ctrl_points*(opt->M_time_steps-1);
1008 matrix_initialize(&E);
1009 matrix_create(nr, nc, &E);
1010
1011 for(r=0; r<nr; ++r) { for(c=0; c<nc; ++c) { E.elts[r][c] = 0.00; } }
1012
1013 for(m=0; m<(opt->M_time_steps-1); ++m) {
1014 for(i=0; i<opt->N_ctrl_points; ++i) {
1015 for(p=0; p<2; ++p) {
1016 i3 = 3*i;
1017 m3 = 3*opt->N_ctrl_points*m;
1018 m2 = 2*opt->N_ctrl_points*m;
1019 dp = mxvdp4(Perturb_Vec, 0, i, (m+1), p);
1020
1021 E.elts[i3 + m3 ] [p + 2*i + m2] = dp[0];
1022 E.elts[i3 + m3 + 1] [p + 2*i + m2] = dp[1];
1023 E.elts[i3 + m3 + 2] [p + 2*i + m2] = dp[2];
1024
1025 dp = NULL;
1026 }
1027 }
1028 }
1029
1030 if(LocalHead) {
1031 fprintf(SUMA_STDERR, "E = [\n");
1032 matrix_print(E);
1033 fprintf(SUMA_STDERR, "];\n");
1034 }
1035
1036 matrix_initialize(&Et);
1037 matrix_create(nr, nc, &Et);
1038 matrix_initialize(&EtE);
1039 matrix_create(nr, nc, &EtE);
1040 matrix_initialize(&EtEI);
1041 matrix_create(nr, nc, &EtEI);
1042 matrix_initialize(&R);
1043 matrix_create(nr, nc, &R);
1044 vector_initialize(&G);
1045 vector_create(nr, &G);
1046
1047 matrix_transpose(E, &Et);
1048 matrix_multiply(Et, E, &EtE);
1049 matrix_psinv(EtE, nullptr, &EtEI);
1050 matrix_multiply(E, EtEI, &R); /* A is temporary. */
1051 vector_multiply(R, Change, &G); /* G for gradient. */
1052
1053 if(LocalHead) {
1054 fprintf(SUMA_STDERR, "Et = [\n");
1055 matrix_print(Et);
1056 fprintf(SUMA_STDERR, "];\n");
1057 fprintf(SUMA_STDERR, "EtE = [\n");
1058 matrix_print(EtE);
1059 fprintf(SUMA_STDERR, "];\n");
1060 fprintf(SUMA_STDERR, "EtEI = [\n");
1061 matrix_print(EtEI);
1062 fprintf(SUMA_STDERR, "];\n");
1063 fprintf(SUMA_STDERR, "R = [\n");
1064 matrix_print(R);
1065 fprintf(SUMA_STDERR, "];\n");
1066 }
1067
1068 if(LocalHead) {
1069 fprintf(SUMA_STDERR, "G = [\n");
1070 for(j=0; j<nr; ++j) fprintf(SUMA_STDERR, "%f\n", G.elts[j]);
1071 fprintf(SUMA_STDERR, "];\n");
1072 }
1073
1074 /* Need to make G be just the direction vectors. Want unit vectors! */
1075 for(i=0; i< (opt->N_ctrl_points*(opt->M_time_steps-1)); ++i) {
1076 i3 = 3*i;
1077 mag_G = 0.0;
1078 mag_G = sqrt( SUMA_POW2(G.elts[i3 ]) +
1079 SUMA_POW2(G.elts[i3+1]) +
1080 SUMA_POW2(G.elts[i3+2]) );
1081
1082 G.elts[i3 ] = G.elts[i3 ]/mag_G;
1083 G.elts[i3+1] = G.elts[i3+1]/mag_G;
1084 G.elts[i3+2] = G.elts[i3+2]/mag_G;
1085 }
1086
1087 if (ps->cs->talk_suma) {
1088 SUMA_SegmentDO *sdo=NULL;
1089 NI_group *ngr = NULL;
1090 int suc;
1091 /* Send G to SUMA */
1092 sdo = SUMA_G2SDO(G, ControlCurve, "G", -0.1); /* Set scaler as negative so can visualize greatest decent. */
1093 /* change that thing to NIML */
1094 ngr = SUMA_SDO2niSDO(sdo);
1095 #if 0
1096 /* write it to diks, for kicks */
1097 sprintf(stmp, "file:G_%d.niml.SDO", opt->iter_count);
1098 NEL_WRITE_TX(ngr, stmp, suc);
1099 #endif
1100 /* send it to suma */
1101 if (!SUMA_SendToSuma (opt->SO, ps->cs, (void *)ngr, SUMA_SEGMENT_OBJECT, 1)) {
1102 SUMA_S_Warn("Failed in SUMA_SendToSuma\nCommunication halted.");
1103 ps->cs->talk_suma = NOPE;
1104 }
1105 SUMA_free_SegmentDO(sdo); sdo = NULL;
1106 NI_free_element(ngr); ngr = NULL;
1107 }
1108
1109 if(LocalHead){
1110 fprintf(SUMA_STDERR, "G = [\n");
1111 for(j=0; j<nr; ++j) fprintf(SUMA_STDERR, "%f\n", G.elts[j]);
1112 fprintf(SUMA_STDERR, "];\n");
1113 }
1114
1115 /* Find energy of sphere using unadjusted path. */
1116 Sx = S_energy(opt, ControlCurve, ps);
1117
1118 /* Find smallest "maximum allowable step" so know how big lamda can be. */
1119 if(opt->dbg_flag > 1) fprintf(SUMA_STDERR, "*************THETA_STEP_MIN:\n");
1120 Theta_step_min = mxvd2(MaxStep, 0, 1);
1121 if(LocalHead) fprintf(SUMA_STDERR, "%f\n", Theta_step_min);
1122 for(m=1; (m<opt->M_time_steps); ++m) {
1123 for(i=0; i<opt->N_ctrl_points; ++i) {
1124
1125 Theta_step = mxvd2(MaxStep, i, m);
1126 if(LocalHead) fprintf(SUMA_STDERR, "%f\n", Theta_step);
1127 if(Theta_step < Theta_step_min) Theta_step_min = Theta_step;
1128 }
1129 }
1130 if(opt->dbg_flag > 1) fprintf(SUMA_STDERR, "Final Theta_step_min: %f\n", Theta_step_min);
1131
1132 /* Set the smallest "max allowable movement", Theta_step_min as the starting point for Lda. */
1133 /* Now search for the largest Lda that gives lower energy than with an Lda of zero. */
1134
1135 if(LocalHead) fprintf(SUMA_STDERR, "Final Theta_step_min: %f\n", Theta_step_min);
1136
1137 Lda = Theta_step_min/1.0;
1138
1139 do{
1140
1141 /* Form list of path points with adjustment. Perturbation magnitude, lamda, is what we are looking for. */
1142 for(m=0; (m<opt->M_time_steps+1); ++m) {
1143 for(i=0; i<opt->N_ctrl_points; ++i) {
1144 i3 = 3*i;
1145 m3 = 3*opt->N_ctrl_points*(m-1);
1146 dp = NULL;
1147 dp = mxvdp3(ControlCurve, 0, i, m);
1148
1149 if(m<1) {
1150 mxvd3(X_Lamda, 0, i, m) = dp[0];
1151 mxvd3(X_Lamda, 1, i, m) = dp[1];
1152 mxvd3(X_Lamda, 2, i, m) = dp[2];
1153 } else if(m == opt->M_time_steps){
1154 mxvd3(X_Lamda, 0, i, m) = dp[0];
1155 mxvd3(X_Lamda, 1, i, m) = dp[1];
1156 mxvd3(X_Lamda, 2, i, m) = dp[2];
1157 } else {
1158 if(opt->dbg_flag > 1 && Lda == Theta_step_min) {
1159 fprintf(SUMA_STDERR, "ControlCurve(%d, %d) = [%f %f %f];\n", m, i, dp[0], dp[1], dp[2]);
1160 fprintf(SUMA_STDERR, "G.elts(%d, %d, %d) = [%f %f %f];\n", (i3 + m3), (i3 + m3 +1), (i3 + m3 +2),
1161 G.elts[i3 + m3 ], G.elts[i3 + m3 + 1], G.elts[i3 + m3 + 2]);
1162 }
1163 /* This must be projected on to the sphere! */
1164 mxvd3(X_Lamda, 0, i, m) = dp[0] - Lda*G.elts[i3 + m3 ];
1165 mxvd3(X_Lamda, 1, i, m) = dp[1] - Lda*G.elts[i3 + m3 + 1];
1166 mxvd3(X_Lamda, 2, i, m) = dp[2] - Lda*G.elts[i3 + m3 + 2];
1167
1168 dp_LG = mxvdp3(X_Lamda, 0, i, m);
1169 mag_LG = sqrt(SUMA_POW2(dp_LG[0]) + SUMA_POW2(dp_LG[1]) + SUMA_POW2(dp_LG[2]));
1170 dp_LG[0] = dp_LG[0]/mag_LG;
1171 dp_LG[1] = dp_LG[1]/mag_LG;
1172 dp_LG[2] = dp_LG[2]/mag_LG;
1173 dp_LG = NULL;
1174 }
1175 dp = NULL;
1176 }
1177 }
1178
1179 /*if(LocalHead) {
1180 fprintf(SUMA_STDERR, "Show X_Lamda:\n");
1181 SUMA_ShowMxVec(X_Lamda, -1, NULL, "\nkkgjjg\n");
1182 }*/
1183
1184 if(repeat<1) {
1185 if(opt->dbg_flag > 1) fprintf(SUMA_STDERR, "Lda before comparison: %f\n", Lda);
1186 /* Find energy of sphere with adjusted path. */
1187 SxL = 0.0;
1188 SxL = S_energy(opt, X_Lamda, ps);
1189 if( SxL < Sx ) repeat = 1;
1190 if( SxL >= Sx ) { /* IMPROVEMENT_NOTE: Perhaps start from a better Lda, looks like we have to go down often. */
1191 /* if(Lda>0.02) Lda = Lda - 0.005;
1192 if(Lda<=0.02) Lda = Lda - 0.0001; */
1193 Lda = Lda - Theta_step_min * 0.02;
1194 if(Lda<0.0001) { Lda = 0.0; repeat = 1; }
1195 }
1196 if(opt->dbg_flag > 1) fprintf(SUMA_STDERR, "SxL = %.12f, Sx = %.12f, Lda = %f\n", SxL, Sx, Lda);
1197 }
1198 } while(repeat < 1);
1199
1200
1201 Change_S = SUMA_FreeMxVec( Change_S );
1202 vector_destroy(&Change);
1203 matrix_destroy(&E);
1204 matrix_destroy(&Et);
1205 matrix_destroy(&EtE);
1206 matrix_destroy(&EtEI);
1207 matrix_destroy(&R);
1208 vector_destroy(&G);
1209
1210 SUMA_RETURN(Lda);
1211 }
1212
1213 /*********************************************** Begin Mesh walking functions here ************************************************/
1214
Debug_Weights(MyCircle * C,MyCircleOpt * opt,matrix M,matrix Mi,vector Vv)1215 SUMA_Boolean Debug_Weights( MyCircle *C, MyCircleOpt *opt, matrix M, matrix Mi, vector Vv)
1216 {
1217 static char FuncName[]={"Debug_Weights"};
1218 int i, i3, j, j3, r, k, c, idm;
1219
1220 SUMA_Boolean LocalHead = NOPE;
1221
1222 SUMA_ENTRY;
1223
1224 FILE *output_matrix = NULL; /*for sending the matrix to a file to be read by matlab. */
1225 output_matrix = fopen( "output_matrix.m", "w" );
1226 /* Print Matrix */
1227 if (opt->dot) {
1228 fprintf(SUMA_STDERR, "%s:\n"
1229 "M = [\n", FuncName);
1230 for (r=0; r<opt->N_ctrl_points; ++r) {
1231 for (k=0; k<(3+opt->N_ctrl_points); ++k) {
1232 for(c=0; c<opt->N_ctrl_points; ++c) {
1233 fprintf (SUMA_STDERR,"%.8f %.8f %.8f ",
1234 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c ) ],
1235 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c+1) ],
1236 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c+2) ]);
1237 }
1238 fprintf (SUMA_STDERR,"\n");
1239 }
1240 }
1241 fprintf(SUMA_STDERR, "];\n");
1242 } else {
1243 fprintf(SUMA_STDERR, "%s:\n"
1244 "M = [\n", FuncName);
1245 for (r=0; r<opt->N_ctrl_points; ++r) {
1246 for (k=0; k<3; ++k) {
1247 for(c=0; c<opt->N_ctrl_points; ++c) {
1248 fprintf (SUMA_STDERR,"%.5f %.5f %.5f ",
1249 M.elts [ (3*r+k) ][ (3*c ) ],
1250 M.elts [ (3*r+k) ][ (3*c+1) ],
1251 M.elts [ (3*r+k) ][ (3*c+2) ]);
1252 }
1253 fprintf (SUMA_STDERR,"\n");
1254 }
1255 }
1256 fprintf(SUMA_STDERR, "];\n");
1257 }
1258
1259 /* Write matrix to a file so can be read by matlab. */
1260 if (opt->dot) {
1261 fprintf(output_matrix, "M = [\n");
1262 for (r=0; r<opt->N_ctrl_points; ++r) {
1263 for (k=0; k<(3+opt->N_ctrl_points); ++k) {
1264 for (c=0; c<opt->N_ctrl_points; ++c) {
1265 fprintf (output_matrix,"%11.8f %11.8f %11.8f ",
1266 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c ) ],
1267 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c+1) ],
1268 M.elts [ ((3+opt->N_ctrl_points) * r+k) ][ (3*c+2) ]);
1269 }
1270 fprintf (output_matrix,"\n");
1271 } }
1272 fprintf(output_matrix, "]; \n \n");
1273 } else {
1274 fprintf(output_matrix, "%s:\n"
1275 "M = [\n", FuncName);
1276 for (r=0; r<opt->N_ctrl_points; ++r) {
1277 for (k=0; k<3; ++k) {
1278 for(c=0; c<opt->N_ctrl_points; ++c) {
1279 fprintf (output_matrix,"%11.8f %11.8f %11.8f ",
1280 M.elts [ (3*r+k) ][ (3*c ) ],
1281 M.elts [ (3*r+k) ][ (3*c+1) ],
1282 M.elts [ (3*r+k) ][ (3*c+2) ]);
1283 }
1284 fprintf (output_matrix,"\n");
1285 } }
1286 fprintf(output_matrix, "]; \n \n");
1287 }
1288
1289
1290 /*Print the Inverse Coefficient Matrix.*/
1291 if(opt->dot) {
1292 fprintf(SUMA_STDERR, "%s:\n" "Mi = [\n", FuncName);
1293 for (r=0; r<opt->N_ctrl_points; ++r) {
1294 for (k=0; k<3; ++k) {
1295 for(c=0; c<opt->N_ctrl_points; ++c) {
1296 fprintf (SUMA_STDERR,"%.5f %.5f %.5f %.5f ",
1297 Mi.elts [ (3*r+k) ][ (4*c ) ],
1298 Mi.elts [ (3*r+k) ][ (4*c+1) ],
1299 Mi.elts [ (3*r+k) ][ (4*c+2) ],
1300 Mi.elts [ (3*r+k) ][ (4*c+3) ]);
1301 }
1302 fprintf (SUMA_STDERR,"\n");
1303 }
1304 }
1305 fprintf(SUMA_STDERR, "];\n");
1306 } else {
1307 fprintf(SUMA_STDERR, "%s:\n" "Mi = [\n", FuncName);
1308 for (r=0; r<opt->N_ctrl_points; ++r) {
1309 for (k=0; k<3; ++k) {
1310 for(c=0; c<opt->N_ctrl_points; ++c) {
1311 fprintf (SUMA_STDERR,"%.5f %.5f %.5f ",
1312 Mi.elts [ (3*r+k) ][ (3*c ) ],
1313 Mi.elts [ (3*r+k) ][ (3*c+1) ],
1314 Mi.elts [ (3*r+k) ][ (3*c+2) ]);
1315 }
1316 fprintf (SUMA_STDERR,"\n");
1317 }
1318 }
1319 fprintf(SUMA_STDERR, "];\n");
1320 }
1321
1322 /* Print velocity vectors. */
1323 fprintf(SUMA_STDERR, "%s:\n"
1324 "V = [ \n", FuncName );
1325 for (i=0; i < opt->N_ctrl_points; ++i) {
1326 if(opt->dot) {
1327 idm = (opt->dim+opt->N_ctrl_points)*i;
1328 fprintf(SUMA_STDERR, " %10.8f; %10.8f; %10.8f; ",
1329 Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2] );
1330 for(k=0; k < opt->N_ctrl_points; ++k) { fprintf(SUMA_STDERR, " 0.0000;" ); }
1331 fprintf(SUMA_STDERR, "\n" );
1332 } else {
1333 idm = opt->dim*i;
1334 fprintf(SUMA_STDERR, " %10.8f; %10.8f; %10.8f \n",
1335 Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2]);
1336 }
1337 }
1338 fprintf(SUMA_STDERR, "];\n \n" );
1339
1340 /* Send velocity vector to output file for checking math in matlab. */
1341 fprintf(output_matrix, "V = [ \n" );
1342 for (i=0; i < opt->N_ctrl_points; ++i) {
1343 if(opt->dot) {
1344 idm = (opt->dim+opt->N_ctrl_points)*i;
1345 fprintf(output_matrix, " %11.20f; %11.20f; %11.20f; ",
1346 Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2] );
1347 for(k=0; k < opt->N_ctrl_points; ++k) { fprintf(output_matrix, " 0.0000;" ); }
1348 fprintf(output_matrix, "\n" );
1349 } else {
1350 idm = opt->dim*i;
1351 fprintf(output_matrix, " %11.20f; %11.20f; %11.20f \n",
1352 Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2]);
1353 }
1354 }
1355 fprintf(output_matrix, "];\n \n" );
1356
1357 /* Print Weights. */
1358 fprintf(SUMA_STDERR, "%s:\n"
1359 "Wv = [\n", FuncName);
1360 for (r=0; r<opt->N_ctrl_points; ++r) {
1361 if (opt->dim == 3) {
1362 fprintf (SUMA_STDERR,"%.20f; %.20f; %.20f; \n",
1363 C->Wv.elts[3*r], C->Wv.elts[3*r+1], C->Wv.elts[3*r+2]);
1364 }else {
1365 fprintf (SUMA_STDERR,"%.20f; %.20f; \n", C->Wv.elts[2*r], C->Wv.elts[2*r+1]);
1366 }
1367 }
1368 fprintf (SUMA_STDERR,"];\n");
1369 for (i=0; i<opt->N_ctrl_points; ++i) {
1370 fprintf (SUMA_STDERR,"Dot_Wv (%d) = %.24f \n"
1371 " W(%d) = [%f; %f; %f]\n"
1372 " Control Point(%d) = [%f; %f; %f]\n",
1373 i, SUMA_MT_DOT( (&(opt->CtrlPts[3*i])), (&(C->Wv.elts[3*i])) ),
1374 i, C->Wv.elts[3*i], C->Wv.elts[3*i+1], C->Wv.elts[3*i+2],
1375 i, opt->CtrlPts[3*i], opt->CtrlPts[3*i+1], opt->CtrlPts[3*i+2]);
1376 }
1377
1378 /* Also send weights to output file for use in matlab. */
1379 fprintf(output_matrix, "Wv = [\n" );
1380 for (r=0; r<opt->N_ctrl_points; ++r) {
1381 if (opt->dim == 3) {
1382 fprintf (output_matrix," %11.20f; %11.20f; %11.20f; \n",
1383 C->Wv.elts[3*r], C->Wv.elts[3*r+1], C->Wv.elts[3*r+2]);
1384 }else {
1385 fprintf (output_matrix," %11.20f; %11.20f; \n", C->Wv.elts[2*r], C->Wv.elts[2*r+1]);
1386 }
1387 }
1388 fprintf (output_matrix,"];\n \n");
1389
1390 fclose (output_matrix); output_matrix = NULL;
1391
1392 SUMA_RETURN(YUP);
1393 }
1394
1395
FindSplineWeights(MyCircle * C,MyCircleOpt * opt,FILE * condition_num,FILE * condition_num_only)1396 SUMA_Boolean FindSplineWeights (MyCircle *C, MyCircleOpt *opt, FILE *condition_num, FILE *condition_num_only)
1397 {
1398 static char FuncName[]={"FindSplineWeights"};
1399 vector Vv;
1400 static matrix M, Mi;
1401 double Mcr[3][3], nrm_cr[3] = {0.0, 0.0, 0.0};
1402 double check_a, check_cr[3] = {0.0, 0.0, 0.0}, tan_v_mag;
1403 int i, idm=0.0, k, r, c, nr, nc, i3, c3, r3, r4, f, row;
1404 double V_row1[3], V_row2[3], V_row3[3], nrm[3]={0.0,0.0,0.0};
1405 double *t=NULL;
1406 double t_rc = 0.0, nrm_rc[3]={0.0,0.0,0.0};
1407 double tan_v[3]={0.0,0.0,0.0}; /* Meaning tangent velocity vector. Stores cross product when calculating given velocity. */
1408 double Vv_mag = 0.0, t_cr, expand_cr = 0.0;
1409 matrix *nullptr = NULL;
1410 double I[3][3], sI[3][3], cond;
1411
1412 SUMA_Boolean LocalHead = NOPE;
1413
1414 SUMA_ENTRY;
1415
1416 if (LocalHead) {
1417 fprintf(SUMA_STDERR, "%s:\n"
1418 "Number of control points: %d\n",
1419 FuncName, opt->N_ctrl_points);
1420 }
1421
1422 if (opt->dim !=2 && opt->dim != 3) {
1423 SUMA_S_Err("Stupid");
1424 SUMA_RETURN(NOPE);
1425 }
1426
1427 t = (double *)SUMA_calloc(opt->N_ctrl_points, sizeof(double));
1428
1429 if (opt->Dtheta) {
1430 SUMA_S_Err("Expecting no Dtheta yet!");
1431 SUMA_RETURN(NOPE);
1432 } else {
1433 opt->Dtheta = (double *)SUMA_calloc(opt->N_ctrl_points , sizeof (double)); /* angular displacement between t = 0 and t = 1 for each node.
1434 One does not need to store for the algorithm but it makes
1435 debugging easier. */
1436 }
1437
1438 opt->Nrm = (double *)SUMA_calloc(opt->N_ctrl_points * 3 , sizeof (double));
1439
1440 /* CALCULATE GIVEN VELOCITY AT CONTROL POINTS USING DESIRED ANGLE AND AXIS OF ROTATION. */
1441 vector_initialize (&Vv);
1442 if (opt->dot) { vector_create ((opt->dim*opt->N_ctrl_points + opt->N_ctrl_points*opt->N_ctrl_points), &Vv); }
1443 else { vector_create (opt->dim*opt->N_ctrl_points, &Vv); }
1444
1445 for (i=0; i < opt->N_ctrl_points; ++i) {
1446 if(opt->dot) { idm = (opt->dim + opt->N_ctrl_points)*i; }
1447 else { idm = opt->dim*i; }
1448 i3 = 3*i;
1449 /* Distance between control point and desired destination of control point */
1450 /* nrm is the axis of rotation from intial control point location to final. */
1451 /* BEWARE: USING _NC HERE, MEANING NO CENTER. WILL THE CENTER ALWAYS BE THE ORIGIN? */
1452 SUMA_ANGLE_DIST_NC((&(opt->CtrlPts_f[i3])), (&(opt->CtrlPts[i3])), opt->Dtheta[i], (&(opt->Nrm[i3])) );
1453
1454 if (LocalHead) {
1455 fprintf(SUMA_STDERR, "%s: Point %d, Dtheta = %.12f rad (%.12f deg)\n",
1456 FuncName, i, opt->Dtheta[i], SUMA_R2D(opt->Dtheta[i]));
1457 }
1458
1459 SUMA_MT_CROSS(tan_v, (&(opt->Nrm[i3])), (&(opt->CtrlPts[i3])) );
1460
1461 /* Need to normalize because using the direction (unit vector) of this tangent vector in the velocity calculation. */
1462 tan_v_mag = sqrt( tan_v[0]*tan_v[0] + tan_v[1]*tan_v[1] + tan_v[2]*tan_v[2] );
1463 if ( tan_v_mag > opt->Zero) {
1464 tan_v[0] = tan_v[0]/tan_v_mag;
1465 tan_v[1] = tan_v[1]/tan_v_mag;
1466 tan_v[2] = tan_v[2]/tan_v_mag; }
1467
1468 /*if (LocalHead) {
1469 fprintf(SUMA_STDERR, " TAN_V normalized = [ %.12f; %.12f; %.12f ]\n"
1470 " Dot product( tan_v, control point) = %.12f\n"
1471 " Dtheta = %f\n",
1472 tan_v[0], tan_v[1], tan_v[2],
1473 SUMA_MT_DOT(tan_v, (&(opt->CtrlPts[i3])) ),
1474 opt->Dtheta[i] ); } */
1475
1476 Vv.elts[idm ] = opt->Dtheta[i]*tan_v[0];
1477 Vv.elts[idm+1] = opt->Dtheta[i]*tan_v[1];
1478 if (opt->dim > 2) { Vv.elts[idm+2] = opt->Dtheta[i]*tan_v[2]; }
1479
1480 /* Dot Product Restriction. Since want tangent velocity contributions, need dot(radius, weight) = 0. */
1481 /* Need row with a zero for each control point. */
1482 if( opt->dot ) {
1483 for(k=3; k < (opt->dim + opt->N_ctrl_points); ++k ) { Vv.elts[idm+k] = 0.00000; } }
1484 }
1485
1486 if( LocalHead ) {
1487 fprintf(SUMA_STDERR, "Given Velocity Vectors for control points:\n");
1488 for (i=0; i < opt->N_ctrl_points; ++i) {
1489 if (opt->dim == 2) {
1490 fprintf(SUMA_STDERR, " %.18f %.18f \n",
1491 Vv.elts[idm ], Vv.elts[idm+1]);
1492 } else {
1493 if(opt->dot ) {
1494 idm = (opt->dim + opt->N_ctrl_points)*i;
1495 fprintf(SUMA_STDERR, " V(%d) = [ %f; %f; %f; %f; %f ] \n",
1496 i, Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2], Vv.elts[idm+3], Vv.elts[idm+4] );
1497 } else {
1498 idm = opt->dim*i;
1499 fprintf(SUMA_STDERR, " V(%d) = [ %f; %f; %f ] \n",
1500 i, Vv.elts[idm ], Vv.elts[idm+1], Vv.elts[idm+2] );
1501 }
1502 }
1503 }
1504 }
1505
1506 if( LocalHead ) {
1507 fprintf(SUMA_STDERR, "Dot product with radius:\n");
1508 for (i=0; i < opt->N_ctrl_points; ++i) {
1509 if(opt->dot ) { idm = (opt->dim + opt->N_ctrl_points)*i; }
1510 else { idm = opt->dim*i; }
1511 fprintf(SUMA_STDERR, " Dot(%d) = %.12f\n", i, SUMA_MT_DOT( (&(Vv.elts[idm])), (&(C->NewNodeList[ 3*opt->CtrlPts_iim[i]]))) );
1512 }
1513 }
1514
1515 if( opt->dot ) { nr = opt->N_ctrl_points * (opt->dim + opt->N_ctrl_points); } /* Need extra rows for dot product. */
1516 else { nr = opt->dim * opt->N_ctrl_points; } /* number of rows */
1517 nc = opt->dim * opt->N_ctrl_points; /* number of columns */
1518 matrix_initialize(&M);
1519 matrix_create(nr, nc, &M);
1520
1521 if (!M.elts) {
1522 SUMA_S_Crit("Failed to allocate");
1523 exit(1);
1524 }
1525
1526 if( !opt->dot ) {
1527 for(r=0; r<opt->N_ctrl_points; ++r) { /* r for row. For the first three rows, r represents the first control point. */
1528 for(c=0; c<opt->N_ctrl_points; ++c) { /* c for column. While r is held constant, c cycles through all control points
1529 so that for the first 3 rows (first 3 C.P.) are compared to all the rest. */
1530 c3 = 3*c; r3 = 3*r;
1531
1532 t_cr = 0.0; expand_cr = 0.0;
1533 /* Create the rotation matrix. */
1534 SUMA_3D_Rotation_Matrix( (&(opt->CtrlPts[c3])), (&(opt->CtrlPts[r3])), Mcr, t_cr, nrm_cr);
1535
1536 expand_cr = Deformation_Kernel(opt, t_cr);
1537
1538 /* Assemble the matrix and include the expansion factor. */
1539 M.elts[ (r3 ) ][ (c3 ) ] = expand_cr * Mcr[0][0];
1540 M.elts[ (r3 ) ][ (c3+1) ] = expand_cr * Mcr[0][1];
1541 M.elts[ (r3 ) ][ (c3+2) ] = expand_cr * Mcr[0][2];
1542 M.elts[ (r3+1) ][ (c3 ) ] = expand_cr * Mcr[1][0];
1543 M.elts[ (r3+1) ][ (c3+1) ] = expand_cr * Mcr[1][1];
1544 M.elts[ (r3+1) ][ (c3+2) ] = expand_cr * Mcr[1][2];
1545 M.elts[ (r3+2) ][ (c3 ) ] = expand_cr * Mcr[2][0];
1546 M.elts[ (r3+2) ][ (c3+1) ] = expand_cr * Mcr[2][1];
1547 M.elts[ (r3+2) ][ (c3+2) ] = expand_cr * Mcr[2][2];
1548 }
1549 }
1550 }
1551
1552 if( opt->dot ) {
1553 for(r=0; r<opt->N_ctrl_points; ++r) { /* r for row. For the first three rows, r represents the first control point.*/
1554 for(c=0; c<opt->N_ctrl_points; ++c) { /* c for column. While r is held constant, c cycles through all control points
1555 so that for the first 3 rows (first 3 C.P.) are compared to all the rest. */
1556 row = (3 + opt->N_ctrl_points)*r; /* Need extra rows for dot product restriction. */
1557 c3 = 3*c; r3 = 3*r;
1558
1559 t_cr = 0.0; expand_cr = 0.0;
1560 /* Create the rotation matrix. */
1561 SUMA_3D_Rotation_Matrix( (&(opt->CtrlPts[c3])), (&(opt->CtrlPts[r3])), Mcr, t_cr, nrm_cr );
1562
1563 expand_cr = Deformation_Kernel(opt, t_cr);
1564
1565 /* Assemble the matrix and include the expansion factor. */
1566 #if 0
1567 if(LocalHead) {
1568 if( r == 0 ) {
1569 fprintf(SUMA_STDERR, "ROTATION MATRIX: \n"
1570 " p1 = [ %f %f %f ] \n"
1571 " p2 = [ %f %f %f ] \n"
1572 " alpha(%d, %d) = %.12f \n"
1573 " u (%d, %d) = [ %.12f %.12f %.12f ] \n"
1574 " expansion = %f \n",
1575 opt->CtrlPts[c3 ], opt->CtrlPts[c3+1], opt->CtrlPts[c3+2],
1576 opt->CtrlPts[r3 ], opt->CtrlPts[r3+1], opt->CtrlPts[r3+2],
1577 r, c, t_cr,
1578 r, c, nrm_cr[0], nrm_cr[1], nrm_cr[2],
1579 expand_cr ); } }
1580 #endif
1581
1582 M.elts[ (row ) ][ (c3 ) ] = expand_cr * Mcr[0][0];
1583 M.elts[ (row ) ][ (c3+1) ] = expand_cr * Mcr[0][1];
1584 M.elts[ (row ) ][ (c3+2) ] = expand_cr * Mcr[0][2];
1585 M.elts[ (row+1) ][ (c3 ) ] = expand_cr * Mcr[1][0];
1586 M.elts[ (row+1) ][ (c3+1) ] = expand_cr * Mcr[1][1];
1587 M.elts[ (row+1) ][ (c3+2) ] = expand_cr * Mcr[1][2];
1588 M.elts[ (row+2) ][ (c3 ) ] = expand_cr * Mcr[2][0];
1589 M.elts[ (row+2) ][ (c3+1) ] = expand_cr * Mcr[2][1];
1590 M.elts[ (row+2) ][ (c3+2) ] = expand_cr * Mcr[2][2];
1591
1592 for(k=3; k < (3 + opt->N_ctrl_points); ++k) {
1593 if( k-3 == c && k-c == 3) {
1594 M.elts[ (row+k) ][ (c3 ) ] = (opt->CtrlPts[r3 ] * (expand_cr * Mcr[0][0]) )
1595 + (opt->CtrlPts[r3+1] * (expand_cr * Mcr[1][0]) )
1596 + (opt->CtrlPts[r3+2] * (expand_cr * Mcr[2][0]) );
1597 M.elts[ (row+k) ][ (c3+1) ] = (opt->CtrlPts[r3 ] * (expand_cr * Mcr[0][1]) )
1598 + (opt->CtrlPts[r3+1] * (expand_cr * Mcr[1][1]) )
1599 + (opt->CtrlPts[r3+2] * (expand_cr * Mcr[2][1]) );
1600 M.elts[ (row+k) ][ (c3+2) ] = (opt->CtrlPts[r3 ] * (expand_cr * Mcr[0][2]) )
1601 + (opt->CtrlPts[r3+1] * (expand_cr * Mcr[1][2]) )
1602 + (opt->CtrlPts[r3+2] * (expand_cr * Mcr[2][2]) ); }
1603 else {
1604 M.elts[ (row+k) ][ (c3 ) ] = 0.0;
1605 M.elts[ (row+k) ][ (c3+1) ] = 0.0;
1606 M.elts[ (row+k) ][ (c3+2) ] = 0.0; }
1607 }
1608 }
1609 }
1610 }
1611
1612 /* Check condition number for matrix M. Send to file for plotting in Matlab. */
1613 fprintf(condition_num, "M = ");
1614 Print_Matrix(opt, M, condition_num);
1615 cond = Matrix_Condition_Num( M, condition_num );
1616 fprintf(condition_num, "%f\n", cond);
1617 fprintf(condition_num_only, "%f\n", cond);
1618
1619 SUMA_LH("Calculating inverse...");
1620 matrix_initialize(&Mi);
1621 matrix_create(nr, nc, &Mi);
1622 matrix_psinv (M, nullptr, &Mi);
1623 SUMA_LH(" Done.");
1624
1625 /* if( LocalHead ) Debug_Weights(C, opt, M, Mi, Vv); */
1626 if( LocalHead) fprintf(SUMA_STDERR, "%s: Done printing inverse matrix.\n", FuncName);
1627
1628 SUMA_LH("Calculating weights...\n");
1629 vector_initialize(&(C->Wv));
1630 vector_multiply(Mi, Vv, &(C->Wv));
1631 SUMA_LH(" Done.");
1632
1633 if( LocalHead ) Debug_Weights(C, opt, M, Mi, Vv);
1634
1635 if (t) SUMA_free(t); t=NULL;
1636 matrix_destroy (&M);
1637 matrix_destroy (&Mi);
1638 vector_destroy (&Vv);
1639
1640 opt->Dtheta = NULL;
1641
1642 SUMA_RETURN(YUP);
1643 }
1644
Velocity(MyCircle * C,MyCircleOpt * opt)1645 SUMA_Boolean Velocity( MyCircle *C, MyCircleOpt *opt)
1646 {
1647 static char FuncName[]={"Velocity"};
1648 char outfile_test_expansion[50];
1649 byte repeat;
1650 int i, i3, j, j3, r;
1651 double v_alpha = 0.0, v_cr[3]={0.0,0.0,0.0}, v_expand, cr_mag;
1652 vector Wr; /*W rotated*/
1653 double v_M[3][3];
1654
1655 SUMA_Boolean LocalHead = NOPE;
1656
1657 SUMA_ENTRY;
1658
1659 /* Compute velocity at each node using contributions from the control points. */
1660 vector_initialize (&Wr);
1661 vector_create (3*opt->N_ctrl_points, &Wr);
1662
1663 if (LocalHead) {
1664 fprintf(SUMA_STDERR, "******************************************************\n"
1665 "%s: CHECKING THE FUNCTION, First control point: \n", FuncName); }
1666
1667 FILE *test_expansion = NULL;
1668 sprintf(outfile_test_expansion, "test_expansion_factor_0.1D");
1669 test_expansion = fopen (outfile_test_expansion, "w");
1670
1671 for (i=0; i< C->N_Node; ++i) { /* i for all points, j for the control points. */
1672 i3 = 3*i;
1673
1674 for (j=0; j<opt->N_ctrl_points; ++j){
1675 j3 = 3*j;
1676
1677 #if 0
1678 /* Need to comment out when using SUMA_3D_Rotation_Matrix to calculate initial velocity field. */
1679 SUMA_ANGLE_DIST_NC( (&(C->NewNodeList[i3])), (&(opt->CtrlPts[j3])), v_alpha, v_cr );
1680
1681 cr_mag = sqrt( v_cr[0]*v_cr[0] + v_cr[1]*v_cr[1] + v_cr[2]*v_cr[2]);
1682 if (LocalHead) {
1683 if( i == opt->CtrlPts_iim[0] ) { fprintf(SUMA_STDERR, "Magnitude of Cross Product = %.12f \n", cr_mag); }}
1684
1685 if(cr_mag > opt->Zero){ v_cr[0] = v_cr[0]/cr_mag; v_cr[1] = v_cr[1]/cr_mag; v_cr[2] = v_cr[2]/cr_mag; }
1686
1687 if (LocalHead) {
1688 if (i == opt->CtrlPts_iim[0]) {
1689 fprintf(SUMA_STDERR, "Using SUMA_ROTATE_ABOUT AXIS to calculate velocity field.\n" ); } }
1690
1691 v_expand = 0.0; v_alpha = 0.0;
1692 /* ROTATE THE WEIGHTS USING DISPLACEMENT ANGLE AND AXIS OF ROTATION CALCULATED ABOVE. */
1693 SUMA_ROTATE_ABOUT_AXIS( (&(C->Wv.elts[j3])), v_cr, v_alpha, (&(Wr.elts[j3])) );
1694 v_expand = Deformation_Kernel(opt, v_alpha);
1695 #endif
1696
1697 v_alpha = 0.0; v_expand = 0.0;
1698 SUMA_3D_Rotation_Matrix( (&(opt->CtrlPts[j3])), (&(C->NewNodeList[i3])), v_M, v_alpha, v_cr);
1699 v_expand = Deformation_Kernel(opt, v_alpha);
1700
1701 if( j==0 ) fprintf(test_expansion, "%d %f %f\n", i, v_alpha, v_expand);
1702
1703 if(LocalHead) {
1704 if (i == opt->CtrlPts_iim[0]) {
1705 fprintf(SUMA_STDERR, "AFTER ROTATION MATRIX CALCULATED: \n"
1706 " v_alpha = %f\n"
1707 " expansion = %.5f\n"
1708 " p1 = [ %f %f %f ]; \n"
1709 " p2 = [ %f %f %f ]; \n"
1710 " Angle to be rotated = %f \n"
1711 " Axis of rotation: \n"
1712 " %.12f %.12f %.12f \n"
1713 " Weight = %f %f %f\n",
1714 v_alpha, v_expand,
1715 opt->CtrlPts[j3 ], opt->CtrlPts[j3+1], opt->CtrlPts[j3+2],
1716 C->NewNodeList[i3 ], C->NewNodeList[i3+1], C->NewNodeList[i3+2],
1717 v_alpha,
1718 v_cr[0], v_cr[1], v_cr[2],
1719 C->Wv.elts[j3], C->Wv.elts[j3+1], C->Wv.elts[j3+2]);
1720 }
1721 if(i == opt->CtrlPts_iim[0]) {
1722 fprintf(SUMA_STDERR, "%s:\n"
1723 "v_M = [\n", FuncName);
1724 for (r=0; r<3; ++r) {
1725 fprintf (SUMA_STDERR," %.5f %.5f %.5f \n",
1726 v_expand*v_M [ (r) ][ (0) ],
1727 v_expand*v_M [ (r) ][ (1) ],
1728 v_expand*v_M [ (r) ][ (2) ]);
1729 }
1730 fprintf(SUMA_STDERR, "];\n");
1731 }
1732 }
1733
1734 Wr.elts[j3 ] = (v_M[0][0]*C->Wv.elts[j3 ] + v_M[0][1]*C->Wv.elts[j3+1] + v_M[0][2]*C->Wv.elts[j3+2]);
1735 Wr.elts[j3+1] = (v_M[1][0]*C->Wv.elts[j3 ] + v_M[1][1]*C->Wv.elts[j3+1] + v_M[1][2]*C->Wv.elts[j3+2]);
1736 Wr.elts[j3+2] = (v_M[2][0]*C->Wv.elts[j3 ] + v_M[2][1]*C->Wv.elts[j3+1] + v_M[2][2]*C->Wv.elts[j3+2]);
1737
1738 /* Check rotated weights. */
1739 if(LocalHead) {
1740 if(i == opt->CtrlPts_iim[0]) {
1741 fprintf(SUMA_STDERR, "%s: \n"
1742 " W rotated = %f %f %f \n"
1743 " expansion factor = %f \n",
1744 FuncName, Wr.elts[j3], Wr.elts[j3+1], Wr.elts[j3+2], v_expand); }
1745 if(i == opt->CtrlPts_iim[0]) {
1746 fprintf(SUMA_STDERR, " Dot Product of Rotated Weights with Radius = %.12f \n"
1747 " Mag_Wv_rotated = %f \n",
1748 SUMA_MT_DOT( (&(Wr.elts[j3])), (&(C->NewNodeList[i3])) ),
1749 sqrt( SUMA_POW2(C->Wv.elts[j3 ])
1750 + SUMA_POW2(C->Wv.elts[j3+1])
1751 + SUMA_POW2(C->Wv.elts[j3+2]) ) );
1752 }
1753 }
1754
1755 Wr.elts[j3 ] *= v_expand;
1756 Wr.elts[j3+1] *= v_expand;
1757 Wr.elts[j3+2] *= v_expand;
1758
1759 if(LocalHead) {
1760 if(i == opt->CtrlPts_iim[0]) {
1761 fprintf(SUMA_STDERR, "%s:\n"
1762 "v_M = [\n", FuncName);
1763 for (r=0; r<3; ++r) {
1764 fprintf (SUMA_STDERR," %.5f %.5f %.5f \n",
1765 v_expand*v_M [ (r) ][ (0) ],
1766 v_expand*v_M [ (r) ][ (1) ],
1767 v_expand*v_M [ (r) ][ (2) ]);
1768 }
1769 fprintf(SUMA_STDERR, "];\n");
1770 }
1771 }
1772
1773 if(LocalHead) {
1774 if( i == opt->CtrlPts_iim[0] ) {
1775 fprintf(SUMA_STDERR, "Velocity Field Components as stored in Wr.elts: \n"
1776 " %f %f %f \n",
1777 Wr.elts[j3 ], Wr.elts[j3+1], Wr.elts[j3+2]); }
1778 }
1779
1780 C->VelocityField[i3 ] += Wr.elts[j3 ];
1781 C->VelocityField[i3+1] += Wr.elts[j3+1];
1782 C->VelocityField[i3+2] += Wr.elts[j3+2];
1783 }
1784
1785 if(LocalHead) {
1786 if(i == opt->CtrlPts_iim[0]) {
1787 fprintf(SUMA_STDERR, "%s: Initial Velocity Field, no adjustment: \n"
1788 " %f %f %f \n",
1789 FuncName, C->VelocityField[i3 ], C->VelocityField[i3+1], C->VelocityField[i3+2]); }
1790
1791 if(i == opt->CtrlPts_iim[0]) {
1792 fprintf(SUMA_STDERR, " Dot Product of Velocity Field with Radius = %.12f \n",
1793 SUMA_MT_DOT( (&(C->VelocityField[i3])), (&(C->NewNodeList[i3])) ) ); }
1794 }
1795 }
1796 fclose (test_expansion); test_expansion = NULL;
1797
1798 vector_destroy (&Wr);
1799
1800 SUMA_RETURN(YUP);
1801 }
1802
Debug_Move(MyCircle * C,MyCircleOpt * opt,SUMA_SurfaceObject * SO,double dt,int niter,int m,int a_niter,int first_bad_niter)1803 SUMA_Boolean Debug_Move( MyCircle *C, MyCircleOpt *opt, SUMA_SurfaceObject *SO, double dt, int niter, int m, int a_niter, int first_bad_niter)
1804 {
1805 static char FuncName[]={"Debug_Move"};
1806 int i, i3, j;
1807 char outfile[] = {"Coords_0.txt"}, outfile_speed[] = {"Plot_Speed0.txt"}, outfile_test[50];
1808 char outfile_segments[50], outfile_Vmag[50], outfile_tri_area[50];
1809 double sideA[3], sideB[3], sideC[3], height[3], side[3], t_area;
1810 int f, g, h, f3, g3, h3;
1811 double Vf_segment[3];
1812 double Point_at_Distance[2][3] = { {0.0, 0.0, 0.0},{ 0.0, 0.0, 0.0} }, V_Mag = 0.0;
1813
1814 SUMA_Boolean LocalHead = NOPE;
1815
1816 SUMA_ENTRY;
1817
1818 if(LocalHead) { fprintf(stderr,"%s: niter = %d, dt = %.3f.\n", FuncName, niter, dt); }
1819
1820
1821 /* Test_move.1D -- Write nodelist and velocity field to file. */
1822 if(!first_bad_niter || niter < first_bad_niter+5){
1823 if(!a_niter) { /* For now, only write to file when niter changes, not when a_niter changes. */
1824 FILE *test = NULL;
1825 sprintf(outfile_test, "%s%d.1D", opt->outfile, (niter));
1826 test = fopen (outfile_test, "w");
1827
1828 if(opt->dom_dim > 2) {
1829 fprintf (test, "col#0: Node Index\n"
1830 "col#1,2,3: Node Coordinates at Beginning of Iteration.\n"
1831 "col#4,5,6: Calculated Velocity Field to be Used in this Iteration.\n"
1832 "col#7: Step Size Used in the Move. (magnitude of velocity*dt)\n"
1833 " dt = %f Niter = %d\n", dt, niter );
1834 }
1835 for (i = 0; i < C->N_Node; ++i) {
1836 i3 = 3*i;
1837 /* Calculate magnitude of step size. Storing in C->Theta[i3+2] because this array has already been created
1838 and is not being used for anything. Written to fourth column of test_move.1D file. */
1839 C->Theta[i3+2] = sqrt( SUMA_POW2(dt*C->VelocityField[i3 ]) +
1840 SUMA_POW2(dt*C->VelocityField[i3+1]) +
1841 SUMA_POW2(dt*C->VelocityField[i3+2]) );
1842 fprintf (test, "%d %11.8f %11.8f %11.8f %11.8f %11.8f %11.8f %11.8f\n",
1843 i, C->NewNodeList[i3 ], C->NewNodeList[i3+1], C->NewNodeList[i3+2],
1844 C->VelocityField[i3 ], C->VelocityField[i3+1], C->VelocityField[i3+2], C->Theta[i3+2]);
1845 }
1846 i = 0; i3 = 3*i; /* In Matlab, to graph the circle, need last line to be the same as the first line. */
1847 fprintf (test, "%d %11.8f %11.8f %11.8f %11.8f %11.8f %11.8f %11.8f\n",
1848 i, C->NewNodeList[i3 ], C->NewNodeList[i3+1], C->NewNodeList[i3+2],
1849 C->VelocityField[i3 ], C->VelocityField[i3+1], C->VelocityField[i3+2], C->Theta[i3+2]);
1850 fclose (test); test = NULL;
1851 }
1852
1853 if(opt->dom_dim > 2) {
1854 /* For calculating base and height of each facet and step size. Base and Height of the triangle
1855 are the minimum distances when considering the movement of the nodes that form the vertices of the triangle.
1856 Need triangles to not flip, so must check to see that move (step size) is not greater than the
1857 distance across the triangle, which is at least the length of the base or height. */
1858 if(!first_bad_niter){
1859 FILE *tri_area = NULL;
1860 sprintf(outfile_tri_area, "tri_area%d_%d.1D", m, niter);
1861 tri_area = fopen (outfile_tri_area, "w");
1862 fprintf (tri_area, "col#0: Facet Index\n"
1863 "col#1: Triangle base.\n"
1864 "col#2: Triangle height.\n"
1865 "col#3: Move distance/size of step for the three nodes of the facet.\n"
1866 " Nodes of facet listed in random order.\n"
1867 "col#4: Triangle area.\n"
1868 "dt = %11f\n", dt);
1869 for (i = 0; i < SO->N_FaceSet; ++i) {
1870 i3 = 3*i;
1871 f = SO->FaceSetList[i3 ];
1872 g = SO->FaceSetList[i3+1];
1873 h = SO->FaceSetList[i3+2];
1874 f3 = 3*f; g3 = 3*g, h3 = 3*h;
1875
1876 SUMA_TRI_AREA( (&(C->NewNodeList[f3])), (&(C->NewNodeList[g3])), (&(C->NewNodeList[h3])), t_area );
1877
1878 /* Determine the vectors that makes up the sides of the triangle by subtracting node locations.*/
1879 SUMA_MT_SUB( sideA, (&(C->NewNodeList[f3])), (&(C->NewNodeList[g3])) );
1880 SUMA_MT_SUB( sideB, (&(C->NewNodeList[f3])), (&(C->NewNodeList[h3])) );
1881 SUMA_MT_SUB( sideC, (&(C->NewNodeList[g3])), (&(C->NewNodeList[h3])) );
1882
1883 /* Find the length of the side by finding the magnitude of the side vectors.
1884 These values represent the possible bases for calculating the area of a triangle
1885 if area = 0.5*base*height. */
1886 side[0] = sqrt( SUMA_POW2(sideA[0]) + SUMA_POW2(sideA[1]) + SUMA_POW2(sideA[2]) );
1887 side[1] = sqrt( SUMA_POW2(sideB[0]) + SUMA_POW2(sideB[1]) + SUMA_POW2(sideB[2]) );
1888 side[2] = sqrt( SUMA_POW2(sideC[0]) + SUMA_POW2(sideC[1]) + SUMA_POW2(sideC[2]) );
1889
1890 /* Calculate the possible heights; the perpendicular bisectors of the triangle;
1891 the minimal distance across the triangle. */
1892 height[0] = (2*t_area) / side[0];
1893 height[1] = (2*t_area) / side[1];
1894 height[2] = (2*t_area) / side[2];
1895
1896 fprintf(tri_area, "%d %f %f %f %f\n"
1897 " %f %f %f\n"
1898 " %f %f %f\n", i,
1899 side[0], height[0], C->Theta[f3+2], t_area,
1900 side[1], height[1], C->Theta[g3+2],
1901 side[2], height[2], C->Theta[h3+2] );
1902 }
1903 fclose(tri_area); tri_area = NULL;
1904 }
1905
1906 /* TO PLOT VELOCITY FIELD AT ANY ITERATION. */
1907 /* Write oriented segment file for plotting in SUMA. */
1908 if(!niter){ /* Only write to file at the beginning of every m-loop. */
1909 if(!first_bad_niter || niter < first_bad_niter+5) {
1910 FILE *plot_segments = NULL;
1911 sprintf(outfile_segments, "SUMA_segments_m%d.1D.dset", m);
1912 plot_segments = fopen (outfile_segments, "w");
1913 fprintf (plot_segments, "#oriented_segments\n");
1914 for (i = 0; i < C->N_Node; ++i) {
1915 i3 = 3*i;
1916 /* To plot end of segment, must calculate the location of the point of the velocity vector.
1917 This is done by adding the position vector of the node to the velocity vector at that node. */
1918 Vf_segment[0] = C->VelocityField[i3 ];
1919 Vf_segment[1] = C->VelocityField[i3+1];
1920 Vf_segment[2] = C->VelocityField[i3+2];
1921
1922 Vf_segment[0] = C->NewNodeList[i3 ] + Vf_segment[0];
1923 Vf_segment[1] = C->NewNodeList[i3+1] + Vf_segment[1];
1924 Vf_segment[2] = C->NewNodeList[i3+2] + Vf_segment[2];
1925
1926 fprintf (plot_segments, "%11.8f %11.8f %11.8f %11.8f %11.8f %11.8f 0.0 0.0 1.0 1.0 1.5\n",
1927 C->NewNodeList[i3], C->NewNodeList[i3+1], C->NewNodeList[i3+2],
1928 Vf_segment[0], Vf_segment[1], Vf_segment[2] );
1929 }
1930 fclose (plot_segments); plot_segments = NULL;
1931 }
1932 }
1933 }
1934 }
1935
1936 if(!niter) {
1937 /* Write velocity magnitudes to file for plotting in SUMA. */
1938 FILE *plot_Vmag = NULL;
1939 sprintf(outfile_Vmag, "SUMA_Vmag_m%d.1D.dset", m);
1940 plot_Vmag = fopen (outfile_Vmag, "w");
1941 for (i = 0; i < C->N_Node; ++i) {
1942 i3 = 3*i;
1943 fprintf (plot_Vmag, "%11.8f\n", sqrt( SUMA_POW2(C->VelocityField[i3 ]) +
1944 SUMA_POW2(C->VelocityField[i3+1]) +
1945 SUMA_POW2(C->VelocityField[i3+2]) ));
1946 }
1947 fclose (plot_Vmag); plot_Vmag = NULL;
1948 }
1949
1950 /* Send output to files for graphing in matlab. */
1951 if(LocalHead) {
1952 if(opt->dom_dim == 2) {
1953 FILE *plot_circle = NULL;
1954 sprintf(outfile, "Coords_%d.txt", (niter+1));
1955 plot_circle = fopen (outfile, "w");
1956 for (i = 0; i < C->N_Node; ++i) {
1957 i3 = 3*i;
1958 fprintf (plot_circle, "%11.8f %11.8f %11.8f\n", C->NewNodeList[i3], C->NewNodeList[i3+1], C->NewNodeList[i3+2]);
1959 }
1960 fclose (plot_circle); plot_circle = NULL;
1961
1962 /* Create file for plotting magnitude of velocity field around the circle. */
1963 FILE *plot_speed = NULL;
1964 sprintf(outfile_speed, "Plot_Speed%d.txt", (niter+1));
1965 plot_speed = fopen(outfile_speed, "w");
1966 for (i = 0; i < C->N_Node; ++i) {
1967 i3 = 3*i;
1968 V_Mag = 0.5*sqrt( SUMA_POW2(C->VelocityField[i3 ]) +
1969 SUMA_POW2(C->VelocityField[i3+1]) +
1970 SUMA_POW2(C->VelocityField[i3+2]) );
1971 /* Using half the magnitude to make matlab plot easier to see. */
1972 SUMA_POINT_AT_DISTANCE_NORM( (&(C->NewNodeList[i3])), (&(C->NewNodeList[i3])), V_Mag, Point_at_Distance );
1973 fprintf(plot_speed, "%11.8f %11.8f %11.8f \n",
1974 Point_at_Distance[0][0], Point_at_Distance[0][1], Point_at_Distance[0][2]);
1975 }
1976 fclose(plot_speed); plot_speed = NULL;
1977 }
1978 }
1979
1980 SUMA_RETURN(YUP);
1981 }
1982
Neighbor(MyCircle * C,MyCircleOpt * opt,SUMA_SurfaceObject * SO,int niter,int a_niter)1983 SUMA_Boolean Neighbor( MyCircle *C, MyCircleOpt *opt, SUMA_SurfaceObject *SO, int niter, int a_niter)
1984 {
1985 static char FuncName[]={"Neighbor"};
1986 char outfile_neighb[50];
1987 int k, i, j, s, i3, j3, s4;
1988 double distance = 0.0;
1989 int close_neighb = 0;
1990
1991 SUMA_Boolean LocalHead = NOPE;
1992
1993 SUMA_ENTRY;
1994
1995 if(LocalHead) fprintf(SUMA_STDERR,"%s: Begin loop for checking neighbors.\n", FuncName);
1996
1997 FILE *neighb = NULL;
1998 if(opt->neighb_adjust) { sprintf(outfile_neighb, "Neighbor_Check%d_%d.txt", niter, a_niter); }
1999 else { sprintf(outfile_neighb, "Neighbor_Check%d.txt", niter); }
2000 neighb = fopen (outfile_neighb, "w");
2001
2002 for(i = 0; i < C->N_Node; ++i) {
2003 s4 = 4*i;
2004 fprintf(neighb, "Node = %d, Stepsize = %f\n", i, C->Vf_Step[s4+3]);
2005 for(k=0; k < SO->FN->N_Neighb[i]; ++k) {
2006 j = SO->FN->FirstNeighb[i][k]; /*Index of node neighbor.*/
2007 i3 = 3*i;
2008 j3 = 3*j;
2009 distance = sqrt( SUMA_POW2(C->NewNodeList[j3 ] - C->NewNodeList[i3 ]) +
2010 SUMA_POW2(C->NewNodeList[j3+1] - C->NewNodeList[i3+1]) +
2011 SUMA_POW2(C->NewNodeList[j3+2] - C->NewNodeList[i3+2]) );
2012 /* Consider keeping steps smaller than one half of the distance. */
2013 distance = 0.5*distance;
2014
2015 fprintf(neighb," Neighbor(%d) = %f\n", j, distance);
2016 if(C->Vf_Step[s4+3] > distance) {
2017 fprintf(neighb," Stepsize too big! Need to adjust.\n");
2018 if(!close_neighb) close_neighb = i;
2019 if(LocalHead) {
2020 fprintf(SUMA_STDERR, "%s: Stepsize too big! Node = %d, Neighbor = %d\n"
2021 " distance to nearest node = %f\n"
2022 " step size = %f\n",
2023 FuncName, i, j, distance, C->Vf_Step[s4+3] );
2024 }
2025 }
2026
2027 if(opt->neighb_adjust) {
2028 if(C->Vf_Step[s4+3] > distance) close_neighb = i; break;
2029 }
2030 }
2031 if(opt->neighb_check) fprintf(neighb,"**********************************\n");
2032 if(opt->neighb_adjust && close_neighb) break;
2033 }
2034 fclose (neighb); neighb = NULL;
2035
2036 if(LocalHead) fprintf(SUMA_STDERR,"%s: End loop for checking neighbors.\n", FuncName);
2037
2038 if(opt->neighb_adjust) SUMA_RETURN(close_neighb);
2039 else SUMA_RETURN(YUP);
2040 }
2041
Calculate_Step(MyCircle * C,MyCircleOpt * opt,double dt)2042 SUMA_Boolean Calculate_Step (MyCircle *C, MyCircleOpt *opt, double dt)
2043 {
2044 static char FuncName[]={"Calculate_Step"};
2045 int i, i3, s, s4;
2046 double um;
2047
2048 SUMA_Boolean LocalHead = NOPE;
2049
2050 SUMA_ENTRY;
2051
2052 /* Calculate step size using dt set at beginning of this loop and make step magnitude adjustment. */
2053 for (i = 0; i < C->N_Node; ++i) {
2054 i3 = i*3;
2055 s4 = i*4;
2056
2057 C->Vf_Step[s4 ] = 0.0;
2058 C->Vf_Step[s4+1] = 0.0;
2059 C->Vf_Step[s4+2] = 0.0;
2060
2061 C->Vf_Step[s4 ] = C->VelocityField[i3 ] * dt;
2062 C->Vf_Step[s4+1] = C->VelocityField[i3+1] * dt;
2063 C->Vf_Step[s4+2] = C->VelocityField[i3+2] * dt;
2064
2065 if(LocalHead) {
2066 if(i == opt->CtrlPts_iim[0]) { /* debug when node is 1st control point */
2067 fprintf(SUMA_STDERR, "%s, first control point,before adjustment: \n"
2068 " Node = %d\n"
2069 " dot(u,rad) = [%.18f]\n"
2070 " u(%d) = Vf*dt = [%f %f %f]\n"
2071 " mag(u) = %.8f\n",
2072 FuncName, i, SUMA_MT_DOT( (&(C->Vf_Step[s4])),(&(C->NewNodeList[i3]))),
2073 i, C->Vf_Step[s4 ], C->Vf_Step[s4+1], C->Vf_Step[s4+2],
2074 sqrt( SUMA_POW2(C->Vf_Step[s4 ])+SUMA_POW2(C->Vf_Step[s4+1])+SUMA_POW2(C->Vf_Step[s4+2]) ) );
2075 }
2076 }
2077
2078 if (opt->adjust) {
2079 /* must turn the magnitude of u to that of uc, where |uc| / |u| = tan(a) / a;
2080 since for unit circle/sphere a = |u|, |uc| = tan(a) */
2081 /* um = |u| */
2082 um = sqrt( SUMA_POW2(C->Vf_Step[s4 ]) +
2083 SUMA_POW2(C->Vf_Step[s4+1]) +
2084 SUMA_POW2(C->Vf_Step[s4+2]));
2085 /* rescale |u| to make it |uc| */
2086 if (um > opt->Zero){
2087 C->Vf_Step[s4 ] *= tan(um)/um;
2088 C->Vf_Step[s4+1] *= tan(um)/um;
2089 C->Vf_Step[s4+2] *= tan(um)/um;
2090 }
2091 }
2092
2093 /* Find the magnitude of the step. This is the step size. */
2094 C->Vf_Step[s4+3] = sqrt( SUMA_POW2(C->Vf_Step[s4 ]) +
2095 SUMA_POW2(C->Vf_Step[s4+1]) +
2096 SUMA_POW2(C->Vf_Step[s4+2]) );
2097 }
2098
2099 SUMA_RETURN(YUP);
2100 }
2101
Move_Points(MyCircle * C,MyCircleOpt * opt)2102 SUMA_Boolean Move_Points (MyCircle *C, MyCircleOpt *opt)
2103 {
2104 static char FuncName[]={"Move_Points"};
2105 int i, i3, s, s4;
2106 double mv_mag, mv_alpha, mv_nrm[3], mv_nrm_mag, newnode_mag;
2107
2108 SUMA_Boolean LocalHead = NOPE;
2109
2110 SUMA_ENTRY;
2111
2112 /* Use array C->NewNodeList_temp to store the new node locations while checking to see if dt is small enough. */
2113 /* When happy with dt and the move, set C->NewNodeList equal to C->NewNodeList_temp. */
2114
2115 for (i = 0; i < 3*C->N_Node; ++i) C->NewNodeList_temp[i] = 0.0; /* Reset temporary node list storage. */
2116
2117 /* MOVE THE POINTS USING ROTATION MACRO. */
2118 for (i = 0; i < C->N_Node; ++i) {
2119 s4 = 4*i;
2120 i3 = 3*i;
2121 mv_mag = sqrt( SUMA_POW2(C->Vf_Step[s4 ]) +
2122 SUMA_POW2(C->Vf_Step[s4+1]) +
2123 SUMA_POW2(C->Vf_Step[s4+2]) );
2124 if( mv_mag > opt->Zero) { mv_alpha = atan( mv_mag ); }
2125 else { mv_alpha = 0.0; }
2126 SUMA_MT_CROSS ( mv_nrm,(&(C->NewNodeList[i3])), (&(C->Vf_Step[s4])) );
2127 mv_nrm_mag = sqrt( mv_nrm[0]*mv_nrm[0] + mv_nrm[1]*mv_nrm[1] + mv_nrm[2]*mv_nrm[2] );
2128 if (mv_nrm_mag > opt->Zero) {
2129 mv_nrm[0] = mv_nrm[0]/mv_nrm_mag; mv_nrm[1] = mv_nrm[1]/mv_nrm_mag; mv_nrm[2] = mv_nrm[2]/mv_nrm_mag; }
2130
2131 /* Move the points a small step using the Rotation macro. */
2132 SUMA_ROTATE_ABOUT_AXIS( (&(C->NewNodeList[i3])), (&(mv_nrm[0])), mv_alpha, (&(C->NewNodeList_temp[i3])) );
2133
2134 /* Project point back onto the circle. */
2135 newnode_mag = sqrt( SUMA_POW2(C->NewNodeList_temp[i3 ]) +
2136 SUMA_POW2(C->NewNodeList_temp[i3+1]) +
2137 SUMA_POW2(C->NewNodeList_temp[i3+2]) );
2138 if (newnode_mag > opt->Zero) {
2139 C->NewNodeList_temp[i3 ] = opt->Radius*(C->NewNodeList_temp[i3 ])/( newnode_mag );
2140 C->NewNodeList_temp[i3+1] = opt->Radius*(C->NewNodeList_temp[i3+1])/( newnode_mag );
2141 C->NewNodeList_temp[i3+2] = opt->Radius*(C->NewNodeList_temp[i3+2])/( newnode_mag ); }
2142 }
2143 /* END OF MOVE. */
2144
2145 #if 0
2146 /* OLD MOVE METHOD. MOVE ALONG VELOCITY VECTOR. */
2147 for (i = 0; i < C->N_Node; ++i) {
2148 i3 = 3*i;
2149 C->NewNodeList[i3 ] = C->NewNodeList[i3 ] + C->Vf_Step[s4 ];
2150 C->NewNodeList[i3+1] = C->NewNodeList[i3+1] + C->Vf_Step[s4+1];
2151 C->NewNodeList[i3+2] = C->NewNodeList[i3+2] + C->Vf_Step[s4+2];
2152
2153 newnode_mag = sqrt( SUMA_POW2(C->NewNodeList[i3 ]) +
2154 SUMA_POW2(C->NewNodeList[i3+1]) +
2155 SUMA_POW2(C->NewNodeList[i3+2]) );
2156
2157 /*if (opt->dbg_flag) { fprintf(stderr,"mag initial: %f\n", mag); } */
2158 if ( newnode_mag > 0.000000001 ) {
2159 C->NewNodeList[i3 ] = opt->Radius * (C->NewNodeList[i3 ])/( newnode_mag );
2160 C->NewNodeList[i3+1] = opt->Radius * (C->NewNodeList[i3+1])/( newnode_mag );
2161 C->NewNodeList[i3+2] = opt->Radius * (C->NewNodeList[i3+2])/( newnode_mag ); }
2162 }
2163 /* END OLD METHOD. */
2164 #endif
2165
2166 SUMA_RETURN(YUP);
2167 }
2168