1 /* ---------------------------------------------------------------
2 This file is to contain functions to form
3 surface object structures that are independent
4 of GIFTI and SUMA
5 No functions defined in suma_datasets.c sould be made
6 here as this object will go in libmri.a.
7
8 This is only for the most basic of functions.
9 Add funky stuff to suma_datasets.c ZSS Feb 28 08
10 ---------------------------------------------------------------*/
11
12 #include "suma_objs.h" /* 21 Apr 2020 */
13 /*------------------------------------------------------------*/
14
SUMA_NewAfniSurfaceObject(void)15 NI_group *SUMA_NewAfniSurfaceObject(void)
16 {
17 static char FuncName[]={"SUMA_NewAfniSurfaceObject"};
18 NI_group *aSO=NULL;
19 NI_group *ngr=NULL;
20 SUMA_ENTRY;
21
22 aSO = NI_new_group_element();
23 NI_rename_group(aSO, "SurfaceObject");
24
25 ngr = SUMA_NewAfniSurfaceObjectTriangle();
26 NI_add_to_group(aSO, ngr);
27 ngr = SUMA_NewAfniSurfaceObjectPointset();
28 NI_add_to_group(aSO, ngr);
29 ngr = SUMA_NewAfniSurfaceObjectNormals();
30 NI_add_to_group(aSO, ngr);
31 SUMA_RETURN(aSO);
32 }
33
SUMA_NewAfniSurfaceObjectTriangle(void)34 NI_group *SUMA_NewAfniSurfaceObjectTriangle(void)
35 {
36 static char FuncName[]={"SUMA_NewAfniSurfaceObjectTriangle"};
37 NI_element *nel=NULL;
38 NI_group *ngr=NULL;
39
40 SUMA_ENTRY;
41 ngr = NI_new_group_element();
42 NI_rename_group(ngr, "Gifti_Triangle");
43 nel = NI_new_data_element("Mesh_IJK", 1);
44 NI_add_to_group(ngr, nel);
45
46 SUMA_RETURN(ngr);
47 }
48
SUMA_NewAfniSurfaceObjectPointset(void)49 NI_group *SUMA_NewAfniSurfaceObjectPointset(void)
50 {
51 static char FuncName[]={"SUMA_NewAfniSurfaceObjectPointset"};
52 NI_element *nel=NULL;
53 NI_group *ngr=NULL;
54
55 SUMA_ENTRY;
56
57 ngr = NI_new_group_element();
58 NI_rename_group(ngr, "Gifti_Pointset");
59 nel = NI_new_data_element("Node_XYZ", 4251);
60 NI_add_to_group(ngr, nel);
61 nel = NI_new_data_element("Coord_System", 16);
62 NI_add_column(nel,NI_DOUBLE,NULL);
63 NI_add_to_group(ngr, nel);
64
65 SUMA_RETURN(ngr);
66 }
67
SUMA_NewAfniSurfaceObjectNormals(void)68 NI_group *SUMA_NewAfniSurfaceObjectNormals(void)
69 {
70 static char FuncName[]={"SUMA_NewAfniSurfaceObjectNormals"};
71 NI_element *nel=NULL;
72 NI_group *ngr=NULL;
73
74 SUMA_ENTRY;
75
76 ngr = NI_new_group_element();
77 NI_rename_group(ngr, "Gifti_Normals");
78 nel = NI_new_data_element("Node_Normals", 1);
79 NI_add_to_group(ngr, nel);
80
81 SUMA_RETURN(ngr);
82 }
83
84 #define IF_FREE(ggg) { if (ggg) SUMA_free(ggg); ggg = NULL; }
85
86
SUMA_FreeAfniSurfaceObject(NI_group * aSO)87 NI_group *SUMA_FreeAfniSurfaceObject(NI_group *aSO)
88 {
89 static char FuncName[]={"SUMA_FreeAfniSurfaceObject"};
90
91 SUMA_ENTRY;
92
93 if (aSO) NI_free_element(aSO);
94
95 SUMA_RETURN(NULL);
96 }
97
SUMA_FindNgrNamedElementRec(NI_group * ngr,char * elname,void ** nelp)98 void SUMA_FindNgrNamedElementRec(NI_group *ngr,
99 char *elname,
100 void **nelp)
101 {
102 static char FuncName[]={"SUMA_FindNgrNamedElementRec"};
103 NI_element *nel = NULL;
104 NI_group *nelg = NULL;
105 char *rs=NULL;
106 int ip;
107 int LocalHead = 0;
108
109 SUMA_ENTRY;
110
111 if (!ngr || !elname) {
112 SUMA_S_Err("NULL input ");
113 SUMA_RETURNe;
114 }
115 /* now read the elements in this group */
116 for( ip=0 ; ip < ngr->part_num ; ip++ ){
117 switch( ngr->part_typ[ip] ){
118 /*-- a sub-group ==> recursion! --*/
119 case NI_GROUP_TYPE:
120 nelg = (NI_group *)ngr->part[ip] ;
121 if (LocalHead > 1) {
122 fprintf( SUMA_STDERR,
123 "%s: Looking for %s in group %s \n",
124 FuncName, elname, ngr->name);
125 }
126 if (!strcmp(elname, nelg->name)) {
127 *nelp=(void *)nelg;
128 if (LocalHead) {
129 fprintf( SUMA_STDERR,
130 "%s: Found %s in group %s\n",
131 FuncName, nelg->name, ngr->name);
132 }
133 SUMA_RETURNe;
134 }
135 SUMA_FindNgrNamedElementRec( (NI_group *)ngr->part[ip],
136 elname,
137 nelp);
138 break ;
139 case NI_ELEMENT_TYPE:
140 nel = (NI_element *)ngr->part[ip] ;
141 if (LocalHead > 1) {
142 fprintf( SUMA_STDERR,
143 "%s:>%d< Looking for %s name=%s \n"
144 "vec_len=%d vec_filled=%d, vec_num=%d\n",
145 FuncName, ip, elname,
146 nel->name, nel->vec_len, nel->vec_filled,
147 nel->vec_num );
148 }
149 if (!strcmp(elname, nel->name)) {
150 *nelp=(void *)nel;
151 if (LocalHead) {
152 fprintf( SUMA_STDERR,
153 "%s: Found %s in group %s\n",
154 FuncName, nel->name, ngr->name);
155 }
156 SUMA_RETURNe;
157 }
158 break;
159 default:
160 SUMA_SL_Err("Don't know what to make of this group element\n"
161 "ignoring.");
162 break;
163 }
164 }
165
166
167 SUMA_RETURNe;
168 }
169
SUMA_FindNgrNamedElement(NI_group * ngr,char * elname)170 NI_element *SUMA_FindNgrNamedElement(NI_group *ngr, char *elname)
171 {
172 static char FuncName[]={"SUMA_FindNgrNamedElement"};
173 void *nel = NULL;
174 char *rs=NULL;
175 int ip;
176 int LocalHead = 0;
177
178 SUMA_ENTRY;
179
180 if (!ngr || !elname) {
181 SUMA_S_Err("NULL input ");
182 SUMA_RETURN(nel);
183 }
184
185 SUMA_FindNgrNamedElementRec(ngr, elname, &nel);
186 if (nel && NI_element_type(nel) == NI_GROUP_TYPE) {
187 /* ignore, return NULL */
188 if (LocalHead) {
189 fprintf( SUMA_STDERR,
190 "%s: Found ngr named %s, returning null\n",
191 FuncName, elname);
192 }
193 nel=NULL;
194 }
195 if (LocalHead) {
196 if (nel) {
197 fprintf( SUMA_STDERR,
198 "%s: Found nel %s\n",
199 FuncName, elname);
200 } else {
201 fprintf( SUMA_STDERR,
202 "%s: nel %s not found\n",
203 FuncName, elname);
204 }
205 }
206 SUMA_RETURN((NI_element *)nel);
207 }
208
209 /* Return any element or group named elname */
SUMA_FindNgrNamedAny(NI_group * ngr,char * elname)210 void *SUMA_FindNgrNamedAny(NI_group *ngr, char *elname)
211 {
212 static char FuncName[]={"SUMA_FindNgrNamedAny"};
213 void *nel = NULL;
214 char *rs=NULL;
215 int ip;
216 int LocalHead = 0;
217
218 SUMA_ENTRY;
219
220 if (!ngr || !elname) {
221 SUMA_S_Err("NULL input ");
222 SUMA_RETURN(nel);
223 }
224
225 SUMA_FindNgrNamedElementRec(ngr, elname, &nel);
226 if (LocalHead) {
227 if (nel) {
228 fprintf( SUMA_STDERR,
229 "%s: Found nel/ngr %s\n",
230 FuncName, elname);
231 } else {
232 fprintf( SUMA_STDERR,
233 "%s: nel/ngr %s not found\n",
234 FuncName, elname);
235 }
236 }
237 SUMA_RETURN(nel);
238 }
239
SUMA_NI_AttrOfNamedElement(NI_group * ngr,char * elname,char * attrname)240 char *SUMA_NI_AttrOfNamedElement(NI_group *ngr, char *elname, char *attrname)
241 {
242 static char FuncName[]={"SUMA_NI_AttrOfNamedElement"};
243 NI_element *nel = NULL;
244
245 SUMA_ENTRY;
246
247 if (!ngr || !elname || !attrname) {
248 SUMA_S_Err("NULL input");
249 fprintf(SUMA_STDERR,"%s: %p %p %p\n", FuncName, ngr, elname, attrname);
250 SUMA_RETURN(NULL);
251 }
252 nel = SUMA_FindNgrNamedElement(ngr, elname);
253 if (!nel) SUMA_RETURN(NULL);
254 SUMA_RETURN(NI_get_attribute(nel,attrname));
255 }
256
SUMA_NI_intAttrOfNamedElement(NI_group * ngr,char * elname,char * attrname)257 int SUMA_NI_intAttrOfNamedElement(NI_group *ngr, char *elname, char *attrname)
258 {
259 static char FuncName[]={"SUMA_NI_intAttrOfNamedElement"};
260 NI_element *nel = NULL;
261
262 SUMA_ENTRY;
263
264 if (!ngr || !elname || !attrname) {
265 SUMA_S_Err("NULL input ");
266 SUMA_RETURN(0);
267 }
268 nel = SUMA_FindNgrNamedElement(ngr, elname);
269 if (!nel) SUMA_RETURN(0);
270 SUMA_RETURN(SUMA_NI_get_int(nel,attrname));
271 }
272
SUMA_NI_doubleAttrOfNamedElement(NI_group * ngr,char * elname,char * attrname)273 double SUMA_NI_doubleAttrOfNamedElement(NI_group *ngr, char *elname,
274 char *attrname)
275 {
276 static char FuncName[]={"SUMA_NI_doubleAttrOfNamedElement"};
277 NI_element *nel = NULL;
278
279 SUMA_ENTRY;
280
281 if (!ngr || !elname || !attrname) {
282 SUMA_S_Err("NULL input ");
283 SUMA_RETURN(0);
284 }
285 nel = SUMA_FindNgrNamedElement(ngr, elname);
286 if (!nel) SUMA_RETURN(0);
287 SUMA_RETURN(SUMA_NI_get_double(nel,attrname));
288 }
289
SUMA_NI_get_int(NI_element * nel,char * attrname)290 int SUMA_NI_get_int(NI_element *nel, char *attrname)
291 {
292 static char FuncName[]={"SUMA_NI_get_int"};
293 int n=0;
294 char *s=NULL;
295
296 SUMA_ENTRY;
297 if (nel && attrname && (s=NI_get_attribute(nel,attrname))) {
298 n = (int)strtol(s,NULL,10);
299 }
300 SUMA_RETURN(n);
301 }
302
SUMA_NI_get_double(NI_element * nel,char * attrname)303 double SUMA_NI_get_double(NI_element *nel, char *attrname)
304 {
305 static char FuncName[]={"SUMA_NI_get_double"};
306 double n=0;
307 char *s=NULL;
308
309 SUMA_ENTRY;
310 if (nel && attrname && (s=NI_get_attribute(nel,attrname))) {
311 n = strtod(s,NULL);
312 }
313 SUMA_RETURN(n);
314 }
315
SUMA_NI_set_int(NI_element * nel,char * attrname,int n)316 void SUMA_NI_set_int(NI_element *nel, char *attrname, int n)
317 {
318 static char FuncName[]={"SUMA_NI_set_int"};
319 char sb[32]={""};
320
321 SUMA_ENTRY;
322 if (nel && attrname) {
323 sprintf(sb,"%d",n);
324 NI_set_attribute(nel, attrname, sb);
325 }
326 SUMA_RETURNe;
327 }
328
SUMA_NI_set_double(NI_element * nel,char * attrname,double n)329 void SUMA_NI_set_double(NI_element *nel, char *attrname, double n)
330 {
331 static char FuncName[]={"SUMA_NI_set_double"};
332 char sb[32]={""};
333
334 SUMA_ENTRY;
335 if (nel && attrname) {
336 sprintf(sb,"%f",n);
337 NI_set_attribute(nel, attrname, sb);
338 }
339 SUMA_RETURNe;
340 }
341
342 /*!
343 \brief A function to calulate shortest distance on a graph,
344 and return the path corresponding to the short distance without
345 relying on the SurfaceObject structure.
346
347 It uses exactly the same approach as SUMA_Dijkstra but it has been
348 modified to eventually survive in the AFNI world without SUMA's defines.
349
350 Perhaps at some point I should make SUMA_Dijkstra call this one
351 directly, something a la SUMA_Dijkstra_usegen . This way I would
352 not have two versions of the same algorithm in the code.
353
354 \param N_Node: Total number of nodes
355 \param NodeList: 3*N_node vector of floats. Each triplet of values
356 contains the xyz coordinates of a node.
357 This can be NULL, as long as FirstNeighbDist is not.
358 \param N_Neihbv: N_node vector of ints. N_Neighbv[n] contains the
359 number of nodes that are connected by an edge
360 (i.e. first order neighbors) to n
361 \param FirstNeighb: N_Node x MAX_N_Neighb matrix of ints.
362 FirstNeighb[n] is a vector of at least N_Neighb[n] node
363 indices. Those are the indices of the first order neighbors
364 of n
365 \param FirstNeighbDist: N_Node x MAX_N_Neighb matrix of floats.
366 FirstNeighbDist[n] is a vector of at least N_Neighb[n]
367 distances from node n. Say node n1 is the second neighbor of
368 n. Then n1 = FirstNeighb[n][1] and the distance between
369 n and n1 is d1 = FirstNeighbDist[n][1].
370 If FirstNeighbDist is NULL, the d1 is calculated based on
371 the Euclidian distance between the XYZ triplets starting at
372 NodeList[3*n] and NodeList[3*n1].
373 Naturally, you'll need to pass one of NodeList
374 or FirstNeighbDist. If both are not NULL, then
375 FirstNeighbDist takes precedence.
376 \param Nx: index of starting node
377 \param Ny: index of ending node
378 \param isNodeInMeshp: N_Node vector which can be used to restrict which nodes
379 can be considered in the path. You can pass NULL to have
380 all nodes be considered
381 \param N_isNodeInMesh: Pointer to the total number of nodes that
382 make up the mesh (subset of SO)
383 This parameter is passed as a pointer because as nodes in the mesh
384 are visited, that number is reduced and represents when the
385 function returns, the number of nodes that were
386 never visited in the search.
387 Set to NULL, if isNodeInMesh is NULL
388 \param Method_Number (int) selector for which algorithm to use. Choose from:
389 0 - Straight forward implementation, slow
390 1 - Variation to eliminate long searches for minimum of L,
391 much much much faster than 0, 5 times more memory.
392 \param Path_length (float *) The distance between Nx and Ny.
393 This value is negative if no path between
394 Nx and Ny was found.
395 \param N_Path (int *) Number of nodes forming the Path vector
396
397 \return Path (float) A vector of N_Path node indices forming the shortest
398 path, from Nx (Path[0]) to Ny (Path[*N_Path - 1]).
399 NULL is returned in case of error.
400
401 */
SUMA_Dijkstra_generic(int N_Node,float * NodeList,int NodeDim,int dist_metric,int * N_Neighbv,int ** FirstNeighb,float ** FirstNeighbDist,int Nx,int Ny,byte * isNodeInMeshp,int * N_isNodeInMesh,int Method_Number,float * Lfinal,int * N_Path,int verb)402 int * SUMA_Dijkstra_generic (int N_Node,
403 float *NodeList, int NodeDim, int dist_metric,
404 int *N_Neighbv, int **FirstNeighb, float **FirstNeighbDist,
405 int Nx, int Ny,
406 byte *isNodeInMeshp,
407 int *N_isNodeInMesh, int Method_Number,
408 float *Lfinal, int *N_Path,
409 int verb)
410 {
411 static char FuncName[] = {"SUMA_Dijkstra_generic"};
412 float *L = NULL, Lmin = -1.0;
413 double de=0.0, le=0.0;
414 int i, iw, iv, v, w, N_Neighb, *Path = NULL, N_loc=-1, kk=0;
415 SUMA_Boolean *isNodeInMesh=NULL;
416 struct timeval start_time;
417 SUMA_DIJKSTRA_PATH_CHAIN *DC = NULL, *DCi=NULL, *DCp=NULL;
418 SUMA_Boolean Found = NOPE;
419 /* variables for method 2 */
420 int N_Lmins, *vLmins=NULL, *vLocInLmins=NULL,
421 iLmins, ReplacingNode, ReplacedNodeLocation;
422 float *Lmins=NULL;
423 SUMA_Boolean LocalHead = NOPE;
424
425 SUMA_ENTRY;
426
427 *Lfinal = -1.0;
428 *N_Path = 0;
429
430 if (!isNodeInMeshp) {
431 if (!(isNodeInMesh = (SUMA_Boolean *)SUMA_malloc( sizeof(SUMA_Boolean) *
432 N_Node) )) {
433
434 SUMA_S_Err("Failed to allocate");
435 goto CLEANUP;
436 }
437 memset((void*)isNodeInMesh, 1, sizeof(SUMA_Boolean) * N_Node);
438 N_isNodeInMesh = &N_loc;
439 *N_isNodeInMesh = N_Node;
440 } else {
441 isNodeInMesh = isNodeInMeshp;
442 }
443
444 /* make sure Both Nx and Ny exist in isNodeInMesh */
445 if ( Nx < 0 || Nx >= N_Node ||
446 Ny < 0 || Ny >= N_Node ) {
447 fprintf (SUMA_STDERR,
448 "\aError %s: Node %d (Nx) or %d (Ny) is outside range [0..%d[ \n"
449 , FuncName, Nx, Ny, N_Node);
450 goto CLEANUP;
451 }
452 if (!isNodeInMesh[Nx]) {
453 fprintf (SUMA_STDERR,
454 "\aError %s: Node %d (Nx) is not in mesh.\n", FuncName, Nx);
455 goto CLEANUP;
456 }
457 if (!isNodeInMesh[Ny]) {
458 fprintf (SUMA_STDERR,
459 "\aError %s: Node %d (Ny) is not in mesh.\n", FuncName, Ny);
460 goto CLEANUP;
461 }
462
463 if (!N_Neighbv || !FirstNeighb) {
464 fprintf (SUMA_STDERR,
465 "Error %s: missing N_Neighb or FirstNeighb.\n", FuncName);
466 goto CLEANUP;
467 }
468
469 if (!NodeList && !FirstNeighbDist) {
470 fprintf (SUMA_STDERR,
471 "Error %s: need at least NodeList or FirstNeighbDist.\n",
472 FuncName);
473 goto CLEANUP;
474 }
475
476 /* allocate for chain */
477 DC = (SUMA_DIJKSTRA_PATH_CHAIN *)SUMA_malloc(
478 sizeof(SUMA_DIJKSTRA_PATH_CHAIN) * N_Node);
479 if (!DC) {
480 fprintf (SUMA_STDERR, "Error %s: Could not allocate. \n", FuncName);
481 goto CLEANUP;
482 }
483
484 switch (Method_Number) {
485
486 case 0: /* Method 0, Brute force */
487 /* allocate for vertices labels */
488 L = (float *) SUMA_calloc (N_Node, sizeof (float));
489 if (!L) {
490 fprintf (SUMA_STDERR, "Error %s: Failed to allocate.\n", FuncName);
491 goto CLEANUP;
492 }
493
494 /* label all vertices with very large numbers,
495 initialize path previous pointers to null */
496 for (i=0; i < N_Node; ++i) {
497 L[i] = LARGE_NUM;
498 DC[i].Previous = NULL;
499 }
500 /* label starting vertex with 0 */
501 L[Nx] = 0.0;
502 Lmin = 0.0;
503 v = Nx;
504 *Lfinal = -1.0;
505 /* initialize path at Nx */
506 DC[Nx].Previous = NULL;
507 DC[Nx].node = Nx;
508 DC[Nx].le = 0.0;
509 DC[Nx].order = 0;
510 *N_Path = 0;
511 /* Brute force method */
512 do {
513 /* find v in Mesh / L(v) is minimal */
514 /* this sucks up a lot of time because it is searching
515 the entire set of N_Node instead of the one that was
516 intersected only.
517 This can be sped up, considerably */
518 SUMA_MIN_LOC_VEC(L, N_Node, Lmin, v);
519 /* locates and finds the minimum of L, nodes not in mesh will
520 keep their large values and will not be picked*/
521 if (!isNodeInMesh[v]) {
522 if (verb) {
523 fprintf (SUMA_STDERR,
524 "\aERROR %s: Dijkstra derailed. v = %d, Lmin = %f.\n"
525 " Try another point.", FuncName, v, Lmin);
526 }
527 goto CLEANUP;
528 }
529 if (v == Ny) {
530 if (LocalHead) fprintf (SUMA_STDERR, "%s: Done.\n", FuncName);
531 *Lfinal = L[v];
532 Found = YUP;
533 } else {
534 N_Neighb = N_Neighbv[v];
535 for (i=0; i < N_Neighb; ++i) {
536 w = FirstNeighb[v][i];
537 if (isNodeInMesh[w]) {
538 iw = NodeDim*w;
539 iv = NodeDim*v;
540 if (!FirstNeighbDist) {/* calculate edge length */
541 if (dist_metric == 0) {
542 le = 0.0; de = 0.0;
543 for (kk=0; kk<NodeDim; ++kk) {
544 de = (NodeList[iw+kk] - NodeList[iv+kk]);
545 le += de*de;
546 }
547 le = sqrt ( le );
548 } else {
549 fprintf (SUMA_STDERR,
550 "ERROR %s: Only Euclidian distance supported\n"
551 , FuncName);
552 goto CLEANUP;
553 }
554 } else {
555 le = FirstNeighbDist[v][i];
556 }
557 if (L[w] > L[v] + le ) {
558 L[w] = L[v] + le;
559 /* update the path */
560 DCp = &(DC[v]); /* previous path */
561 DC[w].Previous = (void *) DCp;
562 DC[w].le = le;
563 DC[w].node = w;
564 DC[w].order = DCp->order + 1;
565 }
566 }
567 }
568
569 /* remove node v from isNodeInMesh and reset their distance
570 value to a very large one,
571 this way you do not have to reinitialize this variable. */
572 isNodeInMesh[v] = NOPE;
573 *N_isNodeInMesh -= 1;
574 L[v] = LARGE_NUM;
575 Found = NOPE;
576 }
577 } while (*N_isNodeInMesh > 0 && !Found);
578
579 if (!Found) {
580 fprintf (SUMA_STDERR,
581 "Error %s: No more nodes in mesh, "
582 "failed to reach target.\n", FuncName);
583 goto CLEANUP;
584 }else {
585 if (LocalHead)
586 fprintf (SUMA_STDERR,
587 "%s: Path between Nodes %d and %d is %f.\n",
588 FuncName, Nx, Ny, *Lfinal);
589 }
590
591
592 if (L) SUMA_free(L); L = NULL;
593 break;
594
595 case 1: /********* Method 1- faster minimum searching *******************/
596
597 /* allocate for vertices labels and minimums vectors*/
598 L = (float *) SUMA_calloc (N_Node, sizeof (float));
599 /* L[i] = distance to a node i*/
600 Lmins = (float *) SUMA_calloc (N_Node, sizeof (float));
601 /* Lmins = vector of minimum calculated distances to node */
602 vLmins = (int *) SUMA_calloc (N_Node, sizeof (int));
603 /* vLmins[i] = index (into L) of the node having a
604 distance Lmins[i] */
605 vLocInLmins = (int *) SUMA_calloc (N_Node, sizeof (int));
606 /* vLocInLmin[j] = index (into Lmins) of a node having
607 index j (into L) */
608
609 if (!L || !Lmins || !vLmins || !vLocInLmins) {
610 fprintf (SUMA_STDERR, "Error %s: Failed to allocate.\n", FuncName);
611 goto CLEANUP;
612 }
613
614 /* label all vertices with very large numbers and initialize
615 vLocInLmins to -1*/
616 for (i=0; i < N_Node; ++i) {
617 L[i] = LARGE_NUM;
618 Lmins[i] = LARGE_NUM;
619 vLocInLmins[i] = -1;
620 DC[i].Previous = NULL;
621 }
622
623 /* label starting vertex with 0 */
624 L[Nx] = 0.0;
625 *Lfinal = -1.0;
626
627 /* initialize values of vectors used to keep track of minimum
628 values of L and their corresponding nodes */
629 Lmins[0] = 0.0;
630 vLmins[0] = Nx;
631 vLocInLmins[Nx] = 0;
632 N_Lmins = 1;
633
634 /* initialize path at Nx */
635 DC[Nx].Previous = NULL;
636 DC[Nx].node = Nx;
637 DC[Nx].le = 0.0;
638 DC[Nx].order = 0;
639 *N_Path = 0;
640
641 /* method with efficient tracking of minimum */
642 if (LocalHead)
643 fprintf (SUMA_STDERR,
644 "%s: about to MIN_LOC ....N_isNodeInMesh = %d\n",
645 FuncName, *N_isNodeInMesh);
646 do {
647 /* find v in Mesh / L(v) is minimal */
648 SUMA_MIN_LOC_VEC(Lmins, N_Lmins, Lmin, iLmins);
649 /* locates the minimum value in Lmins vector */
650 v = vLmins[iLmins]; /* get the node for this Lmin value */
651 if (!isNodeInMesh[v]) {
652 if (verb) {
653 fprintf (SUMA_STDERR,"\aERROR %s: \n"
654 "Dijkstra derailed. v = %d, Lmin = %f\n."
655 "Try another point.", FuncName, v, Lmin);
656 }
657 goto CLEANUP;
658 }
659 #ifdef LOCALDEBUG
660 fprintf (SUMA_STDERR, "%s: Node v = %d.\n", FuncName, v);
661 #endif
662 if (v == Ny) {
663 if (LocalHead) fprintf (SUMA_STDERR, "%s: Done.\n", FuncName);
664 *Lfinal = L[v];
665 Found = YUP;
666 } else {
667 N_Neighb = N_Neighbv[v];
668 for (i=0; i < N_Neighb; ++i) {
669 w = FirstNeighb[v][i];
670 if (isNodeInMesh[w]) {
671 iw = NodeDim*w;
672 iv = NodeDim*v;
673 if (!FirstNeighbDist) {/* calculate edge length */
674 if (dist_metric == 0) {
675 le = 0.0; de = 0.0;
676 for (kk=0; kk<NodeDim; ++kk) {
677 de = (NodeList[iw+kk] - NodeList[iv+kk]);
678 le += de*de;
679 }
680 le = sqrt ( le );
681 } else {
682 fprintf (SUMA_STDERR,
683 "ERROR %s: Only Euclidian distance supported\n"
684 , FuncName);
685 goto CLEANUP;
686 }
687 } else {
688 le = FirstNeighbDist[v][i];
689 }
690 if (L[w] > L[v] + le ) {
691 #ifdef LOCALDEBUG
692 fprintf (SUMA_STDERR,
693 "%s: L[%d]=%f > L[%d] = %f + le = %f.\n",
694 FuncName, w, L[w], v, L[v], le);
695 #endif
696 L[w] = L[v] + le;
697 /* update the path */
698 DCp = &(DC[v]); /* previous path */
699 DC[w].Previous = (void *) DCp;
700 DC[w].le = le;
701 DC[w].node = w;
702 DC[w].order = DCp->order + 1;
703
704 if (vLocInLmins[w] < 0) {
705 #ifdef LOCALDEBUG
706 fprintf (SUMA_STDERR,
707 "%s: adding entry for w = %d - First Hit. \n",
708 FuncName, w);
709 #endif
710 Lmins[N_Lmins] = L[w];
711 /* add this value to Lmins vector */
712 vLmins[N_Lmins] = w;
713 /* store the node for this Lmins value */
714 vLocInLmins[w] = N_Lmins;
715 /* store where that node is represented
716 in Lmins */
717 ++N_Lmins; /* increment N_Lmins */
718 } else {
719 #ifdef LOCALDEBUG
720 fprintf (SUMA_STDERR,
721 "%s: modifying entry for w = %d Second Hit.\n", FuncName, w);
722 #endif
723 Lmins[vLocInLmins[w]] = L[w];
724 /* update value for Lmins */
725 }
726 }else {
727 #ifdef LOCALDEBUG
728 fprintf (SUMA_STDERR,
729 "%s: L[%d]=%f < L[%d] = %f + le = %f.\n",
730 FuncName, w, L[w], v, L[v], le);
731 #endif
732 }
733 }
734 }
735
736 /* remove node v from isNodeInMesh and reset their distance
737 value to a very large one,
738 this way you do not have to reinitialize this variable. */
739 isNodeInMesh[v] = NOPE;
740 *N_isNodeInMesh -= 1;
741 L[v] = LARGE_NUM;
742 Found = NOPE;
743
744 /* also remove the values (by swapping it with last element)
745 for this node from Lmins */
746 #ifdef LOCALDEBUG
747 {
748 int kkk;
749 fprintf (SUMA_STDERR,"Lmins\tvLmins\tvLocInLmins\n");
750 for (kkk=0; kkk < N_Lmins; ++kkk)
751 fprintf (SUMA_STDERR,"%f\t%d\t%d\n",
752 Lmins[kkk], vLmins[kkk],
753 vLocInLmins[vLmins[kkk]] );
754
755 }
756 #endif
757
758 if (vLocInLmins[v] >= 0) { /* remove its entry if there is one */
759 #ifdef LOCALDEBUG
760 fprintf (SUMA_STDERR,
761 "%s: removing node v = %d. N_Lmins = %d\n",
762 FuncName, v, N_Lmins);
763 #endif
764 --N_Lmins;
765 ReplacingNode = vLmins[N_Lmins];
766 ReplacedNodeLocation = vLocInLmins[v];
767 Lmins[vLocInLmins[v]] = Lmins[N_Lmins];
768 vLmins[vLocInLmins[v]] = vLmins[N_Lmins];
769 vLocInLmins[ReplacingNode] = ReplacedNodeLocation;
770 vLocInLmins[v] = -1;
771 Lmins[N_Lmins] = LARGE_NUM;
772 }
773 }
774 } while (*N_isNodeInMesh > 0 && !Found);
775
776 if (!Found) {
777 fprintf (SUMA_STDERR,
778 "Error %s: No more nodes in mesh, "
779 "failed to reach target %d. NLmins = %d\n",
780 FuncName, Ny, N_Lmins);
781 goto CLEANUP;
782 }else {
783 if (LocalHead)
784 fprintf (SUMA_STDERR,
785 "%s: Path between Nodes %d and %d is %f.\n",
786 FuncName, Nx, Ny, *Lfinal);
787 }
788
789
790
791 if (L) SUMA_free(L); L = NULL;
792 if (Lmins) SUMA_free(Lmins); Lmins = NULL;
793 if (vLmins) SUMA_free(vLmins); vLmins = NULL;
794 if (vLocInLmins) SUMA_free(vLocInLmins); vLocInLmins = NULL;
795 break; /********** Method 1- faster minimum searching **************/
796 default:
797 fprintf (SUMA_STDERR,
798 "Error %s: No such method (%d).\n",
799 FuncName, Method_Number);
800 goto CLEANUP;
801 break;
802 }
803
804 /* now reconstruct the path */
805 *N_Path = DC[Ny].order+1;
806 Path = (int *) SUMA_calloc (*N_Path, sizeof(int));
807 if (!Path) {
808 fprintf (SUMA_STDERR, "Error %s: Failed to allocate.\n", FuncName);
809 goto CLEANUP;
810 }
811
812 DCi = &(DC[Ny]);
813 iv = *N_Path - 1;
814 Path[iv] = Ny;
815 if (iv > 0) {
816 do {
817 --iv;
818 DCp = (SUMA_DIJKSTRA_PATH_CHAIN *) DCi->Previous;
819 Path[iv] = DCp->node;
820 DCi = DCp;
821 } while (DCi->Previous);
822 }
823
824 if (iv != 0) {
825 fprintf (SUMA_STDERR,
826 "Error %s: iv = %d. This should not be.\n",
827 FuncName, iv);
828 }
829
830 CLEANUP:
831 if (L) SUMA_free(L);
832 if (Lmins) SUMA_free(Lmins);
833 if (vLmins) SUMA_free(vLmins);
834 if (vLocInLmins) SUMA_free(vLocInLmins);
835 if (DC) SUMA_free(DC);
836 if (!isNodeInMeshp) SUMA_free(isNodeInMesh);
837
838 SUMA_RETURN (Path);
839 }
840