1 /*--------------------------------------------------------------------
2 *
3 * Copyright (c) 1991-2021 by the GMT Team (https://www.generic-mapping-tools.org/team.html)
4 * See LICENSE.TXT file for copying and redistribution conditions.
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU Lesser General Public License as published by
8 * the Free Software Foundation; version 3 or 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 Lesser General Public License for more details.
14 *
15 * Contact info: www.generic-mapping-tools.org
16 *--------------------------------------------------------------------*/
17
18 /*
19 * Author: Walter H.F. Smith
20 * Date: 1-JAN-2010
21 * Version: 5.x
22 */
23
24 #include "gmt_dev.h"
25 #include "gmt_internals.h"
26
27 #define MAX_SWEEPS 50
28
29 /* Local functions */
30
gmtvector_switchrows(double * a,double * b,unsigned int n1,unsigned int n2,unsigned int n)31 GMT_LOCAL void gmtvector_switchrows (double *a, double *b, unsigned int n1, unsigned int n2, unsigned int n) {
32 double *oa = (double *)malloc (sizeof(double)*n);
33
34 memcpy(oa, a+n*n1, sizeof(double)*n);
35 memcpy(a+n*n1, a+n*n2, sizeof(double)*n);
36 memcpy(a+n*n2, oa, sizeof(double)*n);
37
38 gmt_M_double_swap (b[n1], b[n2]);
39
40 gmt_M_str_free (oa);
41 }
42
43 /* Given a matrix a[0..m-1][0...n-1], this routine computes its singular
44 value decomposition, A=UWVt. The matrix U replaces a on output.
45 The diagonal matrix of singular values W is output as a vector
46 w[0...n-1]. The matrix V (Not V transpose) is output as
47 v[0...n-1][0....n-1]. m must be greater than or equal to n; if it is
48 smaller, then a should be filled up to square with zero rows.
49
50 Modified from Numerical Recipes -> page 68.
51 */
52
53 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
54
55 #ifndef HAVE_LAPACK
56 /* Use version provided by DJ 2015-07-06 [SDSC] */
gmtvector_svdcmp_nr(struct GMT_CTRL * GMT,double * a,unsigned int m_in,unsigned int n_in,double * w,double * v)57 GMT_LOCAL int gmtvector_svdcmp_nr (struct GMT_CTRL *GMT, double *a, unsigned int m_in, unsigned int n_in, double *w, double *v) {
58 /* void svdcmp(double *a,int m,int n,double *w,double *v) */
59
60 int flag,i,its,j,jj,k,l=0,nm = 0, n = n_in, m = m_in;
61 double c,f,h,s,x,y,z;
62 double anorm=0.0,tnorm, g=0.0,scale=0.0;
63 double *rv1 = NULL;
64
65 if (m < n) {
66 GMT_Report (GMT->parent, GMT_MSG_ERROR, "Failure in gmt_svdcmp: m < n augment A with additional rows\n");
67 return (GMT_DIM_TOO_SMALL);
68 }
69
70 /* allocate work space */
71
72 rv1 = gmt_M_memory (GMT, NULL, n, double);
73
74 /* do householder reduction to bidiagonal form */
75
76 for (i=0;i<n;i++) {
77 l=i+1;
78 rv1[i]=scale*g;
79 g=s=scale=0.0;
80 if (i < m) {
81 for (k=i;k<m;k++) scale += fabs (a[k*n+i]); /* a[k][i] */
82 if (scale) {
83 for (k=i;k<m;k++) {
84 a[k*n+i] /= scale; /* a[k][i] */
85 s += a[k*n+i]*a[k*n+i]; /* a[k][i] */
86 }
87 f=a[i*n+i]; /* a[i][i] */
88 g= -1.0*SIGN(sqrt(s),f);
89 h=f*g-s;
90 a[i*n+i]=f-g; /* a[i][i] */
91 if (i != n-1) {
92 for (j=l;j<n;j++) {
93 for (s=0.0,k=i;k<m;k++) s += a[k*n+i]*a[k*n+j]; /* a[k][i] a[k][j] */
94 f=s/h;
95 for (k=i;k<m;k++) a[k*n+j] += f*a[k*n+i]; /* a[k][j] a[k][i] */
96 }
97 }
98 for (k=i;k<m;k++) a[k*n+i] *= scale; /* a[k][i] */
99 }
100 }
101 w[i]=scale*g;
102 g=s=scale=0.0;
103 if (i <= m-1 && i != n-1) {
104 for (k=l;k<n;k++) scale += fabs (a[i*n+k]); /* a[i][k] */
105 if (scale) {
106 for (k=l;k<n;k++) {
107 a[i*n+k] /= scale; /* a[i][k] */
108 s += a[i*n+k]*a[i*n+k]; /* a[i][k] */
109 }
110 f=a[i*n+l]; /* a[i][l] */
111 g = -1.0*SIGN(sqrt(s),f);
112 h=f*g-s;
113 a[i*n+l]=f-g; /* a[i][l] */
114 for (k=l;k<n;k++) rv1[k]=a[i*n+k]/h; /* a[i][k] */
115 if (i != m-1) {
116 for (j=l;j<m;j++) {
117 for (s=0.0,k=l;k<n;k++) s += a[j*n+k]*a[i*n+k]; /*a[j][k] a[i][k] */
118 for (k=l;k<n;k++) a[j*n+k] += s*rv1[k]; /* a[j][k] */
119 }
120 }
121 for (k=l;k<n;k++) a[i*n+k] *= scale; /* a[i][k] */
122 }
123 }
124 tnorm=fabs (w[i])+fabs (rv1[i]);
125 anorm=MAX(anorm,tnorm);
126 }
127
128 /* accumulation of right-hand transforms */
129
130 for (i=n-1;i>=0;i--) {
131 if (i < n-1) {
132 if (g) {
133 for (j=l;j<n;j++) v[j*n+i]=(a[i*n+j]/a[i*n+l])/g; /* v[j][i] a[i][j] a[i][l] */
134 for (j=l;j<n;j++) {
135 for (s=0.0,k=l;k<n;k++) s += a[i*n+k]*v[k*n+j]; /* a[i][k] v[k][j] */
136 for (k=l;k<n;k++) v[k*n+j] += s*v[k*n+i]; /* v[k][j] v[k][i] */
137 }
138 }
139 for (j=l;j<n;j++) v[i*n+j]=v[j*n+i]=0.0; /* v[i][j] v[j][i] */
140 }
141 v[i*n+i]=1.0; /* v[i][i] */
142 g=rv1[i];
143 l=i;
144 }
145
146 /* accumulation of left-hand transforms */
147
148 for (i=n-1;i>=0;i--) {
149 l=i+1;
150 g=w[i];
151 if (i < n-1) for (j=l;j<n;j++) a[i*n+j]=0.0; /* a[i][j] */
152 if (g) {
153 g=1.0/g;
154 if (i != n-1) {
155 #ifdef _OPENMP
156 #pragma omp parallel for private(j,k,f,s) shared(a,n,m,g,l)
157 #endif
158 for (j=l;j<n;j++) {
159 for (s=0.0,k=l;k<m;k++) s += a[k*n+i]*a[k*n+j]; /* a[k][i] a[k][j] */
160 f=(s/a[i*n+i])*g; /* a[i][i] */
161 for (k=i;k<m;k++) a[k*n+j] += f*a[k*n+i]; /* a[k][j] a[k][i] */
162 }
163 }
164 for (j=i;j<m;j++) a[j*n+i] *= g; /* a[j][i] */
165 }
166 else {
167 for (j=i;j<m;j++) a[j*n+i]=0.0; /* a[j][i] */
168 }
169 ++a[i*n+i]; /* a[i][i] */
170 }
171
172 /* diagonalization of the bidiagonal form */
173
174 for (k=n-1;k>=0;k--) { /* loop over singular values */
175 for (its=1;its<=30;its++) { /* loop over allowed iterations */
176 flag=1;
177 for (l=k;l>=0;l--) { /* test for splitting */
178 nm=l-1;
179 if (fabs(rv1[l])+anorm == anorm) {
180 flag=0;
181 break;
182 }
183 if (fabs (w[nm])+anorm == anorm) break;
184 }
185 if (flag) {
186 c=0.0; /* cancellation of rv1[l] if l > 1 */
187 s=1.0;
188 for (i=l;i<=k;i++) {
189 f=s*rv1[i];
190 if (fabs (f)+anorm != anorm) {
191 g=w[i];
192 h=hypot (f,g);
193 w[i]=h;
194 h=1.0/h;
195 c=g*h;
196 s=(-1.0*f*h);
197 for (j=0;j<m;j++) {
198 y=a[j*n+nm]; /* a[j][nm] */
199 z=a[j*n+i]; /* a[j][i] */
200 a[j*n+nm]=(y*c)+(z*s); /* a[j][nm] */
201 a[j*n+i]=(z*c)-(y*s); /* a[j][i] */
202 }
203 }
204 }
205 }
206 z=w[k];
207 if (l == k) { /* convergence */
208 if (z < 0.0) { /* singular value is made positive */
209 w[k]= -1.0*z;
210 for (j=0;j<n;j++) v[j*n+k] *= (-1.0); /* v[j][k] */
211 }
212 break;
213 }
214 if (its == 30) {
215 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Failure in gmt_svdcmp: No convergence in 30 iterations\n");
216 #ifndef _OPENMP
217 return (GMT_RUNTIME_ERROR);
218 #endif
219 }
220 x=w[l]; /* shift from bottom 2-by-2 minor */
221 nm=k-1;
222 y=w[nm];
223 g=rv1[nm];
224 h=rv1[k];
225 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
226 g=hypot (f,1.0);
227 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
228
229 /* next QR transformation */
230
231 c=s=1.0;
232 for (j=l;j<=nm;j++) {
233 i=j+1;
234 g=rv1[i];
235 y=w[i];
236 h=s*g;
237 g=c*g;
238 z=hypot(f,h);
239 rv1[j]=z;
240 c=f/z;
241 s=h/z;
242 f=(x*c)+(g*s);
243 g=(g*c)-(x*s);
244 h=y*s;
245 y=y*c;
246 for (jj=0;jj<n;jj++) {
247 x=v[jj*n+j]; /* v[jj][j] */
248 z=v[jj*n+i]; /* v[jj][i] */
249 v[jj*n+j]=(x*c)+(z*s); /* v[jj][j] */
250 v[jj*n+i]=(z*c)-(x*s); /* v[jj][i] */
251 }
252 z=hypot(f,h);
253 w[j]=z; /* rotation can be arbitrary if z=0 */
254 if (z) {
255 z=1.0/z;
256 c=f*z;
257 s=h*z;
258 }
259 f=(c*g)+(s*y);
260 x=(c*y)-(s*g);
261 for (jj=0;jj<m;jj++) {
262 y=a[jj*n+j]; /* a[jj][j] */
263 z=a[jj*n+i]; /* a[jj][i] */
264 a[jj*n+j]=(y*c)+(z*s); /* a[jj][j] */
265 a[jj*n+i]=(z*c)-(y*s); /* a[jj][i] */
266 }
267 }
268 rv1[l]=0.0;
269 rv1[k]=f;
270 w[k]=x;
271 }
272 }
273 gmt_M_free (GMT, rv1);
274 return (GMT_NOERROR);
275 }
276 #endif
277
gmtvector_compare_singular_values(const void * point_1v,const void * point_2v)278 GMT_LOCAL int gmtvector_compare_singular_values (const void *point_1v, const void *point_2v) {
279 /* Routine for qsort to sort struct GMT_SINGULAR_VALUE on decreasing eigenvalues
280 * keeping track of the original order before sorting.
281 */
282 const struct GMT_SINGULAR_VALUE *E_1 = point_1v, *E_2 = point_2v;
283 bool bad_1, bad_2;
284
285 bad_1 = gmt_M_is_dnan (E_1->value);
286 bad_2 = gmt_M_is_dnan (E_2->value);
287 if (bad_1 && bad_2) return (0);
288 if (bad_1) return (+1);
289 if (bad_2) return (-1);
290 if (fabs(E_1->value) < fabs(E_2->value)) return (+1);
291 if (fabs(E_1->value) > fabs(E_2->value)) return (-1);
292 return (0);
293 }
294
gmtvector_fix_up_path_cartonly(struct GMT_CTRL * GMT,double ** a_x,double ** a_y,uint64_t n,unsigned int mode)295 GMT_LOCAL uint64_t gmtvector_fix_up_path_cartonly (struct GMT_CTRL *GMT, double **a_x, double **a_y, uint64_t n, unsigned int mode) {
296 /* Takes pointers to a list of <n> Cartesian x/y pairs and adds
297 * auxiliary points to build a staircase curve.
298 * If mode=1: staircase; first follows y, then x
299 * If mode=2: staircase; first follows x, then y
300 * Returns the new number of points (original plus auxiliary).
301 */
302
303 uint64_t i, k, n_new = 2*n - 1;
304 double *x = NULL, *y = NULL;
305
306 x = *a_x; y = *a_y; /* Default is to return the input unchanged */
307 if (n < 2 || mode == 0) return n; /* Nothing to do */
308
309 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, 1, 2); /* Init or reallocate two tmp vectors */
310 /* Start at first point */
311 GMT->hidden.mem_coord[GMT_X][0] = x[0]; GMT->hidden.mem_coord[GMT_Y][0] = y[0];
312
313 for (i = k = 1; i < n; i++) { /* For remaining points we must insert an intermediate node */
314 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, k+1, 2); /* Init or reallocate two tmp vectors */
315 if (mode == GMT_STAIRS_X) { /* First follow x, then y */
316 GMT->hidden.mem_coord[GMT_X][k] = x[i];
317 GMT->hidden.mem_coord[GMT_Y][k] = y[i-1];
318 }
319 else if (mode == GMT_STAIRS_Y) { /* First follow y, then x */
320 GMT->hidden.mem_coord[GMT_X][k] = x[i-1];
321 GMT->hidden.mem_coord[GMT_Y][k] = y[i];
322 }
323 k++;
324 /* Then add original point */
325 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, k+1, 2); /* Init or reallocate two tmp vectors */
326 GMT->hidden.mem_coord[GMT_X][k] = x[i]; GMT->hidden.mem_coord[GMT_Y][k] = y[i];
327 k++;
328 }
329
330 /* Destroy old allocated memory and put the new one in place */
331 gmt_M_free (GMT, x); gmt_M_free (GMT, y);
332 *a_x = gmtlib_assign_vector (GMT, n_new, GMT_X);
333 *a_y = gmtlib_assign_vector (GMT, n_new, GMT_Y);
334
335 return (n_new);
336 }
337
gmtvector_fix_up_path_polar(struct GMT_CTRL * GMT,double ** a_x,double ** a_y,uint64_t n,double step,unsigned int mode)338 GMT_LOCAL uint64_t gmtvector_fix_up_path_polar (struct GMT_CTRL *GMT, double **a_x, double **a_y, uint64_t n, double step, unsigned int mode) {
339 /* Takes pointers to a list of <n> theta/r pairs (in user units) and adds
340 * auxiliary points if the distance between two given points exceeds
341 * <step> units.
342 * If mode=0: returns points along a straight line
343 * If mode=1: staircase; first follows r, then theta
344 * If mode=2: staircase; first follows theta, then r
345 * Returns the new number of points (original plus auxiliary).
346 */
347
348 uint64_t i, j, n_new, n_step = 0;
349 double *x = NULL, *y = NULL, c;
350
351 if (mode == GMT_STAIRS_OFF) return n; /* Nothing to do */
352
353 x = *a_x; y = *a_y;
354
355 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, 1, 2); /* Init or reallocate tmp vectors */
356 GMT->hidden.mem_coord[GMT_X][0] = x[0]; GMT->hidden.mem_coord[GMT_Y][0] = y[0]; n_new = 1;
357 if (step <= 0.0) step = 1.0; /* Sanity valve; if step not given we set it to 1 */
358
359 for (i = 1; i < n; i++) {
360 if (mode == GMT_STAIRS_X) { /* First follow theta, then r */
361 n_step = lrint (fabs (x[i] - x[i-1]) / step);
362 for (j = 1; j <= n_step; j++) {
363 c = j / (double)n_step;
364 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
365 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1] * (1 - c) + x[i] * c;
366 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i-1];
367 n_new++;
368 }
369 GMT->hidden.mem_coord[GMT_X][n_new] = x[i]; GMT->hidden.mem_coord[GMT_Y][n_new] = y[i]; n_new++;
370 }
371 else if (mode == GMT_STAIRS_Y) { /* First follow r, then theta */
372 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1]; GMT->hidden.mem_coord[GMT_Y][n_new] = y[i]; n_new++;
373 n_step = lrint (fabs (x[i] - x[i-1]) / step);
374 for (j = 1; j <= n_step; j++) {
375 c = j / (double)n_step;
376 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
377 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1] * (1 - c) + x[i] * c;
378 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i];
379 n_new++;
380 }
381 }
382 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
383 }
384
385 /* Destroy old allocated memory and put the new one in place */
386 gmt_M_free (GMT, x); gmt_M_free (GMT, y);
387 *a_x = gmtlib_assign_vector (GMT, n_new, GMT_X);
388 *a_y = gmtlib_assign_vector (GMT, n_new, GMT_Y);
389
390 return (n_new);
391 }
392
gmtvector_fix_up_path_cartesian(struct GMT_CTRL * GMT,double ** a_x,double ** a_y,uint64_t n,double step,unsigned int mode)393 GMT_LOCAL uint64_t gmtvector_fix_up_path_cartesian (struct GMT_CTRL *GMT, double **a_x, double **a_y, uint64_t n, double step, unsigned int mode) {
394 /* Takes pointers to a list of <n> x/y pairs (in user units) and adds
395 * auxiliary points if the distance between two given points exceeds
396 * <step> units.
397 * If mode=0: returns points along a straight line
398 * If mode=1: staircase; first follows y, then x
399 * If mode=2: staircase; first follows x, then y
400 * Returns the new number of points (original plus auxiliary).
401 */
402
403 unsigned int k = 1;
404 uint64_t i, j, n_new, n_step = 0;
405 double *x = NULL, *y = NULL, c;
406
407 x = *a_x; y = *a_y;
408
409 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, 1, 2); /* Init or reallocate tmp vectors */
410 GMT->hidden.mem_coord[GMT_X][0] = x[0]; GMT->hidden.mem_coord[GMT_Y][0] = y[0]; n_new = 1;
411 if (step <= 0.0) step = 1.0; /* Sanity valve; if step not given we set it to 1 */
412
413 for (i = 1; i < n; i++) {
414 if (mode == GMT_STAIRS_Y) { /* First follow x, then y */
415 n_step = lrint (fabs (x[i] - x[i-1]) / step);
416 for (j = 1; j <= n_step; j++) {
417 c = j / (double)n_step;
418 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
419 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1] * (1 - c) + x[i] * c;
420 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i-1];
421 n_new++;
422 }
423 n_step = lrint (fabs (y[i]-y[i-1]) / step);
424 for (j = k; j < n_step; j++) { /* Start at 0 to make sure corner point is saved */
425 c = j / (double)n_step;
426 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
427 GMT->hidden.mem_coord[GMT_X][n_new] = x[i];
428 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i-1] * (1 - c) + y[i] * c;
429 n_new++;
430 }
431 k = 0;
432 }
433 else if (mode == GMT_STAIRS_X) { /* First follow y, then x */
434 n_step = lrint (fabs (y[i]-y[i-1]) / step);
435 for (j = 1; j <= n_step; j++) {
436 c = j / (double)n_step;
437 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
438 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1];
439 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i-1] * (1 - c) + y[i] * c;
440 n_new++;
441 }
442 n_step = lrint (fabs (x[i]-x[i-1]) / step);
443 for (j = k; j < n_step; j++) { /* Start at 0 to make sure corner point is saved */
444 c = j / (double)n_step;
445 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
446 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1] * (1 - c) + x[i] * c;
447 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i];
448 n_new++;
449 }
450 k = 0;
451 }
452 /* Follow straight line */
453 else if ((n_step = lrint (hypot (x[i]-x[i-1], y[i]-y[i-1]) / step)) > 1) { /* Must insert (n_step - 1) points, i.e. create n_step intervals */
454 for (j = 1; j < n_step; j++) {
455 c = j / (double)n_step;
456 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
457 GMT->hidden.mem_coord[GMT_X][n_new] = x[i-1] * (1 - c) + x[i] * c;
458 GMT->hidden.mem_coord[GMT_Y][n_new] = y[i-1] * (1 - c) + y[i] * c;
459 n_new++;
460 }
461 }
462 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
463 GMT->hidden.mem_coord[GMT_X][n_new] = x[i]; GMT->hidden.mem_coord[GMT_Y][n_new] = y[i]; n_new++;
464 }
465
466 /* Destroy old allocated memory and put the new one in place */
467 gmt_M_free (GMT, x); gmt_M_free (GMT, y);
468 *a_x = gmtlib_assign_vector (GMT, n_new, GMT_X);
469 *a_y = gmtlib_assign_vector (GMT, n_new, GMT_Y);
470
471 return (n_new);
472 }
473
gmtvector_resample_path_spherical(struct GMT_CTRL * GMT,double ** lon,double ** lat,uint64_t n_in,double step_out,enum GMT_enum_track mode)474 GMT_LOCAL uint64_t gmtvector_resample_path_spherical (struct GMT_CTRL *GMT, double **lon, double **lat, uint64_t n_in, double step_out, enum GMT_enum_track mode) {
475 /* See gmt_resample_path below for details. */
476
477 bool meridian, new_pair;
478 uint64_t last_row_in = 0, row_in, row_out, n_out;
479 unsigned int k;
480 double dist_out, gap, d_lon = 0.0, L = 0.0, frac_to_a, frac_to_b, minlon, maxlon, a[3], b[3], c[3];
481 double P[3], Rot0[3][3], Rot[3][3], total_angle_rad = 0.0, angle_rad, ya = 0.0, yb = 0.0;
482 double *dist_in = NULL, *lon_out = NULL, *lat_out = NULL, *lon_in = *lon, *lat_in = *lat;
483
484 if (step_out < 0.0) { /* Safety valve */
485 GMT_Report (GMT->parent, GMT_MSG_ERROR, "Internal error: gmtvector_resample_path_spherical given negative step-size\n");
486 return (GMT_RUNTIME_ERROR);
487 }
488 if (mode > GMT_TRACK_SAMPLE_ADJ) { /* Bad mode*/
489 GMT_Report (GMT->parent, GMT_MSG_ERROR, "Internal error: gmtvector_resample_path_spherical given bad mode %d\n", mode);
490 return (GMT_RUNTIME_ERROR);
491 }
492
493 if (mode < GMT_TRACK_SAMPLE_FIX) {
494 if (GMT->current.map.dist[GMT_MAP_DIST].arc) /* Gave an increment in arc length (degree, min, sec) */
495 step_out /= GMT->current.map.dist[GMT_MAP_DIST].scale; /* Get degrees */
496 else /* Gave increment in spatial distance (km, meter, etc.) */
497 step_out = (step_out / GMT->current.map.dist[GMT_MAP_DIST].scale) / GMT->current.proj.DIST_M_PR_DEG; /* Get degrees */
498 return (gmt_fix_up_path (GMT, lon, lat, n_in, step_out, mode)); /* Insert extra points only */
499 }
500
501 dist_in = gmt_dist_array (GMT, lon_in, lat_in, n_in, true); /* Compute cumulative distances along line */
502
503 if (step_out == 0.0) step_out = (dist_in[n_in-1] - dist_in[0])/100.0; /* If nothing is selected we get 101 points */
504 /* Determine n_out, the number of output points */
505 if (mode == GMT_TRACK_SAMPLE_ADJ) { /* Round to nearest multiple of step_out, then adjust step to match exactly */
506 n_out = lrint (dist_in[n_in-1] / step_out);
507 step_out = dist_in[n_in-1] / n_out; /* Ensure exact fit */
508 }
509 else { /* Stop when last multiple is reached */
510 n_out = lrint (floor (dist_in[n_in-1] / step_out));
511 }
512 n_out++; /* Since number of points = number of segments + 1 */
513
514 lon_out = gmt_M_memory (GMT, NULL, n_out, double);
515 lat_out = gmt_M_memory (GMT, NULL, n_out, double);
516
517 lon_out[0] = lon_in[0]; lat_out[0] = lat_in[0]; /* Start at same origin */
518 for (row_in = row_out = 1; row_out < n_out; row_out++) { /* For remaining output points */
519 dist_out = row_out * step_out; /* Rhe desired output distance */
520 while (row_in < n_in && dist_in[row_in] < dist_out) row_in++; /* Wind so row points to the next input point with a distance >= dist_out */
521 gap = dist_in[row_in] - dist_out; /* Distance to next input datapoint */
522 new_pair = (row_in > last_row_in);
523 if (gmt_M_is_zero (gap)) { /* Use the input point as is */
524 lon_out[row_out] = lon_in[row_in]; lat_out[row_out] = lat_in[row_in];
525 }
526 else { /* Must interpolate along great-circle arc from a (point row_in-1) to b (point row_in) */
527 if (new_pair) { /* Must perform calculations on the two points */
528 L = dist_in[row_in] - dist_in[row_in-1]; /* Distance between points a and b */
529 ya = gmt_lat_swap (GMT, lat_in[row_in-1], GMT_LATSWAP_G2O); /* Convert to geocentric */
530 yb = gmt_lat_swap (GMT, lat_in[row_in], GMT_LATSWAP_G2O); /* Convert to geocentric */
531 }
532 frac_to_a = gap / L;
533 frac_to_b = 1.0 - frac_to_a;
534 if (GMT->current.map.loxodrome) { /* Linear resampling along Mercator straight line */
535 if (new_pair) gmt_M_set_delta_lon (lon_in[row_in-1], lon_in[row_in], d_lon);
536 a[0] = D2R * lon_in[row_in-1]; a[1] = d_log (GMT, tand (45.0 + 0.5 * ya));
537 b[0] = D2R * (lon_in[row_in-1] + d_lon); b[1] = d_log (GMT, tand (45.0 + 0.5 * yb));
538 for (k = 0; k < 2; k++) c[k] = a[k] * frac_to_a + b[k] * frac_to_b; /* Linear interpolation to find output point c */
539 lon_out[row_out] = c[0] * R2D;
540 lat_out[row_out] = atand (sinh (c[1]));
541 lat_out[row_out] = gmt_lat_swap (GMT, lat_out[row_out], GMT_LATSWAP_O2G); /* Convert back to geodetic */
542 }
543 else { /* Spherical resampling along segment */
544 if (new_pair) { /* Must perform calculations on the two points */
545 gmt_geo_to_cart (GMT, ya, lon_in[row_in-1], a, true);
546 gmt_geo_to_cart (GMT, yb, lon_in[row_in], b, true);
547 total_angle_rad = d_acos (gmt_dot3v (GMT, a, b)); /* Get spherical angle from a to b in radians; this is same distance as L */
548 gmt_cross3v (GMT, a, b, P); /* Get pole P of plane trough a and b (and center of Earth) */
549 gmt_normalize3v (GMT, P); /* Make sure P has unit length */
550 gmtlib_init_rot_matrix (Rot0, P); /* Get partial rotation matrix since no actual angle is applied yet */
551 }
552 gmt_M_memcpy (Rot, Rot0, 9, double); /* Get a copy of the "0-angle" rotation matrix */
553 angle_rad = total_angle_rad * frac_to_b; /* Angle we need to rotate from a to c */
554 gmtlib_load_rot_matrix (angle_rad, Rot, P); /* Build the actual rotation matrix for this angle */
555 gmt_matrix_vect_mult (GMT, 3U, Rot, a, c); /* Rotate from a to get c */
556 gmt_cart_to_geo (GMT, &lat_out[row_out], &lon_out[row_out], c, true);
557 lat_out[row_out] = gmt_lat_swap (GMT, lat_out[row_out], GMT_LATSWAP_O2G); /* Convert back to geodetic */
558 }
559 minlon = MIN (lon_in[row_in-1], lon_in[row_in]);
560 maxlon = MAX (lon_in[row_in-1], lon_in[row_in]);
561 meridian = doubleAlmostEqualZero (maxlon, minlon); /* A meridian; make sure we get right lon value */
562 if (meridian)
563 lon_out[row_out] = minlon;
564 else if (lon_out[row_out] < minlon)
565 lon_out[row_out] += 360.0;
566 else if (lon_out[row_out] > maxlon)
567 lon_out[row_out] -= 360.0;
568 }
569 last_row_in = row_in;
570 }
571
572 /* Destroy old allocated memory and put the new one in place */
573
574 gmt_M_free (GMT, lon_in);
575 gmt_M_free (GMT, lat_in);
576 gmt_M_free (GMT, dist_in);
577 *lon = lon_out;
578 *lat = lat_out;
579 return (n_out);
580 }
581
gmtvector_resample_path_cartesian(struct GMT_CTRL * GMT,double ** x,double ** y,uint64_t n_in,double step_out,enum GMT_enum_track mode)582 GMT_LOCAL uint64_t gmtvector_resample_path_cartesian (struct GMT_CTRL *GMT, double **x, double **y, uint64_t n_in, double step_out, enum GMT_enum_track mode) {
583 /* See gmt_resample_path below for details. */
584
585 uint64_t last_row_in = 0, row_in, row_out, n_out;
586 bool new_pair;
587 double dist_out, gap, L = 0.0, frac_to_a, frac_to_b;
588 double *dist_in = NULL, *x_out = NULL, *y_out = NULL, *x_in = *x, *y_in = *y;
589
590 if (step_out < 0.0) { /* Safety valve */
591 GMT_Report (GMT->parent, GMT_MSG_ERROR, "Internal error: gmtvector_resample_path_cartesian given negative step-size\n");
592 return (GMT_RUNTIME_ERROR);
593 }
594 if (mode > GMT_TRACK_SAMPLE_ADJ) { /* Bad mode*/
595 GMT_Report (GMT->parent, GMT_MSG_ERROR, "Internal error: gmtvector_resample_path_cartesian given bad mode %d\n", mode);
596 return (GMT_RUNTIME_ERROR);
597 }
598
599 if (mode < GMT_TRACK_SAMPLE_FIX) return (gmtvector_fix_up_path_cartesian (GMT, x, y, n_in, step_out, mode)); /* Insert extra points only */
600
601 dist_in = gmt_dist_array (GMT, x_in, y_in, n_in, true); /* Compute cumulative distances along line */
602 if (step_out == 0.0) step_out = (dist_in[n_in-1] - dist_in[0])/100.0; /* If nothing is selected we get 101 points */
603
604 /* Determine n_out, the number of output points */
605 if (mode == GMT_TRACK_SAMPLE_ADJ) { /* Round to nearest multiples, then adjust step to match exactly */
606 n_out = lrint (dist_in[n_in-1] / step_out);
607 step_out = dist_in[n_in-1] / n_out;
608 }
609 else { /* Stop when last multiple is reached */
610 n_out = lrint (floor (dist_in[n_in-1] / step_out));
611 }
612 n_out++; /* Since number of points = number of segments + 1 */
613
614 x_out = gmt_M_memory (GMT, NULL, n_out, double);
615 y_out = gmt_M_memory (GMT, NULL, n_out, double);
616
617 x_out[0] = x_in[0]; y_out[0] = y_in[0]; /* Start at same origin */
618 for (row_in = row_out = 1; row_out < n_out; row_out++) { /* For remaining output points */
619 dist_out = row_out * step_out; /* The desired output distance */
620 while (row_in < n_in && dist_in[row_in] < dist_out) row_in++; /* Wind so row points to the next input point with a distance >= dist_out */
621 gap = dist_in[row_in] - dist_out; /* Distance to next input datapoint */
622 new_pair = (row_in > last_row_in);
623 if (gmt_M_is_zero (gap)) { /* Use the input point as is */
624 x_out[row_out] = x_in[row_in]; y_out[row_out] = y_in[row_in];
625 }
626 else { /* Must interpolate along great-circle arc from a (point row_in-1) to b (point row_in) */
627 if (new_pair) L = dist_in[row_in] - dist_in[row_in-1]; /* Distance between points a and b */
628 frac_to_a = gap / L;
629 frac_to_b = 1.0 - frac_to_a;
630 x_out[row_out] = x_in[row_in-1] * frac_to_a + x_in[row_in] * frac_to_b; /* Linear interpolation to find output point */
631 y_out[row_out] = y_in[row_in-1] * frac_to_a + y_in[row_in] * frac_to_b; /* Linear interpolation to find output point */
632 }
633 last_row_in = row_in;
634 }
635
636 /* Destroy old allocated memory and put the new one in place */
637
638 gmt_M_free (GMT, x_in);
639 gmt_M_free (GMT, y_in);
640 gmt_M_free (GMT, dist_in);
641 *x = x_out;
642 *y = y_out;
643 return (n_out);
644 }
645
gmt_jacobi(struct GMT_CTRL * GMT,double * a,unsigned int n,unsigned int m,double * d,double * v,double * b,double * z,unsigned int * nrots)646 int gmt_jacobi (struct GMT_CTRL *GMT, double *a, unsigned int n, unsigned int m, double *d, double *v, double *b, double *z, unsigned int *nrots) {
647 /*
648 *
649 * Find eigenvalues & eigenvectors of a square symmetric matrix by Jacobi's
650 * method. Given A, find V and D such that A = V * D * V-transpose, with
651 * V an orthogonal matrix and D a diagonal matrix. The eigenvalues of A
652 * are on diag(D), and the j-th column of V is the eigenvector corresponding
653 * to the j-th diagonal element of D. Returns 0 if OK, -1 if it fails to
654 * converge in MAX_SWEEPS.
655 *
656 * a is sent as a square symmetric matrix, of size n, and row dimension m.
657 * Only the diagonal and super-diagonal elements of a will be used, so the
658 * sub-diagonal elements could be used to preserve a, or could have been
659 * destroyed by an earlier attempt to form the Cholesky decomposition of a.
660 * On return, the super-diagonal elements are destroyed. The diagonal and
661 * sub-diagonal elements are unchanged.
662 * d is returned as an n-vector containing the eigenvalues of a, sorted
663 * so that d[i] >= d[j] when i < j. d = diag(D).
664 * v is returned as an n by n matrix, V, with row dimension m, and the
665 * columns of v are the eigenvectors corresponding to the values in d.
666 * b is an n-vector of workspace, used to keep a copy of the diagonal
667 * elements which is updated only after a full sweep.
668 * z is an n-vector of workspace, used to accumulate the updates to
669 * the diagonal values of a during each sweep. This reduces round-
670 * off problems.
671 * nrots is the number of rotations performed. Bounds on round-off error
672 * can be estimated from this if desired.
673 *
674 * Numerical Details:
675 * The basic algorithms is in many textbooks. The idea is to make an
676 * infinite series (which turns out to be at quadratically convergent)
677 * of steps, in each of which A_new = P-transpose * A_old * P, where P is
678 * a plane-rotation matrix in the p,q plane, through an angle chosen to
679 * zero A_new(p,q) and A_new(q,p). The sum of the diagonal elements
680 * of A is unchanged by these operations, but the sum of squares of
681 * diagonal elements of a is increased by 2 * |A_old(p,q)| at each step.
682 * Although later steps make non-zero again the previously zeroed entries,
683 * the sum of squares of diagonal elements increases with each rotation,
684 * while the sum of squares of off-diagonals keeps decreasing, so that
685 * eventually A_new is diagonal to machine precision. This should
686 * happen in a few (3 to 7) sweeps.
687 *
688 * If only the eigenvalues are wanted then there are faster methods, but
689 * if all eigenvalues and eigenvectors are needed, then this method is
690 * only somewhat slower than the fastest method (Householder tri-
691 * diagonalization followed by symmetric QR iterations), and this method
692 * is numerically extremely stable.
693 *
694 * C G J Jacobi ("Ueber ein leichtes Vefahren, die in der Theorie der
695 * Saekularstoerungen vorkommenden Gelichungen numerisch aufzuloesen",
696 * Crelle's Journal, v. 30, pp. 51--94, 1846) originally searched the
697 * entire (half) matrix for the largest |A(p,q)| to select each step.
698 * When the method was developed for machine computation (R T Gregory,
699 * "Computing eigenvalues and eigenvectors of a symmetric matrix on
700 * the ILLIAC", Math. Tab. and other Aids to Comp., v. 7, pp. 215--220,
701 * 1953) it was done with a series of "sweeps" through the upper triangle,
702 * visiting all p,q in turn. Later, D A Pope and C Tompkins ("Maximizing
703 * functions of rotations - experiments concerning speed of diagonalization
704 * of symmetric matrices using Jacobi's method", J Assoc. Comput. Mach.
705 * v. 4, pp. 459--466, 1957) introduced a variant that skips small
706 * elements on the first few sweeps. The algorithm here was given by
707 * Heinz Rutishauser (1918--1970) and published in Numer. Math. v. 9,
708 * pp 1--10, 1966, and in Linear Algebra (the Handbook for Automatic
709 * Computation, v. II), by James Hardy Wilkinson and C. Reinsch (Springer-
710 * Verlag, 1971). It also appears in Numerical Recipes.
711 *
712 * This algorithm takes care to avoid round-off error in several ways.
713 * First, although there are four values of theta in (-pi, pi] that
714 * would zero A(p,q), there is only one with magnitude <= pi/4.
715 * This one is used. This is most stable, and also has the effect
716 * that, if A_old(p,p) >= A_old(q,q) then A_new(p,p) > A_new(q,q).
717 * Two copies of the diagonal elements are maintained in d[] and b[].
718 * d[] is updated immediately in each rotation, and each new rotation
719 * is computed based on d[], so that each rotation gets the benefit
720 * of the previous ones. However, z[] is also used to accumulate
721 * the sum of all the changes in the diagonal elements during one sweep,
722 * and z[] is used to update b[] after each sweep. Then b is copied
723 * to d. In this way, at the end of each sweep, d is reset to avoid
724 * accumulating round-off.
725 *
726 * This routine determines whether y is small compared to x by testing
727 * if (fabs(y) + fabs(x) == fabs(x) ). It is assumed that the
728 * underflow which may occur here is nevertheless going to allow this
729 * expression to be evaluated as true or false and execution to
730 * continue. If the run environment doesn't allow this, the routine
731 * won't work properly.
732 *
733 * programmer: W. H. F. Smith, 7 June, 1991.
734 * Revised: PW: 12-MAR-1998 for GMT 3.1
735 * Revision by WHF Smith, March 03, 2000, to speed up loop indexes.
736 */
737 unsigned int p, q, pp, pq, mp1, pm, qm, nsweeps, j, jm, i, k;
738 double sum, threshold, g, h, t, theta, c, s, tau;
739
740 /* Begin by initializing v, b, d, and z. v = identity matrix,
741 b = d = diag(a), and z = 0: */
742
743 gmt_M_memset (v, m*n, double);
744 gmt_M_memset (z, n, double);
745
746 mp1 = m + 1;
747
748 for (p = 0, pp = 0; p < n; p++, pp+=mp1) {
749 v[pp] = 1.0;
750 b[p] = a[pp];
751 d[p] = b[p];
752 }
753
754 /* End of initializations. Set counters and begin: */
755
756 (*nrots) = 0;
757 nsweeps = 0;
758
759 while (nsweeps < MAX_SWEEPS) {
760
761 /* Sum off-diagonal elements of upper triangle. */
762 sum = 0.0;
763 for (q = 1, qm = m; q < n; q++, qm += m ) {
764 for (p = 0, pq = qm; p < q; p++, pq++) sum += fabs(a[pq]);
765 }
766
767 /* Exit this loop (converged) when sum == 0.0 */
768 if (sum == 0.0) break;
769
770 /* If (nsweeps < 3) do only bigger elements; else all */
771 threshold = (nsweeps < 3) ? 0.2 * sum / ( n * n ) : 0.0;
772
773 /* Now sweep whole upper triangle doing Givens rotations: */
774 for (q = 1, qm = m; q < n; q++, qm += m ) {
775 for (p = 0, pm = 0, pq = qm; p < q; p++, pm += m, pq++) {
776 /* In 3/2000 I swapped order of these loops,
777 to allow simple incrementing of pq */
778
779 if (a[pq] == 0.0) continue; /* New 3/2000 */
780
781 g = 100.0 * fabs (a[pq]);
782
783 /* After four sweeps, if g is small relative
784 to a(p,p) and a(q,q), skip the
785 rotation and set a(p,q) to zero. */
786
787 if ((nsweeps > 3) && ((fabs (d[p])+g) == fabs (d[p])) && ((fabs (d[q])+g) == fabs (d[q]))) {
788 a[pq] = 0.0;
789 }
790 else if (fabs (a[pq]) > threshold) {
791 h = d[q] - d[p];
792
793 if (h == 0.0)
794 t = 1.0; /* This if block is new 3/2000 */
795 else if (fabs (h) + g == fabs (h))
796 t = a[pq] / h;
797 else {
798 theta = 0.5 * h / a[pq];
799 t = 1.0 / (fabs (theta) + sqrt (1.0 + theta*theta) );
800 if (theta < 0.0) t = -t;
801 }
802
803 c = 1.0 / sqrt (1.0 + t*t);
804 s = t * c;
805 tau = s / (1.0 + c);
806
807 h = t * a[pq];
808 z[p] -= h;
809 z[q] += h;
810 d[p] -= h;
811 d[q] += h;
812 a[pq] = 0.0;
813
814 for (j = 0; j < p; j++) {
815 g = a[j + pm];
816 h = a[j + qm];
817 a[j + pm] = g - s * (h + g * tau);
818 a[j + qm] = h + s * (g - h * tau);
819 }
820 for (j = p+1, jm = m*(p+1); j < q; j++, jm += m ) {
821 g = a[p + jm];
822 h = a[j + qm];
823 a[p + jm] = g - s * (h + g * tau);
824 a[j + qm] = h + s * (g - h * tau);
825 }
826 for (j = q+1, jm = m*(q+1); j < n; j++, jm += m ) {
827 g = a[p + jm];
828 h = a[q + jm];
829 a[p + jm] = g - s * (h + g * tau);
830 a[q + jm] = h + s * (g - h * tau);
831 }
832
833 for (j = 0; j < n; j++) {
834 g = v[j + pm];
835 h = v[j + qm];
836 v[j + pm] = g - s * (h + g * tau);
837 v[j + qm] = h + s * (g - h * tau);
838 }
839
840 (*nrots)++;
841 }
842 }
843 }
844
845 /* End of one sweep of the upper triangle. */
846
847 nsweeps++;
848
849 for (p = 0; p < n; p++) {
850 b[p] += z[p]; /* Update the b copy of diagonal */
851 d[p] = b[p]; /* Replace d with b to reduce round-off error */
852 z[p] = 0.0; /* Clear z. */
853 }
854 }
855
856 /* Get here via break when converged, or when nsweeps == MAX_SWEEPS.
857 Sort eigenvalues by insertion: */
858
859 for (i = 0; i < n-1; i++) {
860 k = i;
861 g = d[i];
862 for (j = i+1; j < n; j++) { /* Find max location */
863 if (d[j] >= g) {
864 k = j;
865 g = d[j];
866 }
867 }
868 if (k != i) { /* Need to swap value and vector */
869 d[k] = d[i];
870 d[i] = g;
871 p = i * m;
872 q = k * m;
873 for (j = 0; j < n; j++) {
874 g = v[j + p];
875 v[j + p] = v[j + q];
876 v[j + q] = g;
877 }
878 }
879 }
880
881 /* Return 0 if converged; else print warning and return -1: */
882
883 if (nsweeps == MAX_SWEEPS) {
884 GMT_Report (GMT->parent, GMT_MSG_WARNING, "gmt_jacobi failed to converge in %d sweeps\n", nsweeps);
885 return(-1);
886 }
887 return(0);
888 }
889
gmt_gauss(struct GMT_CTRL * GMT,double * a,double * vec,unsigned int n,unsigned int nstore,bool itriag)890 int gmt_gauss (struct GMT_CTRL *GMT, double *a, double *vec, unsigned int n, unsigned int nstore, bool itriag) {
891
892 /* subroutine gauss, by william menke */
893 /* july 1978 (modified feb 1983, nov 85) */
894
895 /* a subroutine to solve a system of n linear equations in n unknowns
896 * gaussian reduction with partial pivoting is used
897 * a (sent, destroyed) n by n matrix
898 * vec (sent, overwritten) n vector, replaced w/ solution
899 * nstore (sent) dimension of a
900 * ierror (returned) zero on no error
901 * itriag (sent) matrix triangularized only
902 * on true useful when solving
903 * multiple systems with same a
904 */
905 static unsigned int l1;
906 unsigned int *line = NULL, i = 0, j, k, l, j1, j2, *isub = NULL;
907 int iet, ieb;
908 size_t n_alloc = 0;
909 double big, testa, b, sum;
910
911 iet = 0; /* initial error flags, one for triagularization*/
912 ieb = 0; /* one for backsolving */
913 gmt_M_malloc2 (GMT, line, isub, n, &n_alloc, unsigned int);
914
915 /* triangularize the matrix a */
916 /* replacing the zero elements of the triangularized matrix */
917 /* with the coefficients needed to transform the vector vec */
918
919 if (itriag) { /* triangularize matrix */
920
921 for (j = 0; j < n; j++) { /*line is an array of flags*/
922 line[j] = 0;
923 /* elements of a are not moved during pivoting*/
924 /* line=0 flags unused lines */
925 }
926
927 for (j=0; j<n-1; j++) {
928 /* triangularize matrix by partial pivoting */
929 big = 0.0; /* find biggest element in j-th column*/
930 /* of unused portion of matrix*/
931 for (l1=0; l1<n; l1++) {
932 if (line[l1]==0) {
933 testa = fabs ((*(a+l1*nstore+j)));
934 if (testa>big) {
935 i=l1;
936 big=testa;
937 }
938 }
939 }
940 if (big<=DBL_EPSILON) iet=1; /* test for div by 0 */
941
942 line[i]=1; /* selected unused line becomes used line */
943 isub[j]=i; /* isub points to j-th row of tri. matrix */
944
945 sum=1.0/(*(a+i*nstore+j));
946
947 /*reduce matrix towards triangle */
948 for (k=0; k<n; k++) {
949 if (line[k]==0) {
950 b=(*(a+k*nstore+j))*sum;
951 for (l=j+1; l<n; l++) *(a+k*nstore+l) -= (b*(*(a+i*nstore+l)));
952 *(a+k*nstore+j)=b;
953 }
954 }
955 }
956
957 for( j=0; j<n; j++ ) {
958 /* find last unused row and set its pointer */
959 /* this row contains the apex of the triangle */
960 if (line[j]==0) {
961 l1=j; /* apex of triangle */
962 isub[n-1]=j;
963 break;
964 }
965 }
966
967 }
968
969 /* start backsolving */
970
971 for (i=0; i<n; i++) line[isub[i]] = i; /* invert pointers. line(i) now gives row no in triang matrix of i-th row of actual matrix */
972
973 for (j=0; j<n-1; j++) { /* transform the vector to match triang. matrix */
974 b=vec[isub[j]];
975 for( k=0; k<n; k++ ) {
976 if (line[k]>j) vec[k] -= (b*(*(a+k*nstore+j))); /* skip elements outside of triangle */
977 }
978 }
979
980 b = *(a+l1*nstore+(n-1)); /* apex of triangle */
981 if (fabs(b)<=DBL_EPSILON) ieb=2; /* check for div by zero in backsolving */
982 vec[isub[n-1]]=vec[isub[n-1]]/b;
983
984 for (j1=n-1; j1>0; j1--) { /* backsolve rest of triangle*/
985 j = j1 - 1;
986 sum=vec[isub[j]];
987 for (j2=j+1; j2<n; j2++) sum -= (vec[isub[j2]] * (*(a+isub[j]*nstore+j2)));
988 b = *(a+isub[j]*nstore+j);
989 if (fabs(b)<=DBL_EPSILON) ieb=2; /* test for div by 0 in backsolving */
990 vec[isub[j]]=sum/b; /* solution returned in vec */
991 }
992
993 /* put the solution vector into the proper order */
994
995 for (i=0; i<n; i++) { /* reorder solution */
996 for (k=i; k<n; k++) { /* search for i-th solution element */
997 if (line[k]==i) {
998 j=k;
999 break;
1000 }
1001 }
1002 b = vec[j]; /* swap solution and pointer elements*/
1003 vec[j] = vec[i];
1004 vec[i] = b;
1005 line[j] = line[i];
1006 }
1007
1008 gmt_M_free (GMT, isub);
1009 gmt_M_free (GMT, line);
1010 return (iet + ieb); /* Return final error flag */
1011 }
1012
gmt_gaussjordan(struct GMT_CTRL * GMT,double * a,unsigned int nu,double * b)1013 int gmt_gaussjordan (struct GMT_CTRL *GMT, double *a, unsigned int nu, double *b) {
1014 int i, j, k, bad = 0, n = (int)nu; /* Doing signed ints due to restriction from OpenMP on unsigned loop variables */
1015 double c, d;
1016
1017 for (j = 0; j < (n-1); j++) { /* For all columns j */
1018 /* Find j-th pivot */
1019 k = j; c = fabs(a[k*n+j]);
1020 for (i = j + 1; i < n; i++) {
1021 if ((d = fabs(a[i*n+j])) > c) k = i, c = d;
1022 }
1023 if (c < DBL_EPSILON) {
1024 GMT_Report (GMT->parent, GMT_MSG_ERROR, "gmt_gaussjordan given a singular matrix\n");
1025 bad++;
1026 }
1027 gmtvector_switchrows (a, b, j, k, n); /* Pivot rows */
1028 #ifdef _OPENMP
1029 #pragma omp parallel for private(i,k,c) shared(GMT,a,b,j,n)
1030 #endif
1031 for (i = j + 1; i < n; i++) {
1032 c = a[i*n+j] / a[j*n+j];
1033 for (k = j + 1; k < n; k++) a[i*n+k] -= c*a[j*n+k];
1034 b[i] -= c*b[j];
1035 }
1036 }
1037
1038 b[n-1] /= a[n*n-1];
1039 for (i = n-2; i >= 0; i--) {
1040 c = 0;
1041 for (j = i + 1; j < n; j++) c += a[i*n+j] * b[j];
1042 b[i] = (b[i] - c) / a[i*n+i];
1043 }
1044
1045 return (bad);
1046 }
1047
1048 #ifndef __APPLE__ /* Since it is already declared in Accelerate.h */
1049 extern int dsyev_ (char* jobz, char* uplo, int* n, double* a, int* lda, double* w, double* work, int* lwork, int* info);
1050 #endif
1051
gmt_svdcmp(struct GMT_CTRL * GMT,double * a,unsigned int m_in,unsigned int n_in,double * w,double * v)1052 int gmt_svdcmp (struct GMT_CTRL *GMT, double *a, unsigned int m_in, unsigned int n_in, double *w, double *v) {
1053 /* Front for SVD calculations */
1054 #ifdef HAVE_LAPACK
1055 /* Here we use Lapack */
1056 int n = m_in, lda = m_in, info, lwork;
1057 double wkopt, *work = NULL;
1058 gmt_M_unused(n_in); /* Since we are actually only doing square matrices... */
1059 gmt_M_unused(v);
1060 GMT_Report (GMT->parent, GMT_MSG_INFORMATION, "gmt_svdcmp: Using Lapack dsyev\n");
1061 /* Query and allocate the optimal workspace */
1062 lwork = -1;
1063 dsyev_ ( "Vectors", "Upper", &n, a, &lda, w, &wkopt, &lwork, &info );
1064 lwork = (int)wkopt;
1065 work = gmt_M_memory (GMT, NULL, lwork, double);
1066 /* Solve eigenproblem */
1067 dsyev_ ( "Vectors", "Upper", &n, a, &lda, w, work, &lwork, &info );
1068 /* Check for convergence */
1069 if (info > 0 ) {
1070 GMT_Report (GMT->parent, GMT_MSG_ERROR, "gmt_svdcmp: dsyev failed to compute eigenvalues.\n" );
1071 return (GMT_RUNTIME_ERROR);
1072 }
1073 /* Free workspace */
1074 gmt_M_free (GMT, work);
1075 /* No separate v matrix but stored in a, so... */
1076 v = a;
1077 return (GMT_NOERROR);
1078 #else
1079 GMT_Report (GMT->parent, GMT_MSG_INFORMATION, "gmt_svdcmp: Using GMT's NR-based SVD\n");
1080 return gmtvector_svdcmp_nr (GMT, a, m_in, n_in, w, v);
1081 #endif
1082 }
1083
1084 /* Given the singular value decomposition of a matrix a[0...m-1][0...n-1]
1085 solve the system of equations ax=b for x. Input the matrices
1086 U[0....m-1][0...n-1],w[0...n-1], and V[0...n-1][0...n-1] determined from
1087 svdcmp. Also input the matrix b[0...m-1][0....k-1] and the solution vector
1088 x[0....k-1][0....n-1] is output. Singular values whose ratio to the maximum
1089 singular value are smaller than cutoff are zeroed out. The matrix U is
1090 overwritten.
1091
1092 */
1093
gmt_sort_svd_values(struct GMT_CTRL * GMT,double * w,unsigned int n)1094 struct GMT_SINGULAR_VALUE * gmt_sort_svd_values (struct GMT_CTRL *GMT, double *w, unsigned int n) {
1095 /* Store the eigenvalues in a structure and sort it so that the array is
1096 * sorted from large to small values while the order reflects the original position in w */
1097 struct GMT_SINGULAR_VALUE *eigen = gmt_M_memory (GMT, NULL, n, struct GMT_SINGULAR_VALUE);
1098 for (unsigned int i = 0; i < n; i++) { /* Load in original order from |w| */
1099 eigen[i].value = fabs (w[i]);
1100 eigen[i].order = i;
1101 }
1102 qsort (eigen, n, sizeof (struct GMT_SINGULAR_VALUE), gmtvector_compare_singular_values);
1103 return (eigen);
1104 }
1105
gmt_solve_svd(struct GMT_CTRL * GMT,double * u,unsigned int m,unsigned int nu,double * v,double * w,double * b,unsigned int k,double * x,double cutoff,unsigned int mode)1106 int gmt_solve_svd (struct GMT_CTRL *GMT, double *u, unsigned int m, unsigned int nu, double *v, double *w, double *b, unsigned int k, double *x, double cutoff, unsigned int mode) {
1107 /* Mode = 0 [GMT_SVD_EIGEN_RATIO_CUTOFF]: Use all singular values s_j for which s_j/s_0 > cutoff [0 = all]
1108 * mode = 1 [GMT_SVD_EIGEN_NUMBER_CUTOFF]: Use the first cutoff singular values only.
1109 * mode = 2 [GMT_SVD_EIGEN_PERCENT_CUTOFF]: Use a percentage fraction of eigenvalues we want.
1110 * mode = 3 [GMT_SVD_EIGEN_VARIANCE_CUTOFF]: Use a percentage fraction of explained variance to find the eigenvalues we want.
1111 * We return the number of eigenvalues that passed the checks.
1112 */
1113 double w_abs, sing_max, percent;
1114 int i, j, n_use = 0, n = (int)nu; /* Because OpenMP cannot handle unsigned loop variables */
1115 double s, *tmp = gmt_M_memory (GMT, NULL, n, double);
1116 gmt_M_unused(m); /* Since we are actually only doing square matrices... */
1117 gmt_M_unused(k); /* Since we are actually only doing one rhs row here */
1118 #ifdef HAVE_LAPACK
1119 gmt_M_unused(v); /* Not used when we solve via Lapack */
1120 #endif
1121 GMT_Report (GMT->parent, GMT_MSG_INFORMATION, "gmt_solve_svd: Evaluate solution\n");
1122
1123 /* Find maximum singular value. Assumes w[] may have negative eigenvalues */
1124 sing_max = fabs (w[0]);
1125 for (i = 1; i < n; i++) {
1126 w_abs = fabs (w[i]);
1127 sing_max = MAX (sing_max, w_abs);
1128 }
1129
1130 if (mode == GMT_SVD_EIGEN_PERCENT_CUTOFF) { /* Gave desired fraction of eigenvalues to use instead; scale to # of values */
1131 double was = cutoff;
1132 cutoff = rint (n*cutoff);
1133 mode = GMT_SVD_EIGEN_NUMBER_CUTOFF;
1134 GMT_Report (GMT->parent, GMT_MSG_INFORMATION, "gmt_solve_svd: Given fraction %g corresponds to %d eigenvalues\n", was, irint(cutoff));
1135 }
1136 else if (mode == GMT_SVD_EIGEN_VARIANCE_CUTOFF) { /* Determine N cutoff via model variance desired */
1137 struct GMT_SINGULAR_VALUE *eigen = gmt_sort_svd_values (GMT, w, n); /* Sort the eigenvalues */
1138 double l2_sum_n = 0.0, l2_sum_k = 0.0;
1139 for (i = 0; i < n; i++) /* Get total sum of squared eigenvalues */
1140 l2_sum_n += eigen[i].value * eigen[i].value;
1141 l2_sum_n *= cutoff; /* Fraction of explained model variance */
1142 for (i = 0; i < n; i++) { /* Determine i that gives >= variance percentage */
1143 l2_sum_k += eigen[i].value * eigen[i].value;
1144 if (l2_sum_k >= l2_sum_n) break;
1145 }
1146 cutoff = i;
1147 mode = GMT_SVD_EIGEN_NUMBER_CUTOFF;
1148 }
1149
1150 if (mode == GMT_SVD_EIGEN_NUMBER_CUTOFF) {
1151 /* Find the m largest singular values, with m = cutoff (if <1 it is the fraction of values).
1152 * Either case requires sorted singular values so we need to do some work first.
1153 * It also assumes that the matrix passed is a squared normal equation kind of matrix
1154 * so that the singular values are the individual variance contributions. */
1155 struct GMT_SINGULAR_VALUE *eigen = gmt_sort_svd_values (GMT, w, n); /* Sort the eigenvalues */
1156 int n_eigen = (unsigned int)lrint (cutoff); /* Desired number of eigenvalues to use instead */
1157
1158 for (i = 0; i < n; i++) { /* Visit all singular values in decreasing magnitude */
1159 if (i < n_eigen) { /* Still within specified limit so we add this singular value */
1160 w[eigen[i].order] = 1.0 / w[eigen[i].order];
1161 n_use++;
1162 }
1163 else /* Sorry, we're letting you go */
1164 w[eigen[i].order] = 0.0;
1165 }
1166 gmt_M_free (GMT, eigen);
1167 }
1168 else { /* Loop through singular values removing small ones (if cutoff is nonzero) */
1169 for (i = 0; i < n; i++) {
1170 w_abs = fabs (w[i]);
1171 if ((w_abs/sing_max) > cutoff) {
1172 w[i] = 1.0 / w[i];
1173 n_use++;
1174 }
1175 else
1176 w[i] = 0.0;
1177 }
1178 }
1179 percent = 100.0 * (double)n_use / (double)n;
1180 if (mode == GMT_SVD_EIGEN_RATIO_CUTOFF)
1181 GMT_Report (GMT->parent, GMT_MSG_INFORMATION,
1182 "gmt_solve_svd: Ratio limit %g retained %d singular values (%.1lf%%\n", cutoff, n_use, percent);
1183 else
1184 GMT_Report (GMT->parent, GMT_MSG_INFORMATION,
1185 "gmt_solve_svd: Selected the largest %d singular values (%.1lf%%)\n", n_use, percent);
1186
1187 /* Here w contains 1/eigenvalue so we multiply by w if w != 0 */
1188
1189 #ifdef HAVE_LAPACK
1190 /* New Lapack SVD evaluation */
1191 for (j = 0; j < n; j++) {
1192 if (w[j] == 0.0) continue; /* No point adding up if multiplying the sum by zero */
1193 s = 0.0;
1194 for (i = 0; i < n; i++) s += u[j*n+i]*b[i]; /* Calculate v'*b */
1195 tmp[j] = s * w[j]; /* Now have temp = inv(w)*v'*b */
1196 }
1197 #ifdef _OPENMP
1198 #pragma omp parallel for private(i,j,s) shared(u,tmp,n,x)
1199 #endif
1200 for (j = 0; j < n; j++) { /* Now premultiply by v */
1201 s = 0.0;
1202 for (i = 0; i < n; i++) s += u[i*n+j]*tmp[i];
1203 x[j] = s;
1204 }
1205 #else
1206 for (j = 0; j < n; j++) {
1207 if (w[j] == 0.0) continue; /* No point adding up if multiplying the sum by zero */
1208 s = 0.0;
1209 for (i = 0; i < n; i++) s += u[i*n+j]*b[i]; /* Calculate v'*b */
1210 tmp[j] = s * w[j]; /* Now have temp = inv(w)*v'*b */
1211 }
1212 #ifdef _OPENMP
1213 #pragma omp parallel for private(i,j,s) shared(v,tmp,n,x)
1214 #endif
1215 for (j = 0; j < n; j++) { /* Now premultiply by v */
1216 s = 0.0;
1217 for (i = 0; i < n; i++) s += v[j*n+i]*tmp[i];
1218 x[j] = s;
1219 }
1220 #endif
1221
1222 gmt_M_free (GMT, tmp);
1223 return (n_use);
1224 }
1225
gmt_dot3v(struct GMT_CTRL * GMT,double * a,double * b)1226 double gmt_dot3v (struct GMT_CTRL *GMT, double *a, double *b) {
1227 gmt_M_unused(GMT);
1228 return (a[GMT_X]*b[GMT_X] + a[GMT_Y]*b[GMT_Y] + a[GMT_Z]*b[GMT_Z]);
1229 }
1230
gmt_dot2v(struct GMT_CTRL * GMT,double * a,double * b)1231 double gmt_dot2v (struct GMT_CTRL *GMT, double *a, double *b) {
1232 gmt_M_unused(GMT);
1233 return (a[GMT_X]*b[GMT_X] + a[GMT_Y]*b[GMT_Y]);
1234 }
1235
gmt_mag3v(struct GMT_CTRL * GMT,double * a)1236 double gmt_mag3v (struct GMT_CTRL *GMT, double *a) {
1237 gmt_M_unused(GMT);
1238 return (d_sqrt(a[GMT_X]*a[GMT_X] + a[GMT_Y]*a[GMT_Y] + a[GMT_Z]*a[GMT_Z]));
1239 }
1240
gmt_add3v(struct GMT_CTRL * GMT,double * a,double * b,double * c)1241 void gmt_add3v (struct GMT_CTRL *GMT, double *a, double *b, double *c) {
1242 /* C = A + B */
1243 int k;
1244 gmt_M_unused(GMT);
1245 for (k = 0; k < 3; k++) c[k] = a[k] + b[k];
1246 }
1247
gmt_sub3v(struct GMT_CTRL * GMT,double * a,double * b,double * c)1248 void gmt_sub3v (struct GMT_CTRL *GMT, double *a, double *b, double *c) {
1249 /* C = A - B */
1250 int k;
1251 gmt_M_unused(GMT);
1252 for (k = 0; k < 3; k++) c[k] = a[k] - b[k];
1253 }
1254
gmt_normalize3v(struct GMT_CTRL * GMT,double * a)1255 void gmt_normalize3v (struct GMT_CTRL *GMT, double *a) {
1256 double r_length;
1257 r_length = gmt_mag3v (GMT,a);
1258 if (r_length != 0.0) {
1259 r_length = 1.0 / r_length;
1260 a[GMT_X] *= r_length;
1261 a[GMT_Y] *= r_length;
1262 a[GMT_Z] *= r_length;
1263 }
1264 }
1265
gmt_normalize2v(struct GMT_CTRL * GMT,double * a)1266 void gmt_normalize2v (struct GMT_CTRL *GMT, double *a) {
1267 double r_length;
1268 gmt_M_unused(GMT);
1269 r_length = hypot (a[GMT_X], a[GMT_Y]);
1270 if (r_length != 0.0) {
1271 r_length = 1.0 / r_length;
1272 a[GMT_X] *= r_length;
1273 a[GMT_Y] *= r_length;
1274 }
1275 }
1276
gmt_cross3v(struct GMT_CTRL * GMT,double * a,double * b,double * c)1277 void gmt_cross3v (struct GMT_CTRL *GMT, double *a, double *b, double *c) {
1278 gmt_M_unused(GMT);
1279 c[GMT_X] = a[GMT_Y] * b[GMT_Z] - a[GMT_Z] * b[GMT_Y];
1280 c[GMT_Y] = a[GMT_Z] * b[GMT_X] - a[GMT_X] * b[GMT_Z];
1281 c[GMT_Z] = a[GMT_X] * b[GMT_Y] - a[GMT_Y] * b[GMT_X];
1282 }
1283
gmt_matrix_vect_mult(struct GMT_CTRL * GMT,unsigned int dim,double a[3][3],double b[3],double c[3])1284 void gmt_matrix_vect_mult (struct GMT_CTRL *GMT, unsigned int dim, double a[3][3], double b[3], double c[3]) {
1285 /* c = A * b for 2 or 3 D */
1286 unsigned int i, j;
1287 gmt_M_unused(GMT);
1288
1289 for (i = 0; i < dim; i++) for (j = 0, c[i] = 0.0; j < dim; j++) c[i] += a[i][j] * b[j];
1290 }
1291
1292 /* Things to use if figuring out blas calls to speed up these multiplications:
1293 * Then, if LAPACK is true then gmt_matrix_matrix_mult should call dgemm_ instead of plain code.
1294 */
1295
1296 extern int dgemm_ (char* tra, char* trb, int* na, int* nb, int* nc, double* alpha, double* a, int *nd, double* b, int *ne, double* beta, double* c, int* nf);
1297
gmt_matrix_vector_mult(struct GMT_CTRL * GMT,double * A,double * b,uint64_t n_rowsA,uint64_t n_colsA,double * c)1298 void gmt_matrix_vector_mult (struct GMT_CTRL *GMT, double *A, double *b, uint64_t n_rowsA, uint64_t n_colsA, double *c) {
1299 uint64_t row, col, ij;
1300 gmt_M_unused(GMT);
1301 gmt_M_memset (c, n_colsA, double);
1302 for (row = ij = 0; row < n_rowsA; row++) {
1303 for (col = 0; col < n_colsA; col++, ij++)
1304 c[row] += A[ij] * b[col];
1305 }
1306 }
1307
gmt_matrix_matrix_mult(struct GMT_CTRL * GMT,double * A,double * B,uint64_t n_rowsA,uint64_t n_rowsB,uint64_t n_colsB,double * C)1308 void gmt_matrix_matrix_mult (struct GMT_CTRL *GMT, double *A, double *B, uint64_t n_rowsA, uint64_t n_rowsB, uint64_t n_colsB, double *C) {
1309 #ifdef HAVE_LAPACK
1310 double one = 1.0, zero = 0.0;
1311 int na, nb, nc, nd, ne, nf;
1312 char tr[2] = {'t', '\0'}; /* If B is a vector we must switch to n */
1313 gmt_M_unused(GMT);
1314 gmt_M_memset (C, n_rowsA * n_colsB, double);
1315 na = (int)n_rowsA, nb = (int)n_colsB, nc = (int)n_rowsB, nd = (int)n_rowsB, ne = (int)n_colsB, nf = (int)n_colsB;
1316 // cblas_dgemm (CblasRowMajor, CblasNoTrans, CblasNoTrans, (int)n_rowsA, (int)n_colsB, (int)n_rowsB, one, A, (int)n_rowsB, B, (int)n_colsB, zero, C, (int)n_colsB);
1317 if (n_colsB == 1) { /* Do those separately */
1318 gmt_matrix_vector_mult (GMT, A, B, n_rowsA, n_rowsB, C);
1319 return;
1320 }
1321 #if 0
1322 if (n_colsB == 1) { /* Vector, so no transposing of the matrix */
1323 tr[0] = 'n';
1324 ne = nf = (int)n_rowsB;
1325 }
1326 #endif
1327 dgemm_ ("t", tr, &na, &nb, &nc, &one, A, &nd, B, &ne, &zero, C, &nf);
1328 #else
1329 /* Plain matrix multiplication, no speed up; space must exist */
1330 uint64_t row, col, k, a_ij, b_ij, c_ij, n_colsA = n_rowsB;
1331 gmt_M_unused(GMT);
1332 if (n_colsB == 1) { /* Do those separately */
1333 gmt_matrix_vector_mult (GMT, A, B, n_rowsA, n_rowsB, C);
1334 return;
1335 }
1336 for (row = 0; row < n_rowsA; row++) {
1337 for (col = 0; col < n_colsB; col++) {
1338 a_ij = row * n_colsA; /* Start address of row in A */
1339 b_ij = col; /* Start address of col in B */
1340 c_ij = row * n_colsB + col; /* Address of C element to hold their dot-product */
1341 C[c_ij] = 0.0;
1342 for (k = 0; k < n_rowsB; k++) /* Do the dot product */
1343 C[c_ij] += A[a_ij+k]*B[b_ij+k*n_colsB];
1344 }
1345 }
1346 #endif
1347 }
1348
gmt_matrix_matrix_add(struct GMT_CTRL * GMT,double * A,double * B,uint64_t n_rowsA,uint64_t n_colsA,double * C)1349 void gmt_matrix_matrix_add (struct GMT_CTRL *GMT, double *A, double *B, uint64_t n_rowsA, uint64_t n_colsA, double *C) {
1350 gmt_M_unused(GMT);
1351 uint64_t row, col, ij;
1352 for (row = ij = 0; row < n_rowsA; row++) {
1353 for (col = 0; col < n_colsA; col++, ij++) {
1354 C[ij] = A[ij] + B[ij];
1355 }
1356 }
1357 }
1358
gmt_make_rot_matrix2(struct GMT_CTRL * GMT,double E[3],double w,double R[3][3])1359 void gmt_make_rot_matrix2 (struct GMT_CTRL *GMT, double E[3], double w, double R[3][3]) {
1360 /* Based on Cox and Hart, 1986 */
1361 /* E Euler pole in in cartesian coordinates
1362 * w angular rotation in degrees
1363 *
1364 * R the 3x3 rotation matrix
1365 */
1366
1367 double sin_w, cos_w, c, E_x, E_y, E_z, E_12c, E_13c, E_23c;
1368 gmt_M_unused(GMT);
1369
1370 sincosd (w, &sin_w, &cos_w);
1371 c = 1 - cos_w;
1372
1373 E_x = E[0] * sin_w;
1374 E_y = E[1] * sin_w;
1375 E_z = E[2] * sin_w;
1376 E_12c = E[0] * E[1] * c;
1377 E_13c = E[0] * E[2] * c;
1378 E_23c = E[1] * E[2] * c;
1379
1380 R[0][0] = E[0] * E[0] * c + cos_w;
1381 R[0][1] = E_12c - E_z;
1382 R[0][2] = E_13c + E_y;
1383
1384 R[1][0] = E_12c + E_z;
1385 R[1][1] = E[1] * E[1] * c + cos_w;
1386 R[1][2] = E_23c - E_x;
1387
1388 R[2][0] = E_13c - E_y;
1389 R[2][1] = E_23c + E_x;
1390 R[2][2] = E[2] * E[2] * c + cos_w;
1391 }
1392
gmt_make_rot_matrix(struct GMT_CTRL * GMT,double lonp,double latp,double w,double R[3][3])1393 void gmt_make_rot_matrix (struct GMT_CTRL *GMT, double lonp, double latp, double w, double R[3][3]) {
1394 /* lonp, latp Euler pole in degrees
1395 * w angular rotation in degrees
1396 *
1397 * R the rotation matrix
1398 */
1399
1400 double E[3];
1401
1402 gmt_geo_to_cart (GMT, latp, lonp, E, true);
1403 gmt_make_rot_matrix2 (GMT, E, w, R);
1404 }
1405
gmt_geo_to_cart(struct GMT_CTRL * GMT,double lat,double lon,double * a,bool degrees)1406 void gmt_geo_to_cart (struct GMT_CTRL *GMT, double lat, double lon, double *a, bool degrees) {
1407 /* Convert geographic latitude and longitude (lat, lon)
1408 to a 3-vector of unit length (a). If degrees = true,
1409 input coordinates are in degrees, otherwise in radian */
1410
1411 double clat, clon, slon;
1412 gmt_M_unused(GMT);
1413
1414 if (degrees) {
1415 lat *= D2R;
1416 lon *= D2R;
1417 }
1418 sincos (lat, &a[GMT_Z], &clat);
1419 sincos (lon, &slon, &clon);
1420 a[GMT_X] = clat * clon;
1421 a[GMT_Y] = clat * slon;
1422 }
1423
gmt_cart_to_geo(struct GMT_CTRL * GMT,double * lat,double * lon,double * a,bool degrees)1424 void gmt_cart_to_geo (struct GMT_CTRL *GMT, double *lat, double *lon, double *a, bool degrees) {
1425 /* Convert a 3-vector (a) of unit length into geographic
1426 coordinates (lat, lon). If degrees = true, the output coordinates
1427 are in degrees, otherwise in radian. */
1428
1429 gmt_M_unused(GMT);
1430 if (degrees) {
1431 *lat = d_asind (a[GMT_Z]);
1432 *lon = d_atan2d (a[GMT_Y], a[GMT_X]);
1433 }
1434 else {
1435 *lat = d_asin (a[GMT_Z]);
1436 *lon = d_atan2 (a[GMT_Y], a[GMT_X]);
1437 }
1438 }
1439
gmt_n_cart_to_geo(struct GMT_CTRL * GMT,uint64_t n,double * x,double * y,double * z,double * lon,double * lat)1440 void gmt_n_cart_to_geo (struct GMT_CTRL *GMT, uint64_t n, double *x, double *y, double *z, double *lon, double *lat) {
1441 /* Convert Cartesian vectors back to lon, lat vectors */
1442 uint64_t k;
1443 double V[3];
1444 for (k = 0; k < n; k++) {
1445 V[0] = x[k]; V[1] = y[k]; V[2] = z[k];
1446 gmt_cart_to_geo (GMT, &lat[k], &lon[k], V, true);
1447 }
1448 }
1449
gmt_polar_to_cart(struct GMT_CTRL * GMT,double r,double theta,double * a,bool degrees)1450 void gmt_polar_to_cart (struct GMT_CTRL *GMT, double r, double theta, double *a, bool degrees) {
1451 /* Convert polar (cylindrical) coordinates r, theta
1452 to a 2-vector of unit length (a). If degrees = true,
1453 input theta is in degrees, otherwise in radian */
1454
1455 gmt_M_unused(GMT);
1456 if (degrees) theta *= D2R;
1457 sincos (theta, &a[GMT_Y], &a[GMT_X]);
1458 a[GMT_X] *= r;
1459 a[GMT_Y] *= r;
1460 }
1461
gmt_cart_to_polar(struct GMT_CTRL * GMT,double * r,double * theta,double * a,bool degrees)1462 void gmt_cart_to_polar (struct GMT_CTRL *GMT, double *r, double *theta, double *a, bool degrees) {
1463 /* Convert a 2-vector (a) of unit length into polar (cylindrical)
1464 coordinates (r, theta). If degrees = true, the output coordinates
1465 are in degrees, otherwise in radian. */
1466
1467 gmt_M_unused(GMT);
1468 *r = hypot (a[GMT_X], a[GMT_Y]);
1469 *theta = d_atan2 (a[GMT_Y], a[GMT_X]);
1470 if (degrees) *theta *= R2D;
1471 }
1472
gmt_set_line_resampling(struct GMT_CTRL * GMT,bool active,unsigned int mode)1473 void gmt_set_line_resampling (struct GMT_CTRL *GMT, bool active, unsigned int mode) {
1474 /* Sets the GMT->current.map.path_mode setting given -A and data type.
1475 * By default, path_mode = GMT_RESAMPLE_PATH = 0. */
1476
1477 if (gmt_M_is_geographic (GMT, GMT_IN)) { /* Geographic data: Default is to resample along great circles unless -A given */
1478 if (active && mode == GMT_STAIRS_OFF) GMT->current.map.path_mode = GMT_LEAVE_PATH; /* Turn off resampling */
1479 }
1480 else { /* Cartesian data: Default is to leave alone unless -Ax|y was set */
1481 if (!active) GMT->current.map.path_mode = GMT_LEAVE_PATH; /* Turn off resampling */
1482 }
1483 }
1484
gmt_fix_up_path(struct GMT_CTRL * GMT,double ** a_lon,double ** a_lat,uint64_t n,double step,unsigned int mode)1485 uint64_t gmt_fix_up_path (struct GMT_CTRL *GMT, double **a_lon, double **a_lat, uint64_t n, double step, unsigned int mode) {
1486 /* Takes pointers to a list of <n> lon/lat pairs (in degrees) and adds
1487 * auxiliary points if the great circle distance between two given points exceeds
1488 * <step> spherical degree. If step <= 0 we use the default path_step.
1489 * If mode=0: returns points along a great circle
1490 * If mode=1: first follows meridian, then parallel
1491 * If mode=2: first follows parallel, then meridian
1492 * Returns the new number of points (original plus auxiliary).
1493 */
1494
1495 unsigned int k = 1;
1496 bool meridian, boostable;
1497 uint64_t i, j, n_new, n_step = 0;
1498 double a[3], b[3], x[3], *lon = NULL, *lat = NULL;
1499 double c, d, fraction, theta, minlon, maxlon;
1500 double dlon, lon_i, boost, f_lat_a, f_lat_b;
1501 double E[3], R[3][3], R0[3][3], ds_radians, angle_radians;
1502
1503 if (GMT->current.proj.projection_GMT == GMT_POLAR) return (gmtvector_fix_up_path_polar (GMT, a_lon, a_lat, n, 1.0, mode)); /* r-theta stepping */
1504
1505 if (gmt_M_is_cartesian (GMT, GMT_IN)) return (gmtvector_fix_up_path_cartonly (GMT, a_lon, a_lat, n, mode)); /* Stair case only */
1506
1507 lon = *a_lon; lat = *a_lat; /* Input arrays */
1508
1509 if (gmt_M_is_dnan (lon[0]) || gmt_M_is_dnan (lat[0])) { /* If user manages to pass NaN NaN records then we check on the first record and bail */
1510 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Your data array row 0 contains NaNs - no resampling taken place!\n");
1511 return n;
1512 }
1513
1514 gmt_geo_to_cart (GMT, lat[0], lon[0], a, true); /* Start point of current arc */
1515 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, 1, 2); /* Init or reallocate tmp vectors */
1516 GMT->hidden.mem_coord[GMT_X][0] = lon[0];
1517 GMT->hidden.mem_coord[GMT_Y][0] = lat[0];
1518 n_new = 1;
1519 if (step <= 0.0) step = GMT->current.map.path_step; /* Based on GMT->current.setting.map_line_step converted to degrees */
1520 if (step <= 0.0) step = 0.1; /* Safety valve when no -J and step not set. */
1521
1522 /* 1) Must be careful with connecting longitudes along a parallel since often the
1523 * longitudes might be of different sign. E.g., first may be +115 and the second is -165.
1524 * Naive math would find a jump of -280 degrees but really it is just 80. The test below
1525 * tries to handle these artificial jumps.
1526 * 2) When very close to a pole the distance between two input points can be very small
1527 * and hence the number of steps n_step will be small. This can lead to large jumps in
1528 * longitude that can later confuse us as to when we cross a periodic boundary.
1529 * We try to mitigate that by scaling up the number of steps by a boost factor that is 1
1530 * away from poles and from |lat| = 75 increases to 150 very close to the pole. */
1531 boostable = !(gmt_M_is_linear (GMT) || gmt_M_pole_is_point (GMT)); /* Only boost for projections where poles are lines */
1532 f_lat_a = fabs (lat[0]);
1533 for (i = 1; i < n; i++) {
1534 if (gmt_M_is_dnan (lon[i]) || gmt_M_is_dnan (lat[i])) { /* If user manages to pass NaN NaN records then we check on the first record and bail */
1535 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Your data array row %" PRIu64 " contains NaNs - no resampling taken place!\n", i);
1536 return n;
1537 }
1538 f_lat_b = fabs (lat[i]);
1539
1540 gmt_geo_to_cart (GMT, lat[i], lon[i], b, true); /* End point of current arc */
1541 if (boostable) { /* Enforce closer sampling close to poles or for near anti-meridian separation of points */
1542 gmt_M_set_delta_lon (lon[i-1], lon[i], dlon); /* Beware of jumps due to sign differences */
1543 if (MIN (f_lat_a, f_lat_b) > 75.0) /* Enforce closer sampling close to poles */
1544 boost = 1.0 + 10.0 * (MAX(f_lat_a, f_lat_b) - 75.0); /* Crude way to get a boost from 1 at 75 to ~151 at the pole */
1545 else if ((c = fabs (fabs (dlon)-180.0)) < 5.0) /* Enforce closer sampling if points are ~180 degrees apart in longitude */
1546 boost = 1.0 + 10.0 * c; /* Crude way to get a boost when we are close to points along antimeridian */
1547 else
1548 boost = 1.0;
1549 }
1550 else /* No boost needed */
1551 boost = 1.0;
1552
1553 if (mode == GMT_STAIRS_Y) { /* First follow meridian, then parallel */
1554 gmt_M_set_delta_lon (lon[i-1], lon[i], dlon); /* Beware of jumps due to sign differences */
1555 lon_i = lon[i-1] + dlon; /* Use lon_i instead of lon[i] in the marching since this avoids any jumping */
1556 theta = fabs (dlon) * cosd (lat[i-1]);
1557 n_step = lrint (theta / step);
1558 for (j = 1; j <= n_step; j++) {
1559 c = j / (double)n_step;
1560 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp vectors */
1561 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i-1] * (1 - c) + lon_i * c;
1562 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i-1];
1563 n_new++;
1564 }
1565 theta = fabs (lat[i]-lat[i-1]);
1566 n_step = lrint (theta / step);
1567 for (j = k; j < n_step; j++) { /* Start at 0 to make sure corner point is saved */
1568 c = j / (double)n_step;
1569 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp vectors */
1570 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i];
1571 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i-1] * (1 - c) + lat[i] * c;
1572 n_new++;
1573 }
1574 k = 0;
1575 }
1576
1577 else if (mode == GMT_STAIRS_X) { /* First follow parallel, then meridian */
1578 theta = fabs (lat[i]-lat[i-1]);
1579 n_step = lrint (theta / step);
1580 for (j = 1; j <= n_step; j++) {
1581 c = j / (double)n_step;
1582 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1583 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i-1];
1584 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i-1] * (1 - c) + lat[i] * c;
1585 n_new++;
1586 }
1587 gmt_M_set_delta_lon (lon[i-1], lon[i], dlon); /* Beware of jumps due to sign differences */
1588 lon_i = lon[i-1] + dlon; /* Use lon_i instead of lon[i] in the marching since this avoids any jumping */
1589 theta = fabs (dlon) * cosd(lat[i]);
1590 n_step = lrint (theta / step);
1591 for (j = k; j < n_step; j++) { /* Start at 0 to make sure corner point is saved */
1592 c = j / (double)n_step;
1593 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1594 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i-1] * (1 - c) + lon_i * c;
1595 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i];
1596 n_new++;
1597 }
1598 k = 0;
1599 }
1600 /* Follow great circle */
1601 else if ((theta = d_acosd (gmt_dot3v (GMT, a, b))) == 180.0) { /* trouble, no unique great circle */
1602 if (gmt_M_is_spherical (GMT) || ((lat[i] + lat[i-1]) == 0.0)) {
1603 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Two points in input list are antipodal - great circle resampling is not unique!\n");
1604 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Fix input data or use project -A to generate the desired great circle by providing an azimuth.\n");
1605 }
1606 else {
1607 GMT_Report (GMT->parent, GMT_MSG_WARNING, "Two points in input list are antipodal - great circle resampling is not unique!\n");
1608 GMT_Report (GMT->parent, GMT_MSG_WARNING, "There are two possible geodesics but GMT does not currently calculate geodesics.\n");
1609 }
1610 return 0;
1611 }
1612 else if (doubleAlmostEqual (fabs (fmod (lon[i-1] - lon[i], 360)), 180.0)) {
1613 /* Must march along the two meridians through the nearest pole */
1614 double sL = lat[i-1] + lat[i], dy;
1615 double Narc = 180.0 - sL, Sarc = 180.0 + sL; /* Compute the arc lengths of the two pole passes */
1616 if (Narc < Sarc) { /* Shortest path is to connect through N pole */
1617 n_step = lrint ((90.0 - lat[i-1]) / step); /* Must insert (n_step - 1) points, i.e. create n_step intervals */
1618 dy = (90.0 - lat[i-1]) / n_step; /* Ensure we land exactly on N pole */
1619 for (j = 1; j <= n_step; j++) { /* Start at 1 since 0 is lon[i-1] and end exactly at pole */
1620 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1621 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i-1]; /* Keep longitude constant */
1622 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i-1] + j * dy; /* March towards N pole */
1623 n_new++;
1624 }
1625 n_step = lrint ((90.0 - lat[i]) / step); /* Must insert (n_step - 1) points, i.e. create n_step intervals */
1626 dy = (90.0 - lat[i]) / n_step; /* Ensure we start exactly on N pole */
1627 for (j = 0; j < n_step; j++) { /* Start at 0 to pick up N pole */
1628 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1629 GMT->hidden.mem_coord[GMT_X][n_new] = lon[1];
1630 GMT->hidden.mem_coord[GMT_Y][n_new] = 90.0 - j * dy;
1631 n_new++;
1632 }
1633 }
1634 else { /* Must connect via S pole */
1635 n_step = lrint ((lat[i-1] + 90.0) / step); /* Must insert (n_step - 1) points, i.e. create n_step intervals */
1636 dy = (lat[i-1] + 90.0) / n_step; /* Ensure we land exactly on S pole */
1637 for (j = 1; j <= n_step; j++) { /* Start at 1 since 0 is lon[i-1] and end exactly at pole */
1638 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1639 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i-1];
1640 GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i-1] - j * dy;
1641 n_new++;
1642 }
1643 n_step = lrint ((lat[i] + 90.0) / step); /* Must insert (n_step - 1) points, i.e. create n_step intervals */
1644 dy = (lat[i] + 90.0) / n_step; /* Ensure we start exactly on S pole */
1645 for (j = 0; j < n_step; j++) { /* Start at 0 to pick up S pole */
1646 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1647 GMT->hidden.mem_coord[GMT_X][n_new] = lon[1];
1648 GMT->hidden.mem_coord[GMT_Y][n_new] = j * dy - 90.0;
1649 n_new++;
1650 }
1651 }
1652 /* Next point is now point i */
1653 }
1654 else if ((n_step = lrint (boost * theta / step)) > 1) { /* Must insert (n_step - 1) points, i.e. create n_step intervals */
1655 /* Do this by crossing the end point vectors to get a rotation pole, then rotate a towards b in equal angular steps */
1656 if (GMT->hidden.sample_along_arc) { /* Need accurate incremental spacing so do the slower along-arc approach */
1657 gmt_cross3v (GMT, a, b, E); /* Get pole E to plane trough a and b */
1658 gmt_normalize3v (GMT, E); /* Make sure E has unit length */
1659 gmtlib_init_rot_matrix (R0, E); /* Get partial rotation matrix since no actual angle is applied yet */
1660 ds_radians = D2R * theta / n_step;
1661 for (j = 1; j < n_step; j++) {
1662 angle_radians = j * ds_radians; /* The required rotation for this point relative to FZ origin */
1663 gmt_M_memcpy (R, R0, 9, double); /* Get a copy of the "0-angle" rotation matrix */
1664 gmtlib_load_rot_matrix (angle_radians, R, E); /* Build the actual rotation matrix for this angle */
1665 gmt_matrix_vect_mult (GMT, 3U, R, a, x); /* Rotate a along the arc towards b */
1666 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1667 gmt_cart_to_geo (GMT, &GMT->hidden.mem_coord[GMT_Y][n_new], &GMT->hidden.mem_coord[GMT_X][n_new], x, true); /* Get lon/lat of this point along arc */
1668 n_new++;
1669 }
1670 }
1671 else { /* Use the linear interpolation along the cord which is faster */
1672 fraction = 1.0 / (double)n_step;
1673 minlon = MIN (lon[i-1], lon[i]);
1674 maxlon = MAX (lon[i-1], lon[i]);
1675 meridian = doubleAlmostEqualZero (maxlon, minlon); /* A meridian; make a gap so tests below will give right range */
1676 for (j = 1; j < n_step; j++) {
1677 c = j * fraction;
1678 d = 1 - c;
1679 for (k = 0; k < 3; k++) x[k] = a[k] * d + b[k] * c;
1680 gmt_normalize3v (GMT, x); /* Make unit vector */
1681 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1682 gmt_cart_to_geo (GMT, &GMT->hidden.mem_coord[GMT_Y][n_new], &GMT->hidden.mem_coord[GMT_X][n_new], x, true);
1683 if (meridian)
1684 GMT->hidden.mem_coord[GMT_X][n_new] = minlon;
1685 else if (GMT->hidden.mem_coord[GMT_X][n_new] < minlon)
1686 GMT->hidden.mem_coord[GMT_X][n_new] += 360.0;
1687 else if (GMT->hidden.mem_coord[GMT_X][n_new] > maxlon)
1688 GMT->hidden.mem_coord[GMT_X][n_new] -= 360.0;
1689 n_new++;
1690 }
1691 }
1692 }
1693 gmt_prep_tmp_arrays (GMT, GMT_NOTSET, n_new, 2); /* Init or reallocate tmp read vectors */
1694 GMT->hidden.mem_coord[GMT_X][n_new] = lon[i]; GMT->hidden.mem_coord[GMT_Y][n_new] = lat[i];
1695 n_new++;
1696 gmt_M_cpy3v (a, b);
1697 f_lat_a = f_lat_b;
1698 }
1699
1700 /* Destroy old allocated memory and put the new one in place */
1701 gmt_M_free (GMT, lon);
1702 gmt_M_free (GMT, lat);
1703 gmt_eliminate_lon_jumps (GMT, GMT->hidden.mem_coord[GMT_X], n_new); /* Ensure longitudes are in the same quadrants */
1704 *a_lon = gmtlib_assign_vector (GMT, n_new, GMT_X);
1705 *a_lat = gmtlib_assign_vector (GMT, n_new, GMT_Y);
1706
1707 return (n_new);
1708 }
1709
gmt_resample_path(struct GMT_CTRL * GMT,double ** x,double ** y,uint64_t n_in,double step_out,enum GMT_enum_track mode)1710 uint64_t gmt_resample_path (struct GMT_CTRL *GMT, double **x, double **y, uint64_t n_in, double step_out, enum GMT_enum_track mode) {
1711 /* Takes pointers to a list of <n_in> x/y pairs (in degrees or Cartesian units) and computes
1712 * the distance along that path. We then determine new coordinates at new distances that are
1713 * multiples of the desired step <step_out> which are in the unit set via gmt_init_distaz (geo)
1714 * or user Cartesian. The new path will always contain the first input point, but anything
1715 * beyond that start depends on the mode:
1716 * mode = GMT_TRACK_FILL : Keep input points; add intermediates if any gap exceeds step_out.
1717 * mode = GMT_TRACK_FILL_M : Same, but traverse along meridians, then parallels between points.
1718 * mode = GMT_TRACK_FILL_P : Same, but traverse along parallels, then meridians between points.
1719 * mode = GMT_TRACK_SAMPLE_FIX : Resample track equidistantly; old points may be lost. Use given spacing.
1720 * mode = GMT_TRACK_SAMPLE_ADJ : Resample track equidistantly; old points may be lost. Adjust spacing to fit tracklength exactly.
1721 * Returns the new number of points.
1722 */
1723 uint64_t n_out;
1724 if (gmt_M_is_geographic (GMT, GMT_IN))
1725 n_out = gmtvector_resample_path_spherical (GMT, x, y, n_in, step_out, mode);
1726 else
1727 n_out = gmtvector_resample_path_cartesian (GMT, x, y, n_in, step_out, mode);
1728 return (n_out);
1729 }
1730
gmt_chol_dcmp(struct GMT_CTRL * GMT,double * a,double * d,double * cond,int nr,int n)1731 int gmt_chol_dcmp (struct GMT_CTRL *GMT, double *a, double *d, double *cond, int nr, int n) {
1732
1733 /* Given a, a symmetric positive definite matrix
1734 of size n, and row dimension nr, compute a lower
1735 triangular matrix b, the Cholesky decomposition
1736 of a, so that a = bb'.
1737 The elements of b over-write the diagonal and sub-
1738 diagonal elements of a. The diagonal elements of
1739 a are saved in d, and a's super-diagonal elements
1740 are unchanged, permitting reconstruction of a in
1741 the event that the algorithm fails.
1742 Does not do any pivoting or balancing (I wrote it
1743 for application to Gram matrices, where all the
1744 diagonal elements of a are the same size.)
1745 Returns 0 on success, -k on failure, where k is
1746 the number (in 1 to n) of the point that failed.
1747 Elements a(i,j) for j < k, and a(k,k) will need
1748 restoration in this case.
1749 Success means that the procedure ran to completion
1750 without a negative square root or a divide by zero.
1751 It does not guarantee that the system is well-
1752 conditioned. The condition number is returned
1753 in cond as max(b(i,i)) / min(b(i,i)). Note that
1754 the condition number of a would be the square of
1755 this. This condition number is only set if the
1756 procedure runs successfully.
1757
1758 W H F Smith, 18 Feb 2000.
1759 */
1760 int i, j, k, ik, ij, kj, kk, nrp1;
1761 double eigmax, eigmin;
1762 gmt_M_unused(GMT);
1763
1764 nrp1 = nr + 1;
1765
1766 eigmax = eigmin = sqrt (fabs (a[0]));
1767
1768 for (k = 0, kk = 0; k < n; k++, kk+=nrp1 ) {
1769 d[k] = a[kk];
1770 for (j = 0, kj = k; j < k; j++, kj += nr) a[kk] -= (a[kj]*a[kj]);
1771 if (a[kk] <= 0.0) return (-(k+1));
1772 a[kk] = sqrt(a[kk]);
1773 if (a[kk] <= 0.0) return (-(k+1)); /* Shouldn't happen ? */
1774
1775 if (eigmax < a[kk]) eigmax = a[kk];
1776 if (eigmin > a[kk]) eigmin = a[kk];
1777
1778 for (i = k+1; i < n; i++) {
1779 ik = i + k*nr;
1780 for (j = 0, ij = i, kj = k; j < k; j++, ij+=nr, kj+=nr) a[ik] -= (a[ij]*a[kj]);
1781 a[ik] /= a[kk];
1782 }
1783 }
1784 *cond = eigmax/eigmin;
1785 return (0);
1786 }
1787
gmt_chol_recover(struct GMT_CTRL * GMT,double * a,double * d,int nr,int n,int nerr,bool donly)1788 void gmt_chol_recover (struct GMT_CTRL *GMT, double *a, double *d, int nr, int n, int nerr, bool donly) {
1789
1790 /* Given a, a symmetric positive definite matrix of row dimension nr,
1791 and size n >= abs(nerr), one uses gmt_chol_dcmp() to attempt to find
1792 b, a lower triangular Cholesky decomposition of a, so that b*b' = a.
1793 If a is (numerically) not positive definite then gmt_chol_dcmp()
1794 returns a negative integer nerr, indicating that the diagonal
1795 elements of a from a(1,1) to a(-nerr, -nerr) and the sub-diagonal
1796 elements in columns from 1 to abs(nerr)-1 have been overwritten,
1797 but the Cholesky decomposition did not run to completion. A vector
1798 d has been assigned the original diagonal elements of a from 1 to
1799 abs(nerr), in this case.
1800
1801 gmt_chol_recover() takes a and d and restores a so that some other
1802 solution of a may be attempted.
1803
1804 If (donly != 0) then only the diagonal elements of a will be restored.
1805 This might be enough if the next attempt will be to run gmt_jacobi()
1806 on a, for the jacobi routine uses only the upper right triangle of
1807 a. If (donly == 0) then all elements of a will be restored, by
1808 transposing the upper half to the lower half.
1809
1810 To use these routines, the call should be:
1811
1812 if ( (ier = gmt_chol_dcmp (a, d, &cond, nr, n) ) != 0) {
1813 gmt_chol_recover (a, d, nr, ier, donly);
1814 [and then solve some other way, e.g. gmt_jacobi]
1815 }
1816 else {
1817 gmt_chol_solv (a, x, y, nr, n);
1818 }
1819
1820 W H F Smith, 18 Feb 2000
1821 */
1822
1823 int kbad, i, j, ii, ij, ji, nrp1;
1824 gmt_M_unused(GMT);
1825
1826 kbad = abs (nerr) - 1;
1827 nrp1 = nr + 1;
1828
1829 for (i = 0, ii = 0; i <= kbad; i++, ii += nrp1) a[ii] = d[i];
1830
1831 if (donly) return;
1832
1833 for (j = 0; j < kbad; j++) {
1834 for (i = j+1, ij = j*nrp1 + 1, ji = j*nrp1 + nr; i < n; i++, ij++, ji+=nr) a[ij] = a[ji];
1835 }
1836 return;
1837 }
1838
gmt_chol_solv(struct GMT_CTRL * GMT,double * a,double * x,double * y,int nr,int n)1839 void gmt_chol_solv (struct GMT_CTRL *GMT, double *a, double *x, double *y, int nr, int n) {
1840 /* Given an n by n linear system ax = y, with a a symmetric,
1841 positive-definite matrix, y a known vector, and x an unknown
1842 vector, this routine finds x, if a holds the lower-triangular
1843 Cholesky factor of a obtained from gmt_chol_dcmp(). nr is the
1844 row dimension of a.
1845 The lower triangular Cholesky factor is b such that bb' = a,
1846 so x is found from y using bt = y, t = b'x, where t is a
1847 temporary vector. t is found by forward elimination, and
1848 then x is found by backward elimination. t is stored in x
1849 temporarily.
1850 This routine does not check the condition number of b. It
1851 assumes that gmt_chol_dcmp() ran without error, which means
1852 that all diagonal elements b[ii] are positive; these are
1853 divisors in the loops below.
1854
1855 W H F Smith, 18 Feb 2000
1856 */
1857 int i, j, ij, ji, ii, nrp1;
1858 gmt_M_unused(GMT);
1859
1860 nrp1 = nr + 1;
1861
1862 /* Find t, store in i, working forward: */
1863 for (i = 0, ii = 0; i < n; i++, ii += nrp1) {
1864 x[i] = y[i];
1865 for (j = 0, ij = i; j < i; j++, ij += nr) x[i] -= (a[ij] * x[j]);
1866 x[i] /= a[ii];
1867 }
1868
1869 /* Find x, starting from t stored in x, going backward: */
1870 for (i = n-1, ii = (n-1)*nrp1; i >= 0; i--, ii -= nrp1) {
1871 for (j = n-1, ji = (n-1)+i*nr; j > i; j--, ji--) x[i] -= (a[ji] * x[j]);
1872 x[i] /= a[ii];
1873 }
1874 return;
1875 }
1876