1 /*
2  * Argyll Color Correction System
3  * Average one or more .ti3 (or other CGATS like) file values together.
4  * If just one file is supplied, all patches within it with
5  * the same device value are averaged together.
6  *
7  * Author: Graeme W. Gill
8  * Date:   18/1/2011
9  *
10  * Copyright 2005, 2010, 2011 Graeme W. Gill
11  * All rights reserved.
12  *
13  * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
14  * see the License.txt file for licencing details.
15  *
16  * (based on splitti3.c)
17  */
18 
19 /*
20  * TTBD:
21 
22 	Should probably re-index SAMPLE_ID field
23 
24  */
25 
26 
27 #undef DEBUG
28 
29 #define verbo stdout
30 
31 #include <stdio.h>
32 #include <string.h>
33 #include <sys/types.h>
34 #include <time.h>
35 #include "copyright.h"
36 #include "aconfig.h"
37 #include "numlib.h"
38 #include "sort.h"
39 #include "cgats.h"
40 #include "xicc.h"
41 #include "insttypes.h"
42 
43 static double average(double *vals, int nvals);
44 static double median(double *vals, int nvals);
45 static void geommed(double res[3], double vals[][3], int nvals);
46 
usage(char * diag,...)47 void usage(char *diag, ...) {
48 	int i;
49 	fprintf(stderr,"Average or merge values in .ti3 like files, Version %s\n",ARGYLL_VERSION_STR);
50 	if (diag != NULL) {
51 		va_list args;
52 		fprintf(stderr,"  Diagnostic: ");
53 		va_start(args, diag);
54 		vfprintf(stderr, diag, args);
55 		va_end(args);
56 		fprintf(stderr,"\n");
57 	}
58 	fprintf(stderr,"Author: Graeme W. Gill, licensed under the AGPL Version 3\n");
59 	fprintf(stderr,"usage: average [-options] input1.ti3 input2.ti3 ... output.ti3\n");
60 	fprintf(stderr," -v              Verbose\n");
61 	fprintf(stderr," -e              Median rather than average\n");
62 	fprintf(stderr," -g              Geometric Median of PCS in encoded space\n");
63 	fprintf(stderr," -L              Geometric Median of PCS in L*a*b* space\n");
64 	fprintf(stderr," -X              Geometric Median of PCS in XYZ space\n");
65 	fprintf(stderr," -m              Merge rather than average\n");
66 	fprintf(stderr," input1.ti3      First input file\n");
67 	fprintf(stderr," input2.ti3      Second input file\n");
68 	fprintf(stderr," ...             etc.\n");
69 	fprintf(stderr," output.ti3      Resulting averaged or merged output file\n");
70 	exit(1);
71 }
72 
73 /* Information about each file */
74 struct _inpinfo {
75 	char name[MAXNAMEL+1];
76 	cgats *c;
77 }; typedef struct _inpinfo inpinfo;
78 
main(int argc,char * argv[])79 int main(int argc, char *argv[]) {
80 	int fa,nfa;					/* current argument we're looking at */
81 	int verb = 0;
82 	int domedian = 0;			/* Median rather than average */
83 	int dogeom = 0;				/* Do geometric median of PCS, 2 = Lab, 3 = PCS */
84 	int domerge = 0;			/* Merge rather than average */
85 
86 	int ninps = 0;				/* Number of input files */
87 	inpinfo *inps;				/* Input file info. inp[ninp] == output file */
88 	cgats *ocg;					/* Copy of output cgats * */
89 
90 	cgats_set_elem *setel;		/* Array of set value elements */
91 	int *flags;					/* Point to destination of set */
92 
93 	int nchan = 0;				/* Number of device channels */
94 	int chix[ICX_MXINKS];		/* Device chanel indexes */
95 	int npcs = 0;
96 
97 	int haspcs[2] =  { 0 };		/* Has Lab, XYZ */
98 	int pcsix[3][3];			/* Lab, XYZ chanel indexes */
99 
100 	int i, j, n;
101 
102 	error_program = "average";
103 
104 	if (argc < 2)
105 		usage("Too few arguments (%d, minimum is 2)",argc-1);
106 
107 	/* Process the arguments */
108 	for(fa = 1;fa < argc;fa++) {
109 		nfa = fa;					/* skip to nfa if next argument is used */
110 		if (argv[fa][0] == '-')	{	/* Look for any flags */
111 			char *na = NULL;		/* next argument after flag, null if none */
112 
113 			if (argv[fa][2] != '\000')
114 				na = &argv[fa][2];		/* next is directly after flag */
115 			else {
116 				if ((fa+1) < argc) {
117 					if (argv[fa+1][0] != '-') {
118 						nfa = fa + 1;
119 						na = argv[nfa];		/* next is seperate non-flag argument */
120 					}
121 				}
122 			}
123 
124 			if (argv[fa][1] == '?')
125 				usage("Usage requested");
126 
127 			/* Median */
128 			else if (argv[fa][1] == 'e') {
129 				domedian = 1;
130 			}
131 
132 			/* Geometric Median of PCS */
133 			else if (argv[fa][1] == 'g') {
134 				dogeom = 1;
135 			}
136 
137 			/* Geometric Median of PCS in L*a*b* */
138 			else if (argv[fa][1] == 'L') {
139 				dogeom = 2;
140 			}
141 
142 			/* Geometric Median of PCS in XYZ */
143 			else if (argv[fa][1] == 'X') {
144 				dogeom = 3;
145 			}
146 
147 			/* Merge */
148 			else if (argv[fa][1] == 'm') {
149 				domerge = 1;
150 			}
151 
152 			/* Verbosity */
153 			else if (argv[fa][1] == 'v') {
154 				verb = 1;
155 			}
156 
157 			else  {
158 				usage("Unknown flag '%c'",argv[fa][1]);
159 			}
160 
161 		} else if (argv[fa][0] != '\000') {
162 			/* Get the next filename */
163 
164 			if (ninps == 0)
165 				inps = (inpinfo *)malloc(sizeof(inpinfo));
166 			else
167 				inps = (inpinfo *)realloc(inps, (ninps+1) * sizeof(inpinfo));
168 			if (inps == NULL)
169 				error("Malloc failed in allocating space for file info.");
170 
171 			memset((void *)&inps[ninps], 0, sizeof(inpinfo));
172 			strncpy(inps[ninps].name,argv[fa],MAXNAMEL);
173 			inps[ninps].name[MAXNAMEL] = '\000';
174 
175 			ninps++;
176 		} else {
177 			break;
178 		}
179 	}
180 
181 	if (ninps < 2)
182 		error("Must be at least one input and one output file specified");
183 
184 	ninps--;	/* Number of inputs */
185 
186 	/* Open and read each input file, and create output file */
187 	for (n = 0; n <= ninps; n++) {
188 
189 		if ((inps[n].c = new_cgats()) == NULL)
190 			error("Failed to create cgats object for file '%s'",inps[n].name);
191 
192 		if (n < ninps) {		/* If input file, read it */
193 			inps[n].c->add_other(inps[n].c, ""); 	/* Allow any signature file */
194 
195 			if (inps[n].c->read_name(inps[n].c, inps[n].name))
196 				error("CGATS file '%s' read error : %s",inps[n].name,inps[n].c->err);
197 
198 			if (inps[n].c->ntables < 1)
199 				error ("Input file '%s' doesn't contain at least one table",inps[n].name);
200 		}
201 	}
202 	ocg = inps[ninps].c;		/* Alias for output file */
203 
204 	/* Duplicate everything from the first input file into the output file. */
205 	for (n = 0; n < inps[0].c->ntables; n++) {
206 
207 		if (inps[0].c->t[n].tt == cgats_X) {
208 			ocg->add_other(ocg, inps[0].c->cgats_type);
209 			ocg->add_table(ocg, tt_other, 0);
210 		} else if (inps[0].c->t[n].tt == tt_other) {
211 			int oi;
212 			oi = ocg->add_other(ocg, inps[0].c->others[inps[0].c->t[n].oi]);
213 			ocg->add_table(ocg, tt_other, oi);
214 		} else {
215 			ocg->add_table(ocg, inps[0].c->t[n].tt, 0);
216 		}
217 
218 		/* Duplicate all the keywords */
219 		for (i = 0; i < inps[0].c->t[n].nkwords; i++) {
220 //printf("~1 table %d, adding keyword '%s'\n",n,inps[0].c->t[n].ksym[i]);
221 			ocg->add_kword(ocg, n, inps[0].c->t[n].ksym[i], inps[0].c->t[n].kdata[i], NULL);
222 		}
223 
224 		/* Duplicate all of the fields */
225 		for (i = 0; i < inps[0].c->t[n].nfields; i++) {
226 			ocg->add_field(ocg, n, inps[0].c->t[n].fsym[i], inps[0].c->t[n].ftype[i]);
227 		}
228 
229 		/* If more than one file, must be merging or averaging between files  */
230 		if (ninps > 1) {
231 			/* Duplicate all of the data or first file to output file */
232 			if ((setel = (cgats_set_elem *)malloc(
233 			     sizeof(cgats_set_elem) * inps[0].c->t[n].nfields)) == NULL)
234 				error("Malloc failed!");
235 
236 			for (i = 0; i < inps[0].c->t[n].nsets; i++) {
237 				inps[0].c->get_setarr(inps[0].c, n, i, setel);
238 				ocg->add_setarr(ocg, n, setel);
239 			}
240 			free(setel);
241 		}
242 	}
243 
244 	/* Figure out the indexes of the device channels */
245 	if (inps[0].c->find_kword(inps[0].c, 0, "COLOR_REP") < 0) {
246 		warning("Input file '%s' doesn't contain keyword COLOR_REP", inps[0].name);
247 	} else {
248 		int ti;
249 		char *buf;
250 		char *outc;
251 		int nmask;
252 		char *bident;
253 
254 		if ((ti = inps[0].c->find_kword(inps[0].c, 0, "COLOR_REP")) < 0)
255 			error("Input file '%s' doesn't contain keyword COLOR_REP", inps[0].name);
256 
257 		if ((buf = strdup(inps[0].c->t[0].kdata[ti])) == NULL)
258 			error("Malloc failed");
259 
260 		/* Split COLOR_REP into device and PCS space */
261 		if ((outc = strchr(buf, '_')) == NULL)
262 			error("COLOR_REP '%s' invalid", inps[0].c->t[0].kdata[ti]);
263 		*outc++ = '\000';
264 
265 		if ((nmask = icx_char2inkmask(buf)) == 0) {
266 			error ("File '%s' keyword COLOR_REP has unknown device value '%s'",inps[0].name,buf);
267 		}
268 
269 		nchan = icx_noofinks(nmask);
270 		bident = icx_inkmask2char(nmask, 0);
271 
272 		/* Find device fields */
273 		for (j = 0; j < nchan; j++) {
274 			int ii, imask;
275 			char fname[100];
276 
277 			imask = icx_index2ink(nmask, j);
278 			sprintf(fname,"%s_%s",nmask == ICX_W || nmask == ICX_K ? "GRAY" : bident,
279 			                      icx_ink2char(imask));
280 
281 			if ((ii = inps[0].c->find_field(inps[0].c, 0, fname)) < 0)
282 				error ("Input file doesn't contain field %s",fname);
283 			if (inps[0].c->t[0].ftype[ii] != r_t)
284 				error ("Field %s is wrong type",fname);
285 			chix[j] = ii;
286 		}
287 	}
288 
289 	/* Figure out the indexes of the PCS channels, if any */
290 	{
291 		int npcs;
292 		char *fname[2][3] =  { { "LAB_L", "LAB_A", "LAB_B" },
293 		                       { "XYZ_X", "XYZ_Y", "XYZ_Z" } };
294 
295 		/* For Lab and XYZ */
296 		for (j = 0; j < 2; j++) {
297 			for (npcs = 0; npcs < 3; npcs++) {
298 				int ii;
299 
300 				if ((ii = inps[0].c->find_field(inps[0].c, 0, fname[j][npcs])) < 0)
301 					break;		/* Try next or give up */
302 
303 				if (inps[0].c->t[0].ftype[ii] != r_t)
304 					error ("Field %s is wrong type",fname[j][npcs]);
305 				pcsix[j][npcs] = ii;
306 			}
307 			if (npcs == 3)
308 				haspcs[j] = 1;
309 		}
310 	}
311 	if (!haspcs[0] && !haspcs[1])
312 		warning("No PCS fields found - hope that's OK!");
313 
314 	if (!domerge && verb) {
315 		printf("Averaging the following fields:");
316 		for (j = 0; j < inps[0].c->t[0].nfields; j++) {
317 			int jj;
318 
319 			/* Only real types */
320 			if (inps[0].c->t[0].ftype[j] != r_t) {
321 				continue;
322 			}
323 
324 			/* Not device channels */
325 			for (jj = 0; jj < nchan; jj++) {
326 				if (chix[jj] == j)
327 					break;
328 			}
329 			if (jj < nchan) {
330 				continue;
331 			}
332 
333 			printf(" %s",inps[0].c->t[0].fsym[j]);
334 		}
335 		printf("\n");
336 	}
337 
338 	/* Get ready to add more values to output, */
339 	/* for merging or averaging within one file. */
340 	if ((setel = (cgats_set_elem *)malloc(
341 	     sizeof(cgats_set_elem) * inps[0].c->t[0].nfields)) == NULL)
342 		error("Malloc failed!");
343 
344 	/* If averaging values within the one file */
345 	if (ninps == 1) {
346 		int *valdone;
347 		int npat;
348 		double *vlist;
349 		double (*v3list)[3] = NULL;
350 		int k, e;
351 		n = 0;		/* Output set index */
352 
353 		if ((valdone = (int *)calloc(inps[0].c->t[0].nsets, sizeof(int))) == NULL)
354 			error("Malloc failed!");
355 
356 		if ((vlist = (double *)calloc(inps[0].c->t[0].nsets, sizeof(double))) == NULL)
357 			error("Malloc failed!");
358 
359 		if (dogeom && (haspcs[0] || haspcs[1])) {
360 			if ((v3list = (double (*)[3])calloc(inps[0].c->t[0].nsets, 3 * sizeof(double))) == NULL)
361 				error("Malloc failed!");
362 		}
363 
364 		/* For each patch */
365 		for (i = 0; i < inps[0].c->t[0].nsets; i++) {
366 
367 			if (valdone[i])
368 				continue;
369 
370 			inps[0].c->get_setarr(inps[0].c, 0, i, setel);
371 			ocg->add_setarr(ocg, 0, setel);
372 
373 			/* For each non-device real field values */
374 			for (j = 0; j < inps[0].c->t[0].nfields; j++) {
375 				int jj;
376 
377 				/* Only real types */
378 				if (inps[0].c->t[0].ftype[j] != r_t)
379 					continue;
380 
381 				/* Not device channels */
382 				for (jj = 0; jj < nchan; jj++) {
383 					if (chix[jj] == j)
384 						break;
385 				}
386 				if (jj < nchan)
387 					continue;
388 
389 				/* Locate any patches (including starting patch) with matching device values */
390 				npat = 0;
391 				for (k = i; k < inps[0].c->t[0].nsets; k++) {
392 
393 					/* Check if the device values match */
394 					for (e = 0; e < nchan; e++) {
395 						double diff;
396 
397 						diff = *((double *)inps[0].c->t[0].fdata[i][chix[e]])
398 						     - *((double *)inps[0].c->t[0].fdata[k][chix[e]]);
399 
400 						if (fabs(diff) > 0.001) {
401 							break;
402 						}
403 					}
404 					if (e < nchan) {
405 						continue;
406 					}
407 
408 					vlist[npat++] = *((double *)inps[0].c->t[0].fdata[k][j]);
409 					valdone[k] = 1;
410 				}
411 				if (domedian)
412 					*((double *)ocg->t[0].fdata[n][j]) = median(vlist, npat);
413 				else
414 					*((double *)ocg->t[0].fdata[n][j]) = average(vlist, npat);
415 			}
416 
417 			/* Override per-component average/median if PCS Geometric Median */
418 			if (dogeom && (haspcs[0] || haspcs[1])) {
419 				double res[3];
420 
421 				/* For Lab and XYZ */
422 				for (j = 0; j < 2; j++) {
423 
424 					if (haspcs[j] == 0)
425 						continue;
426 
427 					/* Locate any patches (including starting patch) with matching device values */
428 					npat = 0;
429 					for (k = i; k < inps[0].c->t[0].nsets; k++) {
430 
431 						/* Check if the device values match */
432 						for (e = 0; e < nchan; e++) {
433 							double diff;
434 
435 							diff = *((double *)inps[0].c->t[0].fdata[i][chix[e]])
436 							     - *((double *)inps[0].c->t[0].fdata[k][chix[e]]);
437 
438 							if (fabs(diff) > 0.001) {
439 								break;
440 							}
441 						}
442 						if (e < nchan) {
443 							continue;
444 						}
445 
446 						v3list[npat][0] = *((double *)inps[0].c->t[0].fdata[k][pcsix[j][0]]);
447 						v3list[npat][1] = *((double *)inps[0].c->t[0].fdata[k][pcsix[j][1]]);
448 						v3list[npat][2] = *((double *)inps[0].c->t[0].fdata[k][pcsix[j][2]]);
449 
450 						if (j == 0 && dogeom == 3)		/* Lab and want XYZ */
451 							icmLab2XYZ(&icmD50_100, v3list[npat], v3list[npat]);
452 						else if (j == 1 && dogeom == 2)	/* XYZ and want Lab */
453 							icmXYZ2Lab(&icmD50_100, v3list[npat], v3list[npat]);
454 
455 						npat++;
456 					}
457 					geommed(res, v3list, npat);
458 
459 					if (j == 0 && dogeom == 3)
460 						icmXYZ2Lab(&icmD50_100, res, res);
461 					else if (j == 1 && dogeom == 2)
462 						icmLab2XYZ(&icmD50_100, res, res);
463 
464 					*((double *)ocg->t[0].fdata[n][pcsix[j][0]]) = res[0];
465 					*((double *)ocg->t[0].fdata[n][pcsix[j][1]]) = res[1];
466 					*((double *)ocg->t[0].fdata[n][pcsix[j][2]]) = res[2];
467 				}
468 			}
469 			n++;		/* One more output set */
470 		}
471 
472 		if (v3list != NULL)
473 			free(v3list);
474 		free(vlist);
475 		free(valdone);
476 
477 	/* Averaging patches between identical files, */
478 	/* or concatenating (merging) several files */
479 	} else {
480 
481 		/* Check/process all the other input files */
482 		for (n = 1; n < ninps; n++) {
483 
484 			/* Check all the fields match the first file */
485 			if (inps[0].c->t[0].nfields != inps[n].c->t[0].nfields)
486 				error ("File '%s' has %d fields, file '%s has %d",
487 				       inps[n].name, inps[n].c->t[0].nfields, inps[0].name, inps[0].c->t[0].nfields);
488 			for (j = 0; j < inps[0].c->t[0].nfields; j++) {
489 				if (inps[0].c->t[0].ftype[j] != inps[n].c->t[0].ftype[j])
490 					error ("File '%s' field no. %d named '%s' doesn't match file '%s' field '%s'",
491 					       inps[n].name, j, inps[n].c->t[0].fsym[j], inps[0].name, inps[0].c->t[0].fsym[j]);
492 			}
493 
494 			/* If merging, append all the values */
495 			if (domerge) {
496 				for (i = 0; i < inps[n].c->t[0].nsets; i++) {
497 					inps[n].c->get_setarr(inps[n].c, 0, i, setel);
498 					ocg->add_setarr(ocg, 0, setel);
499 				}
500 
501 			} else {	/* Averaging */
502 
503 				/* Check the number of patches matches the first file */
504 				if (inps[0].c->t[0].nsets != inps[n].c->t[0].nsets)
505 					error ("File '%s' has %d sets, file '%s has %d",
506 					       inps[n].name, inps[n].c->t[0].nsets, inps[0].name, inps[0].c->t[0].nsets);
507 				/* For all the patches: */
508 				for (i = 0; i < inps[n].c->t[0].nsets; i++) {
509 
510 					/* Check that the device values match the first file */
511 					for (j = 0; j < nchan; j++) {
512 						double diff;
513 						diff = *((double *)inps[0].c->t[0].fdata[i][chix[j]])
514 						     - *((double *)inps[n].c->t[0].fdata[i][chix[j]]);
515 
516 						if (fabs(diff) > 0.001)
517 							error ("File '%s' set %d has field '%s' value that differs from '%s'",
518 					       inps[n].name, i+1, inps[n].c->t[0].fsym[j], inps[0].name);
519 					}
520 				}
521 			}
522 		}
523 
524 		/* If averaging */
525 		if (!domerge) {
526 			int npat;
527 			double *vlist;
528 			double (*v3list)[3] = NULL;
529 
530 			if ((vlist = (double *)calloc(ninps, sizeof(double))) == NULL)
531 				error("Malloc failed!");
532 
533 			if (dogeom && (haspcs[0] || haspcs[1])) {
534 				if ((v3list = (double (*)[3])calloc(inps[0].c->t[0].nsets, 3 * sizeof(double))) == NULL)
535 					error("Malloc failed!");
536 			}
537 
538 			/* For all the non-device real field values */
539 			for (j = 0; j < inps[0].c->t[0].nfields; j++) {
540 				int jj;
541 
542 				/* Only real types */
543 				if (inps[0].c->t[0].ftype[j] != r_t)
544 					continue;
545 
546 				/* Not device channels */
547 				for (jj = 0; jj < nchan; jj++) {
548 					if (chix[jj] == j)
549 						break;
550 				}
551 				if (jj < nchan)
552 					continue;
553 
554 				/* For each patch */
555 				for (i = 0; i < inps[n].c->t[0].nsets; i++) {
556 
557 					/* For all input files */
558 					npat = 0;
559 					for (n = 0; n < ninps; n++) {
560 						vlist[npat++] = *((double *)inps[n].c->t[0].fdata[i][j]);
561 					}
562 
563 					if (domedian)
564 						*((double *)ocg->t[0].fdata[i][j]) = median(vlist, npat);
565 					else
566 						*((double *)ocg->t[0].fdata[i][j]) = average(vlist, npat);
567 				}
568 			}
569 
570 			/* Override per-component average/median if PCS Geometric Median */
571 			if (dogeom && (haspcs[0] || haspcs[1])) {
572 				double res[3];
573 
574 				/* For each patch */
575 				for (i = 0; i < inps[n].c->t[0].nsets; i++) {
576 
577 					/* For Lab and XYZ */
578 					for (j = 0; j < 2; j++) {
579 
580 						if (haspcs[j] == 0)
581 							continue;
582 
583 						/* For all input files */
584 						npat = 0;
585 						for (n = 0; n < ninps; n++) {
586 							v3list[npat][0] = *((double *)inps[n].c->t[0].fdata[i][pcsix[j][0]]);
587 							v3list[npat][1] = *((double *)inps[n].c->t[0].fdata[i][pcsix[j][1]]);
588 							v3list[npat][2] = *((double *)inps[n].c->t[0].fdata[i][pcsix[j][2]]);
589 
590 							if (j == 0 && dogeom == 3)		/* Lab and want XYZ */
591 								icmLab2XYZ(&icmD50_100, v3list[npat], v3list[npat]);
592 							else if (j == 1 && dogeom == 2)	/* XYZ and want Lab */
593 								icmXYZ2Lab(&icmD50_100, v3list[npat], v3list[npat]);
594 
595 							npat++;
596 						}
597 						geommed(res, v3list, npat);
598 
599 						if (j == 0 && dogeom == 3)
600 							icmXYZ2Lab(&icmD50_100, res, res);
601 						else if (j == 1 && dogeom == 2)
602 							icmLab2XYZ(&icmD50_100, res, res);
603 
604 						*((double *)ocg->t[0].fdata[i][pcsix[j][0]]) = res[0];
605 						*((double *)ocg->t[0].fdata[i][pcsix[j][1]]) = res[1];
606 						*((double *)ocg->t[0].fdata[i][pcsix[j][2]]) = res[2];
607 					}
608 				}
609 			}
610 
611 			if (v3list != NULL)
612 				free(v3list);
613 			free(vlist);
614 		}
615 	}
616 
617 	/* Write out the output and free the cgats * */
618 	for (n = 0; n <= ninps; n++) {
619 
620 		if (n >= ninps) {		/* If ouput file, write it */
621 			if (inps[n].c->write_name(inps[n].c, inps[ninps].name))
622 				error("CGATS file '%s' write error : %s",inps[n].name,inps[n].c->err);
623 		}
624 		inps[n].c->del(inps[n].c);
625 	}
626 
627 	free(setel);
628 	free(inps);
629 
630 	return 0;
631 }
632 
633 
average(double * vals,int nvals)634 static double average(double *vals, int nvals) {
635 	double rv;
636 	int i;
637 
638 	for (rv = 0.0, i = 0; i < nvals; i++)
639 		rv += vals[i];
640 
641 	if (nvals > 0)
642 		rv /= (double)nvals;
643 
644 	return rv;
645 }
646 
median(double * vals,int nvals)647 static double median(double *vals, int nvals) {
648 	if (nvals < 3)
649 		return average(vals, nvals);
650 
651 #define HEAP_COMPARE(A,B) (A < B)
652 	HEAPSORT(double,vals,nvals);
653 
654 	if ((nvals & 1) != 0)
655 		return vals[nvals/2];
656 	else
657 		return 0.5 * (vals[nvals/2] + vals[nvals/2-1]);
658 }
659 
660 /* Compute Geometric Median of PCS values */
661 /* using Weiszfeld's algorithm. */
geommed(double res[3],double vals[][3],int nvals)662 static void geommed(double res[3], double vals[][3], int nvals) {
663 	int i, j;
664 
665 	/* Start with mean value */
666 	icmSet3(res, 0.0);
667 	for (i = 0; i < nvals; i++)
668 		icmAdd3(res, res, vals[i]);
669 	icmScale3(res, res, 1.0/(double)nvals);
670 
671 //printf("\n~1 average = %f %f %f\n", res[0], res[1], res[2]);
672 
673 	/* Itterate to approach Geometric Mean */
674 	for (j = 0; j < 20; j++) {
675         double tv[3], tt;
676 		int k;
677 
678         icmSet3(tv, 0.0);
679 		tt = 0.0;
680 		for (k = i = 0; i < nvals; i++) {
681 			double norm = icmNorm33(vals[i], res);
682 			if (norm < 1e-6)
683 				continue;
684             tv[0] += vals[i][0]/norm;
685             tv[1] += vals[i][1]/norm;
686             tv[2] += vals[i][2]/norm;
687             tt += 1.0/norm;
688 			k++;
689 //printf("Norm = %f, tv = %f %f %f, tt = %f\n",norm, tv[0], tv[1], tv[2], tt);
690         }
691 		if (k > 0)
692 			icmScale3(res, tv, 1.0/tt);
693 //printf("~1 res = %f %f %f\n", res[0], res[1], res[2]);
694 	}
695 
696 //printf("~1 geomm = %f %f %f\n", res[0], res[1], res[2]);
697 }
698 
699 
700