UVES Pipeline Reference Manual  5.4.0
uves_utils_polynomial.c
1 /* *
2  * This file is part of the ESO UVES Pipeline *
3  * Copyright (C) 2004,2005 European Southern Observatory *
4  * *
5  * This library is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, 51 Franklin St, Fifth Floor, Boston, MA 02111-1307 USA *
18  * */
19 
20 /*
21  * $Author: amodigli $
22  * $Date: 2012-01-12 16:44:43 $
23  * $Revision: 1.68 $
24  * $Name: not supported by cvs2svn $
25  * $Log: not supported by cvs2svn $
26  * Revision 1.67 2011/12/08 14:03:32 amodigli
27  * Fix warnings with CPL6
28  *
29  * Revision 1.66 2010/09/24 09:32:08 amodigli
30  * put back QFITS dependency to fix problem spot by NRI on FIBER mode (with MIDAS calibs) data
31  *
32  * Revision 1.64 2007/09/11 17:08:49 amodigli
33  * mooved uves_polynomial_convert_from_plist_midas to uves_dfs
34  *
35  * Revision 1.63 2007/08/21 13:08:26 jmlarsen
36  * Removed irplib_access module, largely deprecated by CPL-4
37  *
38  * Revision 1.62 2007/06/20 15:34:50 jmlarsen
39  * Changed indentation
40  *
41  * Revision 1.61 2007/06/20 08:30:00 amodigli
42  * added index parameter to support FIBER mode lintab in uves_polynomial_convert_from_plist_midas
43  *
44  * Revision 1.60 2007/06/06 08:17:33 amodigli
45  * replace tab with 4 spaces
46  *
47  * Revision 1.59 2007/05/03 15:23:08 jmlarsen
48  * Removed dead code
49  *
50  * Revision 1.58 2007/05/03 15:18:29 jmlarsen
51  * Added function to add polynomials
52  *
53  * Revision 1.57 2007/04/27 07:21:51 jmlarsen
54  * Polyfit: Changed error code from ILLEGAL_INPUT to SINGULAR_MATRIX
55  *
56  * Revision 1.56 2007/04/24 12:50:29 jmlarsen
57  * Replaced cpl_propertylist -> uves_propertylist which is much faster
58  *
59  * Revision 1.55 2007/03/23 08:01:55 jmlarsen
60  * Fixed mixed code and declarations
61  *
62  * Revision 1.54 2007/03/19 15:10:03 jmlarsen
63  * Optimization in 2d fitting: do not call pow too often
64  *
65  * Revision 1.53 2007/03/13 15:35:11 jmlarsen
66  * Made a few time optimizations
67  *
68  * Revision 1.52 2007/03/05 10:20:49 jmlarsen
69  * Added uves_polynomial_delete_const()
70  *
71  * Revision 1.51 2007/01/15 08:48:51 jmlarsen
72  * Shortened lines
73  *
74  * Revision 1.50 2006/11/24 09:36:49 jmlarsen
75  * Workaround for slow uves_propertylist_get_size
76  *
77  * Revision 1.49 2006/11/15 15:02:15 jmlarsen
78  * Implemented const safe workarounds for CPL functions
79  *
80  * Revision 1.47 2006/11/15 14:04:08 jmlarsen
81  * Removed non-const version of parameterlist_get_first/last/next which is
82  * already in CPL, added const-safe wrapper, unwrapper and deallocator functions
83  *
84  * Revision 1.46 2006/11/13 14:23:55 jmlarsen
85  * Removed workarounds for CPL const bugs
86  *
87  * Revision 1.45 2006/11/06 15:19:42 jmlarsen
88  * Removed unused include directives
89  *
90  * Revision 1.44 2006/09/08 14:06:29 jmlarsen
91  * Removed profiling code
92  *
93  * Revision 1.43 2006/09/06 14:46:21 jmlarsen
94  * Added missing newline in uves_polynomial_dump()
95  *
96  * Revision 1.42 2006/08/17 14:11:25 jmlarsen
97  * Use assure_mem macro to check for memory allocation failure
98  *
99  * Revision 1.41 2006/08/17 13:56:53 jmlarsen
100  * Reduced max line length
101  *
102  * Revision 1.40 2006/07/03 13:27:52 jmlarsen
103  * Moved failing assertion to where it should be
104  *
105  * Revision 1.39 2006/06/01 14:43:17 jmlarsen
106  * Added missing documentation
107  *
108  * Revision 1.38 2006/05/05 13:59:03 jmlarsen
109  * Support fitting zero-degree polynomial
110  *
111  * Revision 1.37 2006/04/24 09:28:29 jmlarsen
112  * Added function to create zero-polynomial
113  *
114  * Revision 1.36 2006/03/27 09:41:37 jmlarsen
115  * Added timing markers
116  *
117  * Revision 1.35 2006/03/09 10:52:25 jmlarsen
118  * Renamed pow->power
119  *
120  * Revision 1.34 2006/03/03 13:54:11 jmlarsen
121  * Changed syntax of check macro
122  *
123  * Revision 1.33 2005/12/19 16:17:56 jmlarsen
124  * Replaced bool -> int
125  *
126  */
127 
128 #ifdef HAVE_CONFIG_H
129 # include <config.h>
130 #endif
131 
132 /*----------------------------------------------------------------------------*/
152 /*----------------------------------------------------------------------------*/
153 
154 /*-----------------------------------------------------------------------------
155  Defines
156  -----------------------------------------------------------------------------*/
157 
158 /*
159  * When storing a 2d polynomial in a table,
160  * these column names are used
161  */
162 #define COLUMN_ORDER1 "Order1"
163 #define COLUMN_ORDER2 "Order2"
164 #define COLUMN_COEFF "Coeff"
165 
168 /*-----------------------------------------------------------------------------
169  Includes
170  -----------------------------------------------------------------------------*/
171 #include <uves_utils_polynomial.h>
172 
173 #include <uves_utils.h>
174 #include <uves_utils_wrappers.h>
175 #include <uves_dump.h>
176 #include <uves_msg.h>
177 #include <uves_error.h>
178 
179 #include <cpl.h>
180 
181 /*-----------------------------------------------------------------------------
182  Typedefs
183  -----------------------------------------------------------------------------*/
186 struct _polynomial
187 {
189  cpl_polynomial *pol;
190 
192  cpl_vector *vec;
193  double *vec_data;
194 
195  int dimension; /* for efficiency */
196 
198  double *shift;
199 
201  double *scale;
202 };
203 
204 /*-----------------------------------------------------------------------------
205  Implementation
206  -----------------------------------------------------------------------------*/
207 /*----------------------------------------------------------------------------*/
218 /*----------------------------------------------------------------------------*/
219 polynomial *
220 uves_polynomial_new(const cpl_polynomial *pol)
221 {
222  polynomial *p = NULL;
223  int i;
224 
225  /* Test input */
226  assure(pol != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
227 
228  /* Allocate and initialize struct */
229  p = cpl_calloc(1, sizeof(polynomial)) ;
230  assure_mem( p );
231 
232  check( p->dimension = cpl_polynomial_get_dimension(pol), "Error reading dimension");
233 
234  /* Allocate vector */
235  p->vec = cpl_vector_new(p->dimension);
236  assure_mem( p->vec );
237  p->vec_data = cpl_vector_get_data(p->vec);
238 
239  /* Shifts are initialized to zero, scales to 1 */
240  p->shift = cpl_calloc(p->dimension + 1, sizeof(double));
241  assure_mem( p->shift );
242 
243  p->scale = cpl_malloc((p->dimension + 1) * sizeof(double));
244  assure_mem( p->scale );
245  for (i = 0; i <= p->dimension; i++)
246  p->scale[i] = 1.0;
247 
248  check( p->pol = cpl_polynomial_duplicate(pol), "Error copying polynomial");
249 
250  cleanup:
251  if (cpl_error_get_code() != CPL_ERROR_NONE)
253 
254  return p;
255 }
256 
257 /*----------------------------------------------------------------------------*/
265 /*----------------------------------------------------------------------------*/
266 polynomial *
268 {
269  polynomial *result = NULL;
270  cpl_polynomial *p = NULL;
271 
272  assure( dim >= 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dim);
273 
274  p = cpl_polynomial_new(dim);
275  assure_mem( p );
276 
277  result = uves_polynomial_new(p);
278  assure_mem( result );
279 
280  cleanup:
281  uves_free_polynomial(&p);
282 
283  return result;
284 }
285 
286 /*----------------------------------------------------------------------------*/
293 /*----------------------------------------------------------------------------*/
294 void
296 {
298 }
299 
300 /*----------------------------------------------------------------------------*/
307 /*----------------------------------------------------------------------------*/
308 void
310 {
311  if (*p == NULL) return;
312  cpl_polynomial_delete((*p)->pol);
313  cpl_vector_delete((*p)->vec);
314  cpl_free((*p)->shift);
315  cpl_free((*p)->scale);
316  uves_free(*p);
317  *p = NULL;
318  return;
319 }
320 /*----------------------------------------------------------------------------*/
326 /*----------------------------------------------------------------------------*/
327 int
329 {
330  int result = -1;
331  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
332 
333  result = cpl_polynomial_get_degree(p->pol);
334 
335  cleanup:
336  return result;
337 }
338 
339 /*----------------------------------------------------------------------------*/
345 /*----------------------------------------------------------------------------*/
346 polynomial *
348 {
349  polynomial *result = NULL;
350  int dimension;
351  int i;
352 
353  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
354  dimension = uves_polynomial_get_dimension(p);
355 
356  check( result = uves_polynomial_new(p->pol),
357  "Error allocating polynomial");
358 
359  for (i = 0; i <= dimension; i++)
360  {
361  result->shift[i] = p->shift[i];
362  result->scale[i] = p->scale[i];
363  }
364 
365  cleanup:
366  if (cpl_error_get_code() != CPL_ERROR_NONE)
367  {
368  uves_polynomial_delete(&result);
369  return NULL;
370  }
371 
372  return result;
373 }
374 
375 
376 /*----------------------------------------------------------------------------*/
387 /*----------------------------------------------------------------------------*/
388 cpl_table *
390 {
391  cpl_table *t = NULL; /* Result */
392  int degree;
393  int i, j, row;
394 
395  /* Check input */
396  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
397  assure( uves_polynomial_get_dimension(p) == 2,
398  CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2D");
399 
400  degree = cpl_polynomial_get_degree(p->pol);
401 
402  /* Allocate space for 3 shifts, 3 scale factors and all
403  coefficients */
404  t = cpl_table_new(3 + 3 + (degree + 1)*(degree + 2)/2);
405  cpl_table_new_column(t, COLUMN_ORDER1, CPL_TYPE_INT);
406  cpl_table_new_column(t, COLUMN_ORDER2, CPL_TYPE_INT);
407  cpl_table_new_column(t, COLUMN_COEFF , CPL_TYPE_DOUBLE);
408 
409  row = 0;
410 
411  /* First write the shifts, write non-garbage to coeff columns (which are not used) */
412  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
413  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
414  cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[0]); row++;
415 
416  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
417  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
418  cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[1]); row++;
419 
420  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
421  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
422  cpl_table_set_double(t, COLUMN_COEFF , row, p->shift[2]); row++;
423 
424  /* Then the scale factors */
425  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
426  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
427  cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[0]); row++;
428 
429  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
430  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
431  cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[1]); row++;
432 
433  cpl_table_set_int (t, COLUMN_ORDER1, row, -1);
434  cpl_table_set_int (t, COLUMN_ORDER2, row, -1);
435  cpl_table_set_double(t, COLUMN_COEFF, row, p->scale[2]); row++;
436 
437  /* And then write the coefficients */
438  for (i = 0; i <= degree; i++){
439  for (j = 0; j+i <= degree; j++){
440  double coeff;
441  cpl_size power[2];
442  power[0] = i;
443  power[1] = j;
444 
445  coeff = cpl_polynomial_get_coeff(p->pol, power);
446  cpl_table_set_int (t, COLUMN_ORDER1, row, power[0]);
447  cpl_table_set_int (t, COLUMN_ORDER2, row, power[1]);
448  cpl_table_set_double(t, COLUMN_COEFF , row, coeff);
449 
450  row++;
451  }
452  }
453 
454  cleanup:
455  return t;
456 }
457 
458 /*----------------------------------------------------------------------------*/
467 /*----------------------------------------------------------------------------*/
468 polynomial *
470 {
471  polynomial *p = NULL; /* Result */
472  cpl_polynomial *pol = NULL;
473  cpl_type type;
474  int i;
475 
476  /* Only 2d supported */
477  check( pol = cpl_polynomial_new(2), "Error initializing polynomial");
478 
479  /* Check table format */
480  assure(t != NULL, CPL_ERROR_NULL_INPUT, "Null table");
481  assure(cpl_table_has_column(t, COLUMN_ORDER1), CPL_ERROR_ILLEGAL_INPUT,
482  "No '%s' column found in table", COLUMN_ORDER1);
483  assure(cpl_table_has_column(t, COLUMN_ORDER2), CPL_ERROR_ILLEGAL_INPUT,
484  "No '%s' column found in table", COLUMN_ORDER2);
485  assure(cpl_table_has_column(t, COLUMN_COEFF ), CPL_ERROR_ILLEGAL_INPUT,
486  "No '%s' column found in table", COLUMN_COEFF );
487 
488  type = cpl_table_get_column_type(t, COLUMN_ORDER1);
489  assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
490  "Column '%s' has type %s. Integer expected", COLUMN_ORDER1,
491  uves_tostring_cpl_type(type));
492 
493  type = cpl_table_get_column_type(t, COLUMN_ORDER2);
494  assure(type == CPL_TYPE_INT , CPL_ERROR_INVALID_TYPE,
495  "Column '%s' has type %s. Integer expected", COLUMN_ORDER2,
496  uves_tostring_cpl_type(type));
497 
498  type = cpl_table_get_column_type(t, COLUMN_COEFF);
499  assure(type == CPL_TYPE_DOUBLE, CPL_ERROR_INVALID_TYPE,
500  "Column '%s' has type %s. Double expected", COLUMN_COEFF ,
501  uves_tostring_cpl_type(type));
502 
503  assure(cpl_table_get_nrow(t) > 1 + 2 + 1 + 2, CPL_ERROR_ILLEGAL_INPUT,
504  "Table must contain at least one coefficient");
505 
506  /* Read the coefficients */
507  for(i = 3 + 3; i < cpl_table_get_nrow(t); i++) {
508  double coeff;
509  cpl_size power[2];
510 
511  check(( power[0] = cpl_table_get_int(t, COLUMN_ORDER1, i, NULL),
512  power[1] = cpl_table_get_int(t, COLUMN_ORDER2, i, NULL),
513  coeff = cpl_table_get_double(t, COLUMN_COEFF , i, NULL)),
514  "Error reading table row %d", i);
515 
516  uves_msg_debug("Pol.coeff.(%" CPL_SIZE_FORMAT ", %" CPL_SIZE_FORMAT ") = %e", power[0], power[1], coeff);
517 
518  check( cpl_polynomial_set_coeff(pol, power, coeff), "Error creating polynomial");
519  }
520  p = uves_polynomial_new(pol);
521 
522  /* Read shifts and rescaling */
523  uves_polynomial_rescale(p, 0, cpl_table_get_double( t, COLUMN_COEFF, 3, NULL));
524  uves_polynomial_rescale(p, 1, cpl_table_get_double( t, COLUMN_COEFF, 4, NULL));
525  uves_polynomial_rescale(p, 2, cpl_table_get_double( t, COLUMN_COEFF, 5, NULL));
526  uves_polynomial_shift (p, 0, cpl_table_get_double( t, COLUMN_COEFF, 0, NULL));
527  uves_polynomial_shift (p, 1, cpl_table_get_double( t, COLUMN_COEFF, 1, NULL));
528  uves_polynomial_shift (p, 2, cpl_table_get_double( t, COLUMN_COEFF, 2, NULL));
529 
530  cleanup:
531  uves_free_polynomial(&pol);
532  if (cpl_error_get_code() != CPL_ERROR_NONE)
534 
535  return p;
536 }
537 
538 
539 /*----------------------------------------------------------------------------*/
545 /*----------------------------------------------------------------------------*/
546 int
548 {
549  int dim = -1;
550  assure(p != NULL, CPL_ERROR_ILLEGAL_INPUT, "Null polynomial");
551 
552 /* slow check( dim = cpl_polynomial_get_dimension(p->pol), "Error reading dimension"); */
553  dim = p->dimension;
554 
555  cleanup:
556  return dim;
557 }
558 
559 /*----------------------------------------------------------------------------*/
567 /*----------------------------------------------------------------------------*/
568 void uves_polynomial_dump(const polynomial *p, FILE *stream)
569 {
570  if (p == NULL)
571  fprintf(stream, "Null polynomial\n");
572  else {
573  int i;
574  cpl_polynomial_dump(p->pol, stream);
575  fprintf(stream, "shift_y \t= %f \tscale_y \t= %f\n", p->shift[0], p->scale[0]);
576  for (i = 1; i <= uves_polynomial_get_dimension(p); i++)
577  {
578  fprintf(stream, "shift_x%d \t= %f \tscale_x%d \t= %f\n",
579  i, p->shift[i], i, p->scale[i]);
580  }
581  }
582  return;
583 }
584 
585 /*----------------------------------------------------------------------------*/
599 /*----------------------------------------------------------------------------*/
600 cpl_error_code
601 uves_polynomial_rescale(polynomial *p, int varno, double scale)
602 {
603  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
604  assure(0 <= varno && varno <= uves_polynomial_get_dimension(p),
605  CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
606 
607  /* Rescaling an x variable by the factor S corresponds to:
608  * p'(x) := p(x/S) =
609  * cpl( (x/S - shiftx ) / scalex ) * scaley + shifty =
610  * cpl( (x - (S*shiftx)) / (S*scalex) ) * scaley + shifty */
611 
612  /* Rescaling the y variable by the factor S corresponds to:
613  * p'(x) := S*p(x) =
614  * S * ( cpl((x - shiftx)/scalex) * scaley + shifty ) =
615  * cpl((x - shiftx)/scalex) * (S*scaley) + (S*shifty)
616  *
617  * therefore the implementation is the same in the two cases. */
618 
619  p->shift[varno] *= scale;
620  p->scale[varno] *= scale;
621 
622  cleanup:
623  return cpl_error_get_code();
624 }
625 
626 /*----------------------------------------------------------------------------*/
640 /*----------------------------------------------------------------------------*/
641 cpl_error_code
642 uves_polynomial_shift(polynomial *p, int varno, double shift)
643 {
644  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
645  assure(0 <= varno && varno <= uves_polynomial_get_dimension(p),
646  CPL_ERROR_ILLEGAL_INPUT, "Illegal variable number: %d", varno);
647 
648  /* The implementation is similar for x and y variables because
649  * p(x-S) =
650  * cpl( (x-S - shiftx) / scalex ) * scaley + shifty =
651  * cpl( (x - (shiftx+S)) / scalex ) * scaley + shifty
652  * and
653  * p(x) + S =
654  * cpl( (x - shiftx)/scalex ) * scaley + shifty + S =
655  * cpl( (x - shiftx)/scalex ) * scaley + (shifty+S) */
656 
657  p->shift[varno] += shift;
658 
659  cleanup:
660  return cpl_error_get_code();
661 }
662 
663 /*----------------------------------------------------------------------------*/
672 /*----------------------------------------------------------------------------*/
673 double
675 {
676  double result = 0;
677 
678  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
679  assure(uves_polynomial_get_dimension(p) == 1,
680  CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
681 
682  check( result =
683  cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], NULL)
684  * p->scale[0] + p->shift[0],
685  "Could not evaluate polynomial");
686 
687  cleanup:
688  return result;
689 }
690 
691 
692 /*----------------------------------------------------------------------------*/
702 /*----------------------------------------------------------------------------*/
703 
704 double
705 uves_polynomial_evaluate_2d(const polynomial *p, double x1, double x2)
706 {
707  double result = 0;
708 
709  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
710  assure(p->dimension == 2, CPL_ERROR_ILLEGAL_INPUT,
711  "Polynomial must be 2d. It's %dd", p->dimension);
712  {
713  double scale = p->scale[0];
714  double shift = p->shift[0];
715 
716  // cpl_vector_set(p->vec, 0, (x1 - p->shift[1]) / p->scale[1]);
717  // cpl_vector_set(p->vec, 1, (x2 - p->shift[2]) / p->scale[2]);
718  p->vec_data[0] = (x1 - p->shift[1]) / p->scale[1];
719  p->vec_data[1] = (x2 - p->shift[2]) / p->scale[2];
720 
721  result = cpl_polynomial_eval(p->pol, p->vec) * scale + shift;
722  }
723 
724  cleanup:
725  return result;
726 }
727 
728 /*----------------------------------------------------------------------------*/
741 /*----------------------------------------------------------------------------*/
742 double
743 uves_polynomial_solve_1d(const polynomial *p, double value, double guess, int multiplicity)
744 {
745  double result = 0;
746  cpl_size power[1];
747  double coeff0;
748 
749  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
750  assure(uves_polynomial_get_dimension(p) == 1, CPL_ERROR_ILLEGAL_INPUT,
751  "Polynomial must be 1d");
752 
753  /* Solving p(x) = value corresponds to solving
754  <=> cpl_p( (x-xshift)/xscale )*yscale + yshift = value
755  <=> cpl_p( (x-xshift)/xscale ) + (yshift - value)/yscale = 0
756 
757  So 1) find zero point for the polynomial cpl() + (yshift-value)/yscale
758  Then 2) shift and rescale the result
759  */
760 
761  power[0] = 0;
762  check(( coeff0 = cpl_polynomial_get_coeff(p->pol, power),
763  cpl_polynomial_set_coeff(p->pol, power, coeff0 + (p->shift[0] - value)/p->scale[0])),
764  "Error setting coefficient");
765 
766  check( cpl_polynomial_solve_1d(p->pol, (guess - p->shift[1]) / p->scale[1],
767  &result, multiplicity), "Could not find root");
768  /* Restore polynomial */
769  cpl_polynomial_set_coeff(p->pol, power, coeff0);
770 
771  /* Shift solution */
772  result = result * p->scale[1] + p->shift[1];
773 
774  cleanup:
775  return result;
776 }
777 
778 /*----------------------------------------------------------------------------*/
795 /*----------------------------------------------------------------------------*/
796 double
797 uves_polynomial_solve_2d(const polynomial *p, double value, double guess,
798  int multiplicity, int varno, double x_value)
799 {
800  double result = 0;
801  polynomial *pol_1d = NULL;
802 
803  assure( 1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
804  "Illegal variable number: %d", varno);
805 
806  check( pol_1d = uves_polynomial_collapse(p, varno, x_value),
807  "Could not collapse polynomial");
808 
809  check( result = uves_polynomial_solve_1d(pol_1d, value, guess, multiplicity),
810  "Could not find root");
811 
812  cleanup:
813  uves_polynomial_delete(&pol_1d);
814  return result;
815 }
816 
817 /*----------------------------------------------------------------------------*/
826 /*----------------------------------------------------------------------------*/
827 double
828 uves_polynomial_derivative_2d(const polynomial *p, double x1, double x2, int varno)
829 {
830  double result = 0;
831  cpl_size power[2];
832 
833  assure (1 <= varno && varno <= 2, CPL_ERROR_ILLEGAL_INPUT,
834  "Illegal variable number (%d)", varno);
835 
836  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
837  assure(uves_polynomial_get_dimension(p) == 2, CPL_ERROR_ILLEGAL_INPUT,
838  "Polynomial must be 2d. It's %dd", uves_polynomial_get_dimension(p));
839 
840  /* d/dx_i [ p(x) ] =
841  * d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] =
842  * [ d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley ]
843  */
844 
845  /* Shift, scale (x1, x2) */
846  x1 = (x1 - p->shift[1])/p->scale[1];
847  x2 = (x2 - p->shift[2])/p->scale[2];
848 
849  /* Get derivative of cpl polynomial.
850  *
851  */
852  {
853  int degree = cpl_polynomial_get_degree(p->pol);
854  double yj = 1; /* y^j */
855  int i, j;
856 
857  result = 0;
858  for (j = 0, yj = 1;
859  j <= degree; j++,
860  yj *= (varno == 1) ? x2 : x1)
861  {
862  /* Proof by example (degree = 3): For each j account for these terms
863  * using Horner's rule:
864  *
865  * d/dx y^j * [ c_3j x^3 + c_2j x^2 + c_1j x^1 + c_0j ] =
866  *
867  * y^j * [ 3c_3j x^2 + 2c_2j x^1 + 1c_1j ] =
868  *
869  * y^j * [ ((3c_3j) x + 2c_2j) x + 1c_1j ]
870  */
871 
872  double sum = 0;
873  for (i = degree; i >= 1; i--)
874  {
875  double c_ij;
876 
877  power[0] = (varno == 1) ? i : j;
878  power[1] = (varno == 1) ? j : i;
879 
880  c_ij = cpl_polynomial_get_coeff(p->pol, power);
881 
882  sum += (i * c_ij);
883  if (i >= 2) sum *= (varno == 1) ? x1 : x2;
884  }
885 
886  /* Collect terms */
887  result += yj * sum;
888  }
889  }
890 
891  result *= p->scale[0];
892 
893 
894 /* Old code: This method (valid for varno = 2)
895  of getting the derivative of
896  the CPL polynomial is slow because of the call to
897  uves_polynomial_collapse()
898 
899  check( pol_1d = uves_polynomial_collapse(p, 1, x1);
900  dummy = cpl_polynomial_eval_1d(pol_1d->pol, (x2 - pol_1d->shift[1])/pol_1d->scale[1], &result),
901  "Error evaluating derivative");
902 */
903 
904  cleanup:
905  return result;
906 }
907 
908 /*----------------------------------------------------------------------------*/
915 /*----------------------------------------------------------------------------*/
916 double
918 {
919  double result = 0;
920  double dummy;
921 
922  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
923  assure(uves_polynomial_get_dimension(p) == 1,
924  CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 1d");
925 
926  check( dummy = cpl_polynomial_eval_1d(p->pol, (x - p->shift[1])/p->scale[1], &result),
927  "Error evaluating derivative");
928 
929  cleanup:
930  return result;
931 }
932 
933 /*----------------------------------------------------------------------------*/
940 /*----------------------------------------------------------------------------*/
941 polynomial *
943 {
944  polynomial *result = NULL;
945  cpl_polynomial *pol = NULL;
946 
947  assure(p1 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
948  assure(p2 != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
949  assure(uves_polynomial_get_dimension(p1) == 2,
950  CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
951  assure(uves_polynomial_get_dimension(p2) == 2,
952  CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
953 
954  /* cpl_polynomial1((x - shift_x1)/scale_x1) * scale_y1 + shift_y1
955  +
956  cpl_polynomial2((x - shift_x2)/scale_x2) * scale_y2 + shift_y2
957  = ???
958  Not easy.
959 
960  Use brute force:
961  */
962 
963  {
964  int degree, i, j;
965 
966  degree = uves_max_int(uves_polynomial_get_degree(p1),
968 
969  pol = cpl_polynomial_new(2);
970  for (i = 0; i <= degree; i++)
971  for (j = 0; j <= degree; j++) {
972  double coeff1, coeff2;
973  cpl_size power[2];
974 
975  /* Simple: add coefficients of the same power */
976  coeff1 = uves_polynomial_get_coeff_2d(p1, i, j);
977  coeff2 = uves_polynomial_get_coeff_2d(p2, i, j);
978 
979  power[0] = i;
980  power[1] = j;
981  cpl_polynomial_set_coeff(pol, power, coeff1 + coeff2);
982  }
983  }
984 
985  result = uves_polynomial_new(pol);
986 
987  cleanup:
988  uves_free_polynomial(&pol);
989  return result;
990 }
991 
992 /*----------------------------------------------------------------------------*/
1005 /*----------------------------------------------------------------------------*/
1006 static cpl_error_code
1007 derivative_cpl_polynomial(cpl_polynomial *p, int varno)
1008 {
1009  int dimension, degree;
1010  int i, j;
1011  cpl_size power[2];
1012 
1013  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
1014  dimension = cpl_polynomial_get_dimension(p);
1015  degree = cpl_polynomial_get_degree(p);
1016  assure( 1 <= dimension && dimension <= 2, CPL_ERROR_ILLEGAL_INPUT,
1017  "Illegal dimension: %d", dimension);
1018  assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
1019  "Illegal variable number: %d", varno);
1020 
1021  if (dimension == 1)
1022  {
1023  /* a_i := (i+1) * a_(i+1) */
1024  for(i = 0; i <= degree; i++)
1025  {
1026  double coeff;
1027  power[0] = i+1;
1028  /* power[1] is ignored */
1029 
1030  coeff = cpl_polynomial_get_coeff(p, power);
1031 
1032  power[0] = i;
1033  cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
1034  }
1035  }
1036 
1037  if (dimension == 2)
1038  {
1039  /* a_ij := (i+1) * a_{(i+1),j} */
1040  for(i = 0; i <= degree; i++)
1041  {
1042  for(j = 0; i + j <= degree; j++)
1043  {
1044  double coeff;
1045  power[varno - 1] = i+1; /* varno == 1: 0,1 */
1046  power[2 - varno] = j; /* varno == 2: 1,0 */
1047 
1048  coeff = cpl_polynomial_get_coeff(p, power);
1049 
1050  power[varno - 1] = i;
1051 
1052  cpl_polynomial_set_coeff(p, power, (i+1) * coeff);
1053  }
1054  }
1055  }
1056 
1057  cleanup:
1058  return cpl_error_get_code();
1059 }
1060 
1061 /*----------------------------------------------------------------------------*/
1071 /*----------------------------------------------------------------------------*/
1072 cpl_error_code
1074 {
1075  int dimension;
1076 
1077  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
1078  check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
1079  assure( 1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
1080  "Illegal variable number: %d", varno);
1081 
1082 
1083  /* d/dx_i [ cpl( (x - shiftx) / scalex ) * scaley + shifty ] =
1084  * sum_j d(cpl)/dx_j ( (x - shiftx) / scalex ) * scaley * dx_j/dx_i / scalex_j =
1085  * d(cpl)/dx_i ( (x - shiftx) / scalex ) * scaley/scalex_i,
1086  *
1087  * so transform : shifty -> 0
1088  * shiftx -> shiftx
1089  * scaley -> scaley/scalex_i
1090  * scalex -> scalex
1091  * cpl -> d(cpl)/dx_i
1092  */
1093 
1094  p->shift[0] = 0;
1095  p->scale[0] = p->scale[0] / p->scale[varno];
1096 
1097  check( derivative_cpl_polynomial(p->pol, varno),
1098  "Error calculating derivative of CPL-polynomial");
1099 
1100  cleanup:
1101  return cpl_error_get_code();
1102 }
1103 
1104 
1105 /*----------------------------------------------------------------------------*/
1114 /*----------------------------------------------------------------------------*/
1115 double
1116 uves_polynomial_get_coeff_2d(const polynomial *p, int degree1, int degree2)
1117 {
1118  polynomial *pp = NULL;
1119  int dimension;
1120  double result = 0;
1121  double factorial;
1122 
1123  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
1124  check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
1125  assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
1126  assure( 0 <= degree1, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree1);
1127  assure( 0 <= degree2, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree2);
1128 
1129  /* Calculate the coefficient as
1130  * d^N p / (dx1^degree1 dx2^degree2) / (degree1! * degree2!)
1131  * evaluated in (0,0)
1132  */
1133 
1134  pp = uves_polynomial_duplicate(p);
1135 
1136  factorial = 1;
1137  while(degree1 > 0)
1138  {
1139  check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
1140 
1141  factorial *= degree1;
1142  degree1 -= 1;
1143  }
1144 
1145  while(degree2 > 0)
1146  {
1147  check( uves_polynomial_derivative(pp, 2), "Error calculating derivative");
1148 
1149  factorial *= degree2;
1150  degree2 -= 1;
1151  }
1152 
1153  check( result = uves_polynomial_evaluate_2d(pp, 0, 0) / factorial,
1154  "Error evaluating polynomial");
1155 
1156  cleanup:
1158  return result;
1159 }
1160 /*----------------------------------------------------------------------------*/
1170 /*----------------------------------------------------------------------------*/
1171 double
1173 {
1174  polynomial *pp = NULL;
1175  int dimension;
1176  double result = 0;
1177  double factorial;
1178 
1179  assure( p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
1180  check ( dimension = uves_polynomial_get_dimension(p), "Error reading dimension");
1181  assure(dimension == 1, CPL_ERROR_ILLEGAL_INPUT, "Illegal dimension: %d", dimension);
1182  assure( 0 <= degree, CPL_ERROR_ILLEGAL_INPUT, "Illegal degree: %d", degree);
1183 
1184  /* Calculate the coefficient as
1185  * d^degree p/dx^degree / (degree1! * degree2!)
1186  * evaluated in 0.
1187  */
1188 
1189  pp = uves_polynomial_duplicate(p);
1190 
1191  factorial = 1;
1192  while(degree > 0)
1193  {
1194  check( uves_polynomial_derivative(pp, 1), "Error calculating derivative");
1195 
1196  factorial *= degree;
1197  degree -= 1;
1198  }
1199 
1200  check( result = uves_polynomial_evaluate_1d(pp, 0) / factorial,
1201  "Error evaluating polynomial");
1202 
1203  cleanup:
1205  return result;
1206 }
1207 
1208 
1209 /*----------------------------------------------------------------------------*/
1225 /*----------------------------------------------------------------------------*/
1226 polynomial *
1227 uves_polynomial_collapse(const polynomial *p, int varno, double value)
1228 {
1229  polynomial *result = NULL;
1230  cpl_polynomial *pol = NULL;
1231  cpl_size *power = NULL;
1232 
1233  int i, j;
1234  int degree, dimension;
1235 
1236  assure(p != NULL, CPL_ERROR_NULL_INPUT, "Null polynomial");
1237  dimension = uves_polynomial_get_dimension(p);
1238  assure(dimension > 0, CPL_ERROR_ILLEGAL_INPUT,
1239  "Polynomial has non-positive dimension: %d", dimension);
1240  assure(dimension != 1, CPL_ERROR_ILLEGAL_OUTPUT,
1241  "Don't collapse a 1d polynomial. Evaluate it!");
1242 
1243  /* To generalize this function to work with dimensions higher than 2,
1244  also changes needs to be made below (use varno properly). For now,
1245  support only 2d. */
1246  assure(dimension == 2, CPL_ERROR_ILLEGAL_INPUT, "Polynomial must be 2d");
1247 
1248  assure(1 <= varno && varno <= dimension, CPL_ERROR_ILLEGAL_INPUT,
1249  "Wrong variable number");
1250  value = (value - p->shift[varno]) / p->scale[varno];
1251 
1252  /* Compute new coefficients */
1253  degree = cpl_polynomial_get_degree(p->pol);
1254  pol = cpl_polynomial_new(dimension - 1);
1255  power = cpl_malloc(sizeof(cpl_size) * dimension);
1256  assure_mem( power );
1257  for (i = 0; i <= degree; i++)
1258  {
1259  double coeff;
1260 
1261  power[2-varno] = i; /* map 2->0 and 1->1 */
1262 
1263  /* Collect all terms with x^i (using Horner's rule) */
1264  coeff = 0;
1265  for (j = degree - i; j >= 0; j--)
1266  {
1267  power[varno-1] = j; /* map 2->1 and 1->0 */
1268  coeff += cpl_polynomial_get_coeff(p->pol, power);
1269  if (j > 0) coeff *= value;
1270  }
1271  /* Write coefficient in 1d polynomial */
1272  power[0] = i;
1273  cpl_polynomial_set_coeff(pol, power, coeff);
1274  }
1275 
1276  /* Wrap the polynomial */
1277  result = uves_polynomial_new(pol);
1278 
1279  /* Copy the shifts and scales, skip variable number varno */
1280  j = 0;
1281  for(i = 0; i <= dimension - 1; i++)
1282  {
1283  if (i == varno)
1284  {
1285  /* Don't copy */
1286  j += 2;
1287  /* For the remainder of this for loop, j = i+1 */
1288  }
1289  else
1290  {
1291  result->shift[i] = p->shift[j];
1292  result->scale[i] = p->scale[j];
1293  j += 1;
1294  }
1295  }
1296 
1297  assure(cpl_error_get_code() == CPL_ERROR_NONE, cpl_error_get_code(),
1298  "Error collapsing polynomial");
1299 
1300  cleanup:
1301  cpl_free(power); power = NULL;
1302  uves_free_polynomial(&pol);
1303  if (cpl_error_get_code() != CPL_ERROR_NONE)
1304  {
1305  uves_polynomial_delete(&result);
1306  }
1307  return result;
1308 }
1309 
1310 
1311 
1312 /*----------------------------------------------------------------------------*/
1332 /*----------------------------------------------------------------------------*/
1334  const cpl_vector * x_pos,
1335  const cpl_vector * values,
1336  const cpl_vector * sigmas,
1337  int poly_deg,
1338  double * mse)
1339 {
1340  int nc ;
1341  int np ;
1342  cpl_matrix * ma = NULL;
1343  cpl_matrix * mb = NULL;
1344  cpl_matrix * mx = NULL;
1345  const double * x_pos_data ;
1346  const double * values_data ;
1347  const double * sigmas_data = NULL;
1348  double mean_x, mean_z;
1349  polynomial * result = NULL;
1350  cpl_polynomial * out ;
1351  cpl_vector * x_val = NULL;
1352  int i, j ;
1353 
1354  /* Check entries */
1355  assure_nomsg( x_pos != NULL && values != NULL, CPL_ERROR_NULL_INPUT);
1356  assure( poly_deg >= 0, CPL_ERROR_ILLEGAL_INPUT,
1357  "Polynomial degree is %d. Must be non-negative", poly_deg);
1358  np = cpl_vector_get_size(x_pos) ;
1359 
1360  nc = 1 + poly_deg ;
1361  assure( np >= nc, CPL_ERROR_ILLEGAL_INPUT,
1362  "Not enough points (%d) to fit %d-order polynomial. %d point(s) needed",
1363  np, poly_deg, nc);
1364 
1365  /* Fill up look-up table for coefficients to compute */
1366  /* Initialize matrices */
1367  /* ma contains the polynomial terms for each input point. */
1368  /* mb contains the values */
1369  ma = cpl_matrix_new(np, nc) ;
1370  mb = cpl_matrix_new(np, 1) ;
1371 
1372  /* Get mean values */
1373  mean_x = cpl_vector_get_mean(x_pos);
1374  mean_z = cpl_vector_get_mean(values);
1375 
1376  /* Fill up matrices, shift */
1377  x_pos_data = cpl_vector_get_data_const(x_pos) ;
1378  values_data = cpl_vector_get_data_const(values) ;
1379  if (sigmas != NULL)
1380  {
1381  sigmas_data = cpl_vector_get_data_const(sigmas) ;
1382  }
1383 
1384  if (sigmas != NULL)
1385  {
1386  for (i=0 ; i<np ; i++)
1387  {
1388  /* Catch division by zero */
1389  if (sigmas_data[i] == 0)
1390  {
1391  uves_free_matrix(&ma) ;
1392  uves_free_matrix(&mb) ;
1393  assure(false, CPL_ERROR_DIVISION_BY_ZERO,
1394  "Sigmas must be non-zero");
1395  }
1396  for (j=0 ; j<nc ; j++)
1397  {
1398  cpl_matrix_set(ma, i, j,
1399  uves_pow_int(x_pos_data[i] - mean_x, j) /
1400  sigmas_data[i]) ;
1401  }
1402  /* mb contains surface values (z-axis) */
1403  cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / sigmas_data[i]);
1404  }
1405  }
1406  else /* Use sigma = 1 */
1407  {
1408  for (i=0 ; i<np ; i++)
1409  {
1410  for (j=0 ; j<nc ; j++)
1411  {
1412  cpl_matrix_set(ma, i, j,
1413  uves_pow_int(x_pos_data[i] - mean_x, j) / 1);
1414  }
1415  /* mb contains surface values (z-values) */
1416  cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
1417  }
1418  }
1419 
1420  /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
1421  check( mx = cpl_matrix_solve_normal(ma, mb),
1422  "Could not invert matrix");
1423  uves_free_matrix(&ma);
1424  uves_free_matrix(&mb);
1425 
1426  /* Store coefficients for output */
1427  out = cpl_polynomial_new(1) ;
1428  cpl_size deg=0;
1429  for (deg=0 ; deg<nc ; deg++) {
1430  cpl_polynomial_set_coeff(out, &deg, cpl_matrix_get(mx, deg, 0)) ;
1431  }
1432  uves_free_matrix(&mx);
1433 
1434  /* If requested, compute mean squared error */
1435  if (mse != NULL) {
1436  *mse = 0.00 ;
1437  x_val = cpl_vector_new(1) ;
1438  for (i=0 ; i<np ; i++)
1439  {
1440  double residual;
1441  cpl_vector_set(x_val, 0, x_pos_data[i] - mean_x) ;
1442  /* Subtract from the true value, square, accumulate */
1443  residual = (values_data[i] - mean_z) - cpl_polynomial_eval(out, x_val);
1444  *mse += residual*residual;
1445  }
1446  uves_free_vector(&x_val) ;
1447  /* Average the error term */
1448  *mse /= (double)np ;
1449  }
1450 
1451  /* Create and shift result */
1452  result = uves_polynomial_new(out);
1453  uves_free_polynomial(&out);
1454 
1455  uves_polynomial_shift(result, 0, mean_z);
1456  uves_polynomial_shift(result, 1, mean_x);
1457 
1458  cleanup:
1459  uves_free_vector(&x_val);
1460  uves_free_matrix(&ma);
1461  uves_free_matrix(&mb);
1462  uves_free_matrix(&mx);
1463  return result;
1464 }
1465 
1466 
1467 /*----------------------------------------------------------------------------*/
1511 /*----------------------------------------------------------------------------*/
1512 polynomial *
1514  const cpl_bivector * xy_pos,
1515  const cpl_vector * values,
1516  const cpl_vector * sigmas,
1517  int poly_deg1,
1518  int poly_deg2,
1519  double * mse,
1520  double * red_chisq,
1521  polynomial ** variance)
1522 {
1523  int nc ;
1524  int degx, degy ;
1525  int * degx_tab ;
1526  int * degy_tab ;
1527  int np ;
1528  cpl_matrix * ma ;
1529  cpl_matrix * mb ;
1530  cpl_matrix * mx ;
1531  cpl_matrix * mat;
1532  cpl_matrix * mat_ma;
1533  cpl_matrix * cov = NULL;
1534  const double * xy_pos_data_x ;
1535  const double * xy_pos_data_y ;
1536  const double * values_data ;
1537  const double * sigmas_data = NULL;
1538  const cpl_vector* xy_pos_x;
1539  const cpl_vector* xy_pos_y;
1540  double mean_x, mean_y, mean_z;
1541  cpl_polynomial * out ;
1542  cpl_polynomial * variance_cpl ;
1543  polynomial * result = NULL;
1544  cpl_size * powers ;
1545 
1546  /* Check entries */
1547  assure(xy_pos && values, CPL_ERROR_NULL_INPUT, "Null input");
1548  assure(poly_deg1 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree1 is %d", poly_deg1);
1549  assure(poly_deg2 >= 0, CPL_ERROR_ILLEGAL_INPUT, "Polynomial degree2 is %d", poly_deg2);
1550  np = cpl_bivector_get_size(xy_pos) ;
1551 
1552  /* Can't calculate variance and chi_sq without sigmas */
1553  assure( (variance == NULL && red_chisq == NULL) || sigmas != NULL,
1554  CPL_ERROR_ILLEGAL_INPUT,
1555  "Cannot calculate variance or chi_sq without knowing");
1556 
1557  /* Fill up look-up table for coefficients to compute */
1558  nc = (1 + poly_deg1)*(1 + poly_deg2) ; /* rectangular matrix */
1559 
1560  assure(np >= nc, CPL_ERROR_SINGULAR_MATRIX, "%d coefficients. Only %d points", nc, np);
1561  /* The error code here is set to SINGULAR_MATRIX, in order to allow the caller
1562  to detect when too many coefficients are fitted to too few points */
1563 
1564  /* Need an extra point to calculate reduced chi^2 */
1565  assure(red_chisq == NULL || np > nc, CPL_ERROR_ILLEGAL_INPUT,
1566  "%d coefficients. %d points. Cannot calculate chi square", nc, np);
1567 
1568  degx_tab = cpl_malloc(nc * sizeof(int)) ;
1569  assure_mem( degx_tab );
1570 
1571  degy_tab = cpl_malloc(nc * sizeof(int)) ;
1572  if (degy_tab == NULL) {
1573  cpl_free(degx_tab);
1574  assure_mem( false );
1575  }
1576 
1577  {
1578  int i=0 ;
1579  for (degy=0 ; degy<=poly_deg2 ; degy++) { /* rectangular matrix */
1580  for (degx=0 ; degx<=poly_deg1 ; degx++) {
1581  degx_tab[i] = degx ;
1582  degy_tab[i] = degy ;
1583  i++ ;
1584  }
1585  }
1586  }
1587 
1588  /* Initialize matrices */
1589  /* ma contains the polynomial terms in the order described */
1590  /* above in each column, for each input point. */
1591  /* mb contains the values */
1592  ma = cpl_matrix_new(np, nc) ;
1593  mb = cpl_matrix_new(np, 1) ;
1594 
1595  /* Get the mean of each variable */
1596  xy_pos_x = cpl_bivector_get_x_const(xy_pos);
1597  xy_pos_y = cpl_bivector_get_y_const(xy_pos);
1598 
1599  mean_x = cpl_vector_get_mean(xy_pos_x);
1600  mean_y = cpl_vector_get_mean(xy_pos_y);
1601  mean_z = cpl_vector_get_mean(values);
1602 
1603  /* Fill up matrices. At the same time shift the data
1604  so that it is centered around zero */
1605  xy_pos_data_x = cpl_vector_get_data_const(xy_pos_x) ;
1606  xy_pos_data_y = cpl_vector_get_data_const(xy_pos_y) ;
1607  values_data = cpl_vector_get_data_const(values) ;
1608  if (sigmas != NULL)
1609  {
1610  sigmas_data = cpl_vector_get_data_const(sigmas) ;
1611  }
1612 
1613  if (sigmas != NULL)
1614  {
1615  int i;
1616  for (i=0 ; i<np ; i++) {
1617  double *ma_data = cpl_matrix_get_data(ma);
1618  double *mb_data = cpl_matrix_get_data(mb);
1619 
1620  int j = 0;
1621  double valy = 1;
1622 
1623  /* Catch division by zero */
1624  if (sigmas_data[i] == 0)
1625  {
1626  uves_free_matrix(&ma) ;
1627  uves_free_matrix(&mb) ;
1628  cpl_free(degx_tab) ;
1629  cpl_free(degy_tab) ;
1630  assure(false, CPL_ERROR_DIVISION_BY_ZERO,
1631  "Sigmas must be non-zero. sigma[%d] is %f", i, sigmas_data[i]);
1632  }
1633 
1634  for (degy=0 ; degy<=poly_deg2 ; degy++) {
1635  double valx = 1;
1636  for (degx=0 ; degx<=poly_deg1 ; degx++) {
1637  ma_data[j + i*nc] = valx * valy / sigmas_data[i];
1638  valx *= (xy_pos_data_x[i] - mean_x);
1639  j++;
1640  }
1641  valy *= (xy_pos_data_y[i] - mean_y);
1642  }
1643 
1644  /* mb contains surface values (z-axis) */
1645 
1646  mb_data[0 + i*1] = (values_data[i] - mean_z) / sigmas_data[i];
1647  }
1648  }
1649  else /* Use sigma = 1 */
1650  {
1651  int i;
1652  for (i=0 ; i<np ; i++) {
1653  double *ma_data = cpl_matrix_get_data(ma);
1654  double *mb_data = cpl_matrix_get_data(mb);
1655 
1656  double valy = 1;
1657  int j = 0;
1658  for (degy=0 ; degy<=poly_deg2 ; degy++) {
1659  double valx = 1;
1660  for (degx=0 ; degx<=poly_deg1 ; degx++) {
1661  ma_data[j + i*nc] = valx * valy / 1;
1662  valx *= (xy_pos_data_x[i] - mean_x);
1663  j++;
1664  }
1665  valy *= (xy_pos_data_y[i] - mean_y);
1666  }
1667 
1668  /* mb contains surface values (z-axis) */
1669 // cpl_matrix_set(mb, i, 0, (values_data[i] - mean_z) / 1) ;
1670  mb_data[0 + i*1] = (values_data[i] - mean_z) / 1;
1671  }
1672  }
1673 
1674  /* If variance polynomial is requested,
1675  compute covariance matrix = (A^T * A)^-1 */
1676  if (variance != NULL)
1677  {
1678  mat = cpl_matrix_transpose_create(ma);
1679  if (mat != NULL)
1680  {
1681  mat_ma = cpl_matrix_product_create(mat, ma);
1682  if (mat_ma != NULL)
1683  {
1684  cov = cpl_matrix_invert_create(mat_ma);
1685  /* Here, one might do a (paranoia) check that the covariance
1686  matrix is symmetrical and has positive eigenvalues (so that
1687  the returned variance polynomial is guaranteed to be positive) */
1688 
1689  variance_cpl = cpl_polynomial_new(2);
1690  }
1691  }
1692  uves_free_matrix(&mat);
1693  uves_free_matrix(&mat_ma);
1694  }
1695 
1696  /* Solve XA=B by a least-square solution (aka pseudo-inverse). */
1697  mx = cpl_matrix_solve_normal(ma, mb) ;
1698 
1699  uves_free_matrix(&ma) ;
1700  uves_free_matrix(&mb) ;
1701  if (mx == NULL) {
1702  cpl_free(degx_tab) ;
1703  cpl_free(degy_tab) ;
1704  uves_free_matrix(&cov) ;
1705  assure(false, CPL_ERROR_ILLEGAL_OUTPUT, "Matrix inversion failed") ;
1706  }
1707 
1708  /* Store coefficients for output */
1709  out = cpl_polynomial_new(2) ;
1710  powers = cpl_malloc(2 * sizeof(cpl_size)) ;
1711  if (powers == NULL) {
1712  cpl_free(degx_tab) ;
1713  cpl_free(degy_tab) ;
1714  uves_free_matrix(&mx) ;
1715  uves_free_matrix(&cov) ;
1716  uves_free_polynomial(&out) ;
1717  assure_mem( false );
1718  }
1719 
1720  {
1721  int i;
1722  for (i = 0 ; i < nc ; i++)
1723  {
1724  powers[0] = degx_tab[i] ;
1725  powers[1] = degy_tab[i] ;
1726  cpl_polynomial_set_coeff(out, powers, cpl_matrix_get(mx, i, 0)) ;
1727 
1728  /* Create variance polynomial (if requested) */
1729  if (variance != NULL && /* Requested? */
1730  cov != NULL && variance_cpl != NULL /* covariance computation succeeded? */
1731  )
1732  {
1733  int j;
1734  for (j = 0; j < nc; j++)
1735  {
1736  double coeff;
1737  /* Add cov_ij to the proper coeff:
1738  cov_ij * dp/d(ai) * dp/d(aj) =
1739  cov_ij * (x^degx[i] * y^degy[i]) * (x^degx[i] * y^degy[i]) =
1740  cov_ij * x^(degx[i]+degx[j]) * y^(degy[i] + degy[j]),
1741 
1742  i.e. add cov_ij to coeff (degx[i]+degx[j], degy[i]+degy[j]) */
1743  powers[0] = degx_tab[i] + degx_tab[j] ;
1744  powers[1] = degy_tab[i] + degy_tab[j] ;
1745 
1746  coeff = cpl_polynomial_get_coeff(variance_cpl, powers);
1747  cpl_polynomial_set_coeff(variance_cpl, powers,
1748  coeff + cpl_matrix_get(cov, i, j)) ;
1749  }
1750  }
1751  }
1752  }
1753 
1754  cpl_free(powers) ;
1755  cpl_free(degx_tab) ;
1756  cpl_free(degy_tab) ;
1757  uves_free_matrix(&cov) ;
1758  uves_free_matrix(&mx) ;
1759 
1760  /* Create and shift result */
1761  result = uves_polynomial_new(out);
1762  uves_free_polynomial(&out);
1763  uves_polynomial_shift(result, 0, mean_z);
1764  uves_polynomial_shift(result, 1, mean_x);
1765  uves_polynomial_shift(result, 2, mean_y);
1766 
1767  /* Wrap up variance polynomial */
1768  if (variance != NULL)
1769  {
1770  *variance = uves_polynomial_new(variance_cpl);
1771  uves_free_polynomial(&variance_cpl);
1772  /* The variance of the fit does not change
1773  when a constant is added to the a_00
1774  coefficient of the polynomial, so don't:
1775  uves_polynomial_shift(*variance, 0, mean_z); */
1776  uves_polynomial_shift(*variance, 1, mean_x);
1777  uves_polynomial_shift(*variance, 2, mean_y);
1778 
1779  /* Maybe here add a consistency check that the variance polynomial is
1780  positive at all input points */
1781  }
1782 
1783  /* If requested, compute mean squared error */
1784  if (mse != NULL || red_chisq != NULL)
1785  {
1786  int i;
1787 
1788  if (mse != NULL) *mse = 0.00 ;
1789  if (red_chisq != NULL) *red_chisq = 0.00 ;
1790  for (i = 0 ; i < np ; i++)
1791  {
1792  double regress = uves_polynomial_evaluate_2d(result,
1793  xy_pos_data_x[i],
1794  xy_pos_data_y[i]);
1795  /* Subtract from the true value, square, accumulate */
1796  if (mse != NULL)
1797  {
1798  double residual = values_data[i] - regress;
1799  *mse += residual*residual;
1800  }
1801  if (red_chisq != NULL)
1802  {
1803  *red_chisq += uves_pow_int((values_data[i] - regress) /
1804  sigmas_data[i], 2);
1805  }
1806  }
1807  /* Get average */
1808  if (mse != NULL) *mse /= (double) np ;
1809 
1810  if (red_chisq != NULL)
1811  {
1812  passure( np > nc, "%d %d", np, nc); /* Was already checked */
1813  *red_chisq /= (double) (np - nc) ;
1814  }
1815  }
1816 
1817  cleanup:
1818  return result ;
1819 }
1820 
1821