00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <stdlib.h>
00029 #include <stdio.h>
00030 #include <stdarg.h>
00031 #include <limits.h>
00032 #include <float.h>
00033 #include <math.h>
00034 #include <assert.h>
00035 #include <string.h>
00036 #include <errno.h>
00037 #include "version.h"
00038 #include "nan.h"
00039 #include "csa.h"
00040
00041 int csa_verbose = 0;
00042
00043 #define NPASTART 5 // Number of Points Allocated at Start
00044 #define SVD_NMAX 30 // Maximal number of iterations in svd()
00045
00046
00047 #define NPMIN_DEF 3
00048 #define NPMAX_DEF 40
00049 #define K_DEF 140
00050 #define NPPC_DEF 5
00051
00052 struct square;
00053 typedef struct square square;
00054
00055 typedef struct
00056 {
00057 square * parent;
00058 int index;
00059
00060 point vertices[3];
00061 point middle;
00062 double h;
00063 double r;
00064
00065
00066
00067
00068 int nallocated;
00069 int npoints;
00070 point** points;
00071
00072 int primary;
00073
00074
00075
00076 int hascoeffs;
00077
00078 int order;
00079
00080 } triangle;
00081
00082 struct square
00083 {
00084 csa * parent;
00085 int i, j;
00086
00087 int nallocated;
00088 int npoints;
00089 point ** points;
00090
00091 int primary;
00092
00093 triangle* triangles[4];
00094
00095 double coeffs[25];
00096 };
00097
00098 struct csa
00099 {
00100 double xmin;
00101 double xmax;
00102 double ymin;
00103 double ymax;
00104
00105 int nallocated;
00106 int npoints;
00107 point ** points;
00108
00109
00110
00111
00112 int ni;
00113 int nj;
00114 double h;
00115 square *** squares;
00116
00117 int npt;
00118 triangle** pt;
00119
00120
00121
00122
00123 int npmin;
00124
00125 int npmax;
00126
00127
00128 double k;
00129
00130
00131
00132 int nppc;
00133 };
00134
00135 static void csa_quit( char* format, ... )
00136 {
00137 va_list args;
00138
00139 fflush( stdout );
00140
00141
00142 fprintf( stderr, "error: csa: " );
00143 va_start( args, format );
00144 vfprintf( stderr, format, args );
00145 va_end( args );
00146
00147 exit( 1 );
00148 }
00149
00150
00151
00152
00153
00154
00155
00156 static void* alloc2d( int n1, int n2, size_t unitsize )
00157 {
00158 unsigned int size;
00159 char * p;
00160 char ** pp;
00161 int i;
00162
00163 assert( n1 > 0 );
00164 assert( n2 > 0 );
00165 assert( (double) n1 * (double) n2 <= (double) UINT_MAX );
00166
00167 size = n1 * n2;
00168 if ( ( p = calloc( size, unitsize ) ) == NULL )
00169 csa_quit( "alloc2d(): %s\n", strerror( errno ) );
00170
00171 assert( (double) n2 * (double) sizeof ( void* ) <= (double) UINT_MAX );
00172
00173 size = n2 * sizeof ( void* );
00174 if ( ( pp = malloc( size ) ) == NULL )
00175 csa_quit( "alloc2d(): %s\n", strerror( errno ) );
00176 for ( i = 0; i < n2; i++ )
00177 pp[i] = &p[i * n1 * unitsize];
00178
00179 return pp;
00180 }
00181
00182
00183
00184
00185 static void free2d( void* pp )
00186 {
00187 void* p;
00188
00189 assert( pp != NULL );
00190 p = ( (void **) pp )[0];
00191 free( pp );
00192 assert( p != NULL );
00193 free( p );
00194 }
00195
00196 static triangle* triangle_create( square* s, point vertices[], int index )
00197 {
00198 triangle* t = malloc( sizeof ( triangle ) );
00199
00200 t->parent = s;
00201 memcpy( t->vertices, vertices, sizeof ( point ) * 3 );
00202 t->middle.x = ( vertices[0].x + vertices[1].x + vertices[2].x ) / 3.0;
00203 t->middle.y = ( vertices[0].y + vertices[1].y + vertices[2].y ) / 3.0;
00204 t->h = s->parent->h;
00205 t->index = index;
00206
00207 t->r = 0.0;
00208 t->points = 0;
00209 t->nallocated = 0;
00210 t->npoints = 0;
00211 t->hascoeffs = 0;
00212 t->primary = 0;
00213 t->order = -1;
00214
00215 return t;
00216 }
00217
00218 static void triangle_addpoint( triangle* t, point* p )
00219 {
00220 if ( t->nallocated == t->npoints )
00221 {
00222 if ( t->nallocated == 0 )
00223 {
00224 t->points = malloc( NPASTART * sizeof ( point* ) );
00225 t->nallocated = NPASTART;
00226 }
00227 else
00228 {
00229 t->points = realloc( t->points, t->nallocated * 2 * sizeof ( point* ) );
00230 t->nallocated *= 2;
00231 }
00232 }
00233
00234 t->points[t->npoints] = p;
00235 t->npoints++;
00236 }
00237
00238 static void triangle_destroy( triangle* t )
00239 {
00240 if ( t->points != NULL )
00241 free( t->points );
00242 free( t );
00243 }
00244
00245
00246
00247
00248
00249
00250
00251 static void triangle_calculatebc( triangle* t, point* p, double bc[] )
00252 {
00253 double dx = p->x - t->vertices[0].x;
00254 double dy = p->y - t->vertices[0].y;
00255
00256 if ( t->index == 0 )
00257 {
00258 bc[1] = ( dy - dx ) / t->h;
00259 bc[2] = -( dx + dy ) / t->h;
00260 }
00261 else if ( t->index == 1 )
00262 {
00263 bc[1] = ( dx + dy ) / t->h;
00264 bc[2] = ( dy - dx ) / t->h;
00265 }
00266 else if ( t->index == 2 )
00267 {
00268 bc[1] = ( dx - dy ) / t->h;
00269 bc[2] = ( dx + dy ) / t->h;
00270 }
00271 else
00272 {
00273 bc[1] = -( dx + dy ) / t->h;
00274 bc[2] = ( dx - dy ) / t->h;
00275 }
00276 bc[0] = 1.0 - bc[1] - bc[2];
00277 }
00278
00279 static square* square_create( csa* parent, double xmin, double ymin, int i, int j )
00280 {
00281 int ii;
00282
00283 square * s = malloc( sizeof ( square ) );
00284 double h = parent->h;
00285
00286 s->parent = parent;
00287 s->i = i;
00288 s->j = j;
00289
00290 s->points = NULL;
00291 s->nallocated = 0;
00292 s->npoints = 0;
00293
00294 s->primary = 0;
00295
00296
00297
00298
00299 for ( ii = 0; ii < 4; ++ii )
00300 {
00301 point vertices[3];
00302
00303 vertices[0].x = xmin + h / 2.0;
00304 vertices[0].y = ymin + h / 2.0;
00305 vertices[1].x = xmin + h * ( ( ( ii + 1 ) % 4 ) / 2 );
00306 vertices[1].y = ymin + h * ( ( ( ii + 2 ) % 4 ) / 2 );
00307 vertices[2].x = xmin + h * ( ii / 2 );
00308 vertices[2].y = ymin + h * ( ( ( ii + 1 ) % 4 ) / 2 );
00309 s->triangles[ii] = triangle_create( s, vertices, ii );
00310 }
00311
00312 for ( ii = 0; ii < 25; ++ii )
00313 s->coeffs[ii] = NaN;
00314
00315 return s;
00316 }
00317
00318 static void square_destroy( square* s )
00319 {
00320 int i;
00321
00322 for ( i = 0; i < 4; ++i )
00323 triangle_destroy( s->triangles[i] );
00324 if ( s->points != NULL )
00325 free( s->points );
00326 free( s );
00327 }
00328
00329 static void square_addpoint( square* s, point* p )
00330 {
00331 if ( s->nallocated == s->npoints )
00332 {
00333 if ( s->nallocated == 0 )
00334 {
00335 s->points = malloc( NPASTART * sizeof ( point* ) );
00336 s->nallocated = NPASTART;
00337 }
00338 else
00339 {
00340 s->points = realloc( s->points, s->nallocated * 2 * sizeof ( point* ) );
00341 s->nallocated *= 2;
00342 }
00343 }
00344
00345 s->points[s->npoints] = p;
00346 s->npoints++;
00347 }
00348
00349 csa* csa_create()
00350 {
00351 csa* a = malloc( sizeof ( csa ) );
00352
00353 a->xmin = DBL_MAX;
00354 a->xmax = -DBL_MAX;
00355 a->ymin = DBL_MAX;
00356 a->ymax = -DBL_MAX;
00357
00358 a->points = malloc( NPASTART * sizeof ( point* ) );
00359 a->nallocated = NPASTART;
00360 a->npoints = 0;
00361
00362 a->ni = 0;
00363 a->nj = 0;
00364 a->h = NaN;
00365 a->squares = NULL;
00366
00367 a->npt = 0;
00368 a->pt = NULL;
00369
00370 a->npmin = NPMIN_DEF;
00371 a->npmax = NPMAX_DEF;
00372 a->k = K_DEF;
00373 a->nppc = NPPC_DEF;
00374
00375 return a;
00376 }
00377
00378 void csa_destroy( csa* a )
00379 {
00380 int i, j;
00381
00382 if ( a->squares != NULL )
00383 {
00384 for ( j = 0; j < a->nj; ++j )
00385 for ( i = 0; i < a->ni; ++i )
00386 square_destroy( a->squares[j][i] );
00387 free2d( a->squares );
00388 }
00389 if ( a->pt != NULL )
00390 free( a->pt );
00391 if ( a->points != NULL )
00392 free( a->points );
00393 free( a );
00394 }
00395
00396 void csa_addpoints( csa* a, int n, point points[] )
00397 {
00398 int na = a->nallocated;
00399 int i;
00400
00401 assert( a->squares == NULL );
00402
00403
00404
00405
00406 while ( na < a->npoints + n )
00407 na *= 2;
00408 if ( na != a->nallocated )
00409 {
00410 a->points = realloc( a->points, na * sizeof ( point* ) );
00411 a->nallocated = na;
00412 }
00413
00414 for ( i = 0; i < n; ++i )
00415 {
00416 point* p = &points[i];
00417
00418 a->points[a->npoints] = p;
00419 a->npoints++;
00420
00421 if ( p->x < a->xmin )
00422 a->xmin = p->x;
00423 if ( p->x > a->xmax )
00424 a->xmax = p->x;
00425 if ( p->y < a->ymin )
00426 a->ymin = p->y;
00427 if ( p->y > a->ymax )
00428 a->ymax = p->y;
00429 }
00430 }
00431
00432
00433
00434
00435 static void csa_setprimaryflag( csa* a )
00436 {
00437 square*** squares = a->squares;
00438 int nj1 = a->nj - 1;
00439 int ni1 = a->ni - 1;
00440 int i, j;
00441
00442 for ( j = 1; j < nj1; ++j )
00443 {
00444 for ( i = 1; i < ni1; ++i )
00445 {
00446 if ( squares[j][i]->npoints > 0 )
00447 {
00448 if ( ( i + j ) % 2 == 0 )
00449 {
00450 squares[j][i]->primary = 1;
00451 squares[j - 1][i - 1]->primary = 1;
00452 squares[j + 1][i - 1]->primary = 1;
00453 squares[j - 1][i + 1]->primary = 1;
00454 squares[j + 1][i + 1]->primary = 1;
00455 }
00456 else
00457 {
00458 squares[j - 1][i]->primary = 1;
00459 squares[j + 1][i]->primary = 1;
00460 squares[j][i - 1]->primary = 1;
00461 squares[j][i + 1]->primary = 1;
00462 }
00463 }
00464 }
00465 }
00466 }
00467
00468
00469
00470 static void csa_squarize( csa* a )
00471 {
00472 int nps[7] = { 0, 0, 0, 0, 0, 0 };
00473
00474 double dx = a->xmax - a->xmin;
00475 double dy = a->ymax - a->ymin;
00476 int npoints = a->npoints;
00477 double h;
00478 int i, j, ii, nadj;
00479
00480 if ( csa_verbose )
00481 {
00482 fprintf( stderr, "squarizing csa:\n" );
00483 fflush( stderr );
00484 }
00485
00486 assert( a->squares == NULL );
00487
00488
00489
00490
00491 h = sqrt( dx * dy * a->nppc / npoints );
00492 if ( dx < h )
00493 h = dy * a->nppc / npoints;
00494 if ( dy < h )
00495 h = dx * a->nppc / npoints;
00496 a->h = h;
00497
00498 a->ni = (int) ( ceil( dx / h ) + 2 );
00499 a->nj = (int) ( ceil( dy / h ) + 2 );
00500
00501 if ( csa_verbose )
00502 {
00503 fprintf( stderr, " %d x %d squares\n", a->ni, a->nj );
00504 fflush( stderr );
00505 }
00506
00507
00508
00509
00510 a->squares = alloc2d( a->ni, a->nj, sizeof ( void* ) );
00511 for ( j = 0; j < a->nj; ++j )
00512 for ( i = 0; i < a->ni; ++i )
00513 a->squares[j][i] = square_create( a, a->xmin + h * ( i - 1 ), a->ymin + h * ( j - 1 ), i, j );
00514
00515
00516
00517
00518 for ( ii = 0; ii < npoints; ++ii )
00519 {
00520 point* p = a->points[ii];
00521
00522 i = (int) ( floor( ( p->x - a->xmin ) / h ) + 1 );
00523 j = (int) ( floor( ( p->y - a->ymin ) / h ) + 1 );
00524 square_addpoint( a->squares[j][i], p );
00525 }
00526
00527
00528
00529
00530 csa_setprimaryflag( a );
00531
00532
00533
00534
00535
00536
00537 a->pt = malloc( ( a->ni / 2 + 1 ) * a->nj * sizeof ( triangle* ) );
00538 for ( j = 0, ii = 0, nadj = 0; j < a->nj; ++j )
00539 {
00540 for ( i = 0; i < a->ni; ++i )
00541 {
00542 square* s = a->squares[j][i];
00543
00544 if ( s->npoints > 0 )
00545 {
00546 int nn = s->npoints / 5;
00547
00548 if ( nn > 6 )
00549 nn = 6;
00550 nps[nn]++;
00551 ii++;
00552 }
00553 if ( s->primary && s->npoints == 0 )
00554 nadj++;
00555 if ( s->primary )
00556 {
00557 a->pt[a->npt] = s->triangles[0];
00558 s->triangles[0]->primary = 1;
00559 a->npt++;
00560 }
00561 }
00562 }
00563
00564 if ( csa_verbose )
00565 {
00566 fprintf( stderr, " %d non-empty squares\n", ii );
00567 fprintf( stderr, " %d primary squares\n", a->npt );
00568 fprintf( stderr, " %d primary squares with no data\n", nadj );
00569 fprintf( stderr, " %.2f points per square \n", (double) a->npoints / ii );
00570 }
00571
00572 if ( csa_verbose == 2 )
00573 {
00574 for ( i = 0; i < 6; ++i )
00575 fprintf( stderr, " %d-%d points -- %d squares\n", i * 5, i * 5 + 4, nps[i] );
00576 fprintf( stderr, " %d or more points -- %d squares\n", i * 5, nps[i] );
00577 }
00578
00579 if ( csa_verbose == 2 )
00580 {
00581 fprintf( stderr, " j\\i" );
00582 for ( i = 0; i < a->ni; ++i )
00583 fprintf( stderr, "%3d ", i );
00584 fprintf( stderr, "\n" );
00585 for ( j = a->nj - 1; j >= 0; --j )
00586 {
00587 fprintf( stderr, "%3d ", j );
00588 for ( i = 0; i < a->ni; ++i )
00589 {
00590 square* s = a->squares[j][i];
00591
00592 if ( s->npoints > 0 )
00593 fprintf( stderr, "%3d ", s->npoints );
00594 else
00595 fprintf( stderr, " . " );
00596 }
00597 fprintf( stderr, "\n" );
00598 }
00599 }
00600
00601 if ( csa_verbose )
00602 fflush( stderr );
00603 }
00604
00605
00606
00607
00608 static void getsquares( csa* a, triangle* t, int* n, square*** squares )
00609 {
00610 int imin = (int) floor( ( t->middle.x - t->r - a->xmin ) / t->h );
00611 int imax = (int) ceil( ( t->middle.x + t->r - a->xmin ) / t->h );
00612 int jmin = (int) floor( ( t->middle.y - t->r - a->ymin ) / t->h );
00613 int jmax = (int) ceil( ( t->middle.y + t->r - a->ymin ) / t->h );
00614 int i, j;
00615
00616 if ( imin < 0 )
00617 imin = 0;
00618 if ( imax >= a->ni )
00619 imax = a->ni - 1;
00620 if ( jmin < 0 )
00621 jmin = 0;
00622 if ( jmax >= a->nj )
00623 jmax = a->nj - 1;
00624
00625 *n = 0;
00626 ( *squares ) = malloc( ( imax - imin + 1 ) * ( jmax - jmin + 1 ) * sizeof ( square* ) );
00627
00628 for ( j = jmin; j <= jmax; ++j )
00629 {
00630 for ( i = imin; i <= imax; ++i )
00631 {
00632 square* s = a->squares[j][i];
00633
00634 if ( s->npoints > 0 )
00635 {
00636 ( *squares )[*n] = a->squares[j][i];
00637 ( *n )++;
00638 }
00639 }
00640 }
00641 }
00642
00643 static double distance( point* p1, point* p2 )
00644 {
00645 return hypot( p1->x - p2->x, p1->y - p2->y );
00646 }
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656 static void thindata( triangle* t, int npmax )
00657 {
00658 csa * a = t->parent->parent;
00659 int imax = (int) ceil( sqrt( (double) ( npmax * 3 / 2 ) ) );
00660 square *** squares = alloc2d( imax, imax, sizeof ( void* ) );
00661 double h = t->r * 2.0 / imax;
00662 double h2 = h / 2.0;
00663 double xmin = t->middle.x - t->r;
00664 double ymin = t->middle.y - t->r;
00665 int i, j, ii;
00666
00667 for ( j = 0; j < imax; ++j )
00668 for ( i = 0; i < imax; ++i )
00669 squares[j][i] = square_create( a, xmin + h * i, ymin + h * j, i, j );
00670
00671 for ( ii = 0; ii < t->npoints; ++ii )
00672 {
00673 point * p = t->points[ii];
00674 int i = (int) floor( ( p->x - xmin ) / h );
00675 int j = (int) floor( ( p->y - ymin ) / h );
00676 square* s = squares[j][i];
00677
00678 if ( s->npoints == 0 )
00679 square_addpoint( s, p );
00680 else
00681
00682 {
00683 point pmiddle;
00684
00685 pmiddle.x = xmin + h * i + h2;
00686 pmiddle.y = ymin + h * j + h2;
00687
00688 if ( distance( s->points[0], &pmiddle ) > distance( p, &pmiddle ) )
00689 s->points[0] = p;
00690 }
00691 }
00692
00693 t->npoints = 0;
00694 for ( j = 0; j < imax; ++j )
00695 {
00696 for ( i = 0; i < imax; ++i )
00697 {
00698 square* s = squares[j][i];
00699
00700 if ( squares[j][i]->npoints != 0 )
00701 triangle_addpoint( t, s->points[0] );
00702 square_destroy( s );
00703 }
00704 }
00705
00706 free2d( squares );
00707 imax++;
00708 }
00709
00710
00711
00712
00713 static void csa_attachpoints( csa* a )
00714 {
00715 int npmin = a->npmin;
00716 int npmax = a->npmax;
00717 int nincreased = 0;
00718 int nthinned = 0;
00719 int i;
00720
00721 assert( a->npt > 0 );
00722
00723 if ( csa_verbose )
00724 {
00725 fprintf( stderr, "pre-processing data points:\n " );
00726 fflush( stderr );
00727 }
00728
00729 for ( i = 0; i < a->npt; ++i )
00730 {
00731 triangle* t = a->pt[i];
00732 int increased = 0;
00733
00734 if ( csa_verbose )
00735 {
00736 fprintf( stderr, "." );
00737 fflush( stderr );
00738 }
00739
00740 t->r = t->h * 1.25;
00741 while ( 1 )
00742 {
00743 int nsquares = 0;
00744 square** squares = NULL;
00745 int ii;
00746
00747 getsquares( a, t, &nsquares, &squares );
00748 for ( ii = 0; ii < nsquares; ++ii )
00749 {
00750 square* s = squares[ii];
00751 int iii;
00752
00753 for ( iii = 0; iii < s->npoints; ++iii )
00754 {
00755 point* p = s->points[iii];
00756
00757 if ( distance( p, &t->middle ) <= t->r )
00758 triangle_addpoint( t, p );
00759 }
00760 }
00761
00762 free( squares );
00763
00764 if ( t->npoints < npmin )
00765 {
00766 if ( !increased )
00767 {
00768 increased = 1;
00769 nincreased++;
00770 }
00771 t->r *= 1.25;
00772 t->npoints = 0;
00773 }
00774 else if ( t->npoints > npmax )
00775 {
00776 nthinned++;
00777 thindata( t, npmax );
00778 if ( t->npoints > npmin )
00779 break;
00780 else
00781 {
00782
00783
00784
00785
00786
00787 t->r *= 1.25;
00788 t->npoints = 0;
00789 }
00790 }
00791 else
00792 break;
00793 }
00794 }
00795
00796 if ( csa_verbose )
00797 {
00798 fprintf( stderr, "\n %d sets enhanced, %d sets thinned\n", nincreased, nthinned );
00799 fflush( stderr );
00800 }
00801 }
00802
00803 static int n2q( int n )
00804 {
00805 assert( n >= 3 );
00806
00807 if ( n >= 10 )
00808 return 3;
00809 else if ( n >= 6 )
00810 return 2;
00811 else
00812 return 1;
00813 }
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825 static void svd( double** a, int n, int m, double* w, double** v )
00826 {
00827 double * rv1;
00828 int i, j, k, l = -1;
00829 double tst1, c, f, g, h, s, scale;
00830
00831 assert( m > 0 && n > 0 );
00832
00833 rv1 = malloc( n * sizeof ( double ) );
00834
00835
00836
00837
00838 g = 0.0;
00839 scale = 0.0;
00840 tst1 = 0.0;
00841 for ( i = 0; i < n; i++ )
00842 {
00843 l = i + 1;
00844 rv1[i] = scale * g;
00845 g = 0.0;
00846 s = 0.0;
00847 scale = 0.0;
00848 if ( i < m )
00849 {
00850 for ( k = i; k < m; k++ )
00851 scale += fabs( a[k][i] );
00852 if ( scale != 0.0 )
00853 {
00854 for ( k = i; k < m; k++ )
00855 {
00856 a[k][i] /= scale;
00857 s += a[k][i] * a[k][i];
00858 }
00859 f = a[i][i];
00860 g = -copysign( sqrt( s ), f );
00861 h = f * g - s;
00862 a[i][i] = f - g;
00863 if ( i < n - 1 )
00864 {
00865 for ( j = l; j < n; j++ )
00866 {
00867 s = 0.0;
00868 for ( k = i; k < m; k++ )
00869 s += a[k][i] * a[k][j];
00870 f = s / h;
00871 for ( k = i; k < m; k++ )
00872 a[k][j] += f * a[k][i];
00873 }
00874 }
00875 for ( k = i; k < m; k++ )
00876 a[k][i] *= scale;
00877 }
00878 }
00879 w[i] = scale * g;
00880 g = 0.0;
00881 s = 0.0;
00882 scale = 0.0;
00883 if ( i < m && i < n - 1 )
00884 {
00885 for ( k = l; k < n; k++ )
00886 scale += fabs( a[i][k] );
00887 if ( scale != 0.0 )
00888 {
00889 for ( k = l; k < n; k++ )
00890 {
00891 a[i][k] /= scale;
00892 s += a[i][k] * a[i][k];
00893 }
00894 f = a[i][l];
00895 g = -copysign( sqrt( s ), f );
00896 h = f * g - s;
00897 a[i][l] = f - g;
00898 for ( k = l; k < n; k++ )
00899 rv1[k] = a[i][k] / h;
00900 for ( j = l; j < m; j++ )
00901 {
00902 s = 0.0;
00903 for ( k = l; k < n; k++ )
00904 s += a[j][k] * a[i][k];
00905 for ( k = l; k < n; k++ )
00906 a[j][k] += s * rv1[k];
00907 }
00908 for ( k = l; k < n; k++ )
00909 a[i][k] *= scale;
00910 }
00911 }
00912 {
00913 double tmp = fabs( w[i] ) + fabs( rv1[i] );
00914
00915 tst1 = ( tst1 > tmp ) ? tst1 : tmp;
00916 }
00917 }
00918
00919
00920
00921
00922 for ( i = n - 1; i >= 0; i-- )
00923 {
00924 if ( i < n - 1 )
00925 {
00926 if ( g != 0.0 )
00927 {
00928 for ( j = l; j < n; j++ )
00929
00930
00931
00932 v[j][i] = ( a[i][j] / a[i][l] ) / g;
00933 for ( j = l; j < n; j++ )
00934 {
00935 s = 0.0;
00936 for ( k = l; k < n; k++ )
00937 s += a[i][k] * v[k][j];
00938 for ( k = l; k < n; k++ )
00939 v[k][j] += s * v[k][i];
00940 }
00941 }
00942 for ( j = l; j < n; j++ )
00943 {
00944 v[i][j] = 0.0;
00945 v[j][i] = 0.0;
00946 }
00947 }
00948 v[i][i] = 1.0;
00949 g = rv1[i];
00950 l = i;
00951 }
00952
00953
00954
00955
00956 for ( i = ( m < n ) ? m - 1 : n - 1; i >= 0; i-- )
00957 {
00958 l = i + 1;
00959 g = w[i];
00960 if ( i != n - 1 )
00961 for ( j = l; j < n; j++ )
00962 a[i][j] = 0.0;
00963 if ( g != 0.0 )
00964 {
00965 for ( j = l; j < n; j++ )
00966 {
00967 s = 0.0;
00968 for ( k = l; k < m; k++ )
00969 s += a[k][i] * a[k][j];
00970
00971
00972
00973 f = ( s / a[i][i] ) / g;
00974 for ( k = i; k < m; k++ )
00975 a[k][j] += f * a[k][i];
00976 }
00977 for ( j = i; j < m; j++ )
00978 a[j][i] /= g;
00979 }
00980 else
00981 for ( j = i; j < m; j++ )
00982 a[j][i] = 0.0;
00983 a[i][i] += 1.0;
00984 }
00985
00986
00987
00988
00989 for ( k = n - 1; k >= 0; k-- )
00990 {
00991 int k1 = k - 1;
00992 int its = 0;
00993
00994 while ( 1 )
00995 {
00996 int docancellation = 1;
00997 double x, y, z;
00998 int l1 = -1;
00999
01000 its++;
01001 if ( its > SVD_NMAX )
01002 csa_quit( "svd(): no convergence in %d iterations", SVD_NMAX );
01003
01004 for ( l = k; l >= 0; l-- )
01005 {
01006 double tst2 = fabs( rv1[l] ) + tst1;
01007
01008 if ( tst2 == tst1 )
01009 {
01010 docancellation = 0;
01011 break;
01012 }
01013 l1 = l - 1;
01014
01015
01016
01017
01018 tst2 = fabs( w[l - 1] ) + tst1;
01019 if ( tst2 == tst1 )
01020 break;
01021 }
01022
01023
01024
01025 if ( docancellation )
01026 {
01027 c = 0.0;
01028 s = 1.0;
01029 for ( i = l; i <= k; i++ )
01030 {
01031 f = s * rv1[i];
01032 rv1[i] = c * rv1[i];
01033 if ( ( fabs( f ) + tst1 ) == tst1 )
01034 break;
01035 g = w[i];
01036 h = hypot( f, g );
01037 w[i] = h;
01038 h = 1.0 / h;
01039 c = g * h;
01040 s = -f * h;
01041 for ( j = 0; j < m; j++ )
01042 {
01043 double y = a[j][l1];
01044 double z = a[j][i];
01045
01046 a[j][l1] = y * c + z * s;
01047 a[j][i] = z * c - y * s;
01048 }
01049 }
01050 }
01051
01052
01053
01054 z = w[k];
01055 if ( l != k )
01056 {
01057 int i1;
01058
01059
01060
01061
01062 x = w[l];
01063 y = w[k1];
01064 g = rv1[k1];
01065 h = rv1[k];
01066 f = 0.5 * ( ( ( g + z ) / h ) * ( ( g - z ) / y ) + y / h - h / y );
01067 g = hypot( f, 1.0 );
01068 f = x - ( z / x ) * z + ( h / x ) * ( y / ( f + copysign( g, f ) ) - h );
01069
01070
01071
01072 c = 1.0;
01073 s = 1.0;
01074 for ( i1 = l; i1 < k; i1++ )
01075 {
01076 i = i1 + 1;
01077 g = rv1[i];
01078 y = w[i];
01079 h = s * g;
01080 g = c * g;
01081 z = hypot( f, h );
01082 rv1[i1] = z;
01083 c = f / z;
01084 s = h / z;
01085 f = x * c + g * s;
01086 g = g * c - x * s;
01087 h = y * s;
01088 y *= c;
01089 for ( j = 0; j < n; j++ )
01090 {
01091 x = v[j][i1];
01092 z = v[j][i];
01093 v[j][i1] = x * c + z * s;
01094 v[j][i] = z * c - x * s;
01095 }
01096 z = hypot( f, h );
01097 w[i1] = z;
01098
01099
01100
01101 if ( z != 0.0 )
01102 {
01103 c = f / z;
01104 s = h / z;
01105 }
01106 f = c * g + s * y;
01107 x = c * y - s * g;
01108 for ( j = 0; j < m; j++ )
01109 {
01110 y = a[j][i1];
01111 z = a[j][i];
01112 a[j][i1] = y * c + z * s;
01113 a[j][i] = z * c - y * s;
01114 }
01115 }
01116 rv1[l] = 0.0;
01117 rv1[k] = f;
01118 w[k] = x;
01119 }
01120 else
01121 {
01122
01123
01124
01125 if ( z < 0.0 )
01126 {
01127 w[k] = -z;
01128 for ( j = 0; j < n; j++ )
01129 v[j][k] = -v[j][k];
01130 }
01131 break;
01132 }
01133 }
01134 }
01135
01136 free( rv1 );
01137 }
01138
01139
01140
01141 static void lsq( double** A, int ni, int nj, double* z, double* w, double* sol )
01142 {
01143 double** V = alloc2d( ni, ni, sizeof ( double ) );
01144 double** B = alloc2d( nj, ni, sizeof ( double ) );
01145 int i, j, ii;
01146
01147 svd( A, ni, nj, w, V );
01148
01149 for ( j = 0; j < ni; ++j )
01150 for ( i = 0; i < ni; ++i )
01151 V[j][i] /= w[i];
01152 for ( i = 0; i < ni; ++i )
01153 {
01154 double* v = V[i];
01155
01156 for ( j = 0; j < nj; ++j )
01157 {
01158 double* a = A[j];
01159 double* b = &B[i][j];
01160
01161 for ( ii = 0; ii < ni; ++ii )
01162 *b += v[ii] * a[ii];
01163 }
01164 }
01165 for ( i = 0; i < ni; ++i )
01166 sol[i] = 0.0;
01167 for ( i = 0; i < ni; ++i )
01168 for ( j = 0; j < nj; ++j )
01169 sol[i] += B[i][j] * z[j];
01170
01171 free2d( B );
01172 free2d( V );
01173 }
01174
01175
01176
01177
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189
01190
01191
01192 static void csa_findprimarycoeffs( csa* a )
01193 {
01194 int n[4] = { 0, 0, 0, 0 };
01195 int i;
01196
01197 if ( csa_verbose )
01198 fprintf( stderr, "calculating spline coefficients for primary triangles:\n " );
01199
01200 for ( i = 0; i < a->npt; ++i )
01201 {
01202 triangle* t = a->pt[i];
01203 int npoints = t->npoints;
01204 point ** points = t->points;
01205 double * z = malloc( npoints * sizeof ( double ) );
01206 int q = n2q( t->npoints );
01207 int ok = 1;
01208 double b[10];
01209 double b1[6];
01210 int ii;
01211
01212 if ( csa_verbose )
01213 {
01214 fprintf( stderr, "." );
01215 fflush( stderr );
01216 }
01217
01218 for ( ii = 0; ii < npoints; ++ii )
01219 z[ii] = points[ii]->z;
01220
01221 do
01222 {
01223 double bc[3];
01224 double wmin, wmax;
01225
01226 if ( !ok )
01227 q--;
01228
01229 assert( q >= 0 );
01230
01231 if ( q == 3 )
01232 {
01233 double ** A = alloc2d( 10, npoints, sizeof ( double ) );
01234 double w[10];
01235
01236 for ( ii = 0; ii < npoints; ++ii )
01237 {
01238 double * aii = A[ii];
01239 double tmp;
01240
01241 triangle_calculatebc( t, points[ii], bc );
01242
01243
01244
01245
01246
01247 tmp = bc[0] * bc[0];
01248 aii[0] = tmp * bc[0];
01249 tmp *= 3.0;
01250 aii[1] = tmp * bc[1];
01251 aii[2] = tmp * bc[2];
01252 tmp = bc[1] * bc[1];
01253 aii[6] = tmp * bc[1];
01254 tmp *= 3.0;
01255 aii[3] = tmp * bc[0];
01256 aii[7] = tmp * bc[2];
01257 tmp = bc[2] * bc[2];
01258 aii[9] = tmp * bc[2];
01259 tmp *= 3.0;
01260 aii[5] = tmp * bc[0];
01261 aii[8] = tmp * bc[1];
01262 aii[4] = bc[0] * bc[1] * bc[2] * 6.0;
01263 }
01264
01265 lsq( A, 10, npoints, z, w, b );
01266
01267 wmin = w[0];
01268 wmax = w[0];
01269 for ( ii = 1; ii < 10; ++ii )
01270 {
01271 if ( w[ii] < wmin )
01272 wmin = w[ii];
01273 else if ( w[ii] > wmax )
01274 wmax = w[ii];
01275 }
01276 if ( wmin < wmax / a->k )
01277 ok = 0;
01278
01279 free2d( A );
01280 }
01281 else if ( q == 2 )
01282 {
01283 double ** A = alloc2d( 6, npoints, sizeof ( double ) );
01284 double w[6];
01285
01286 for ( ii = 0; ii < npoints; ++ii )
01287 {
01288 double* aii = A[ii];
01289
01290 triangle_calculatebc( t, points[ii], bc );
01291
01292
01293
01294
01295
01296
01297 aii[0] = bc[0] * bc[0];
01298 aii[1] = bc[0] * bc[1] * 2.0;
01299 aii[2] = bc[0] * bc[2] * 2.0;
01300 aii[3] = bc[1] * bc[1];
01301 aii[4] = bc[1] * bc[2] * 2.0;
01302 aii[5] = bc[2] * bc[2];
01303 }
01304
01305 lsq( A, 6, npoints, z, w, b1 );
01306
01307 wmin = w[0];
01308 wmax = w[0];
01309 for ( ii = 1; ii < 6; ++ii )
01310 {
01311 if ( w[ii] < wmin )
01312 wmin = w[ii];
01313 else if ( w[ii] > wmax )
01314 wmax = w[ii];
01315 }
01316 if ( wmin < wmax / a->k )
01317 ok = 0;
01318 else
01319 {
01320 ok = 1;
01321 b[0] = b1[0];
01322 b[1] = ( b1[0] + 2.0 * b1[1] ) / 3.0;
01323 b[2] = ( b1[0] + 2.0 * b1[2] ) / 3.0;
01324 b[3] = ( b1[3] + 2.0 * b1[1] ) / 3.0;
01325 b[4] = ( b1[1] + b1[2] + b1[4] ) / 3.0;
01326 b[5] = ( b1[5] + 2.0 * b1[2] ) / 3.0;
01327 b[6] = b1[3];
01328 b[7] = ( b1[3] + 2.0 * b1[4] ) / 3.0;
01329 b[8] = ( b1[5] + 2.0 * b1[4] ) / 3.0;
01330 b[9] = b1[5];
01331 }
01332
01333 free2d( A );
01334 }
01335 else if ( q == 1 )
01336 {
01337 double ** A = alloc2d( 3, npoints, sizeof ( double ) );
01338 double w[3];
01339
01340 for ( ii = 0; ii < npoints; ++ii )
01341 {
01342 double* aii = A[ii];
01343
01344 triangle_calculatebc( t, points[ii], bc );
01345
01346 aii[0] = bc[0];
01347 aii[1] = bc[1];
01348 aii[2] = bc[2];
01349 }
01350
01351 lsq( A, 3, npoints, z, w, b1 );
01352
01353 wmin = w[0];
01354 wmax = w[0];
01355 for ( ii = 1; ii < 3; ++ii )
01356 {
01357 if ( w[ii] < wmin )
01358 wmin = w[ii];
01359 else if ( w[ii] > wmax )
01360 wmax = w[ii];
01361 }
01362 if ( wmin < wmax / a->k )
01363 ok = 0;
01364 else
01365 {
01366 ok = 1;
01367 b[0] = b1[0];
01368 b[1] = ( 2.0 * b1[0] + b1[1] ) / 3.0;
01369 b[2] = ( 2.0 * b1[0] + b1[2] ) / 3.0;
01370 b[3] = ( 2.0 * b1[1] + b1[0] ) / 3.0;
01371 b[4] = ( b1[0] + b1[1] + b1[2] ) / 3.0;
01372 b[5] = ( 2.0 * b1[2] + b1[0] ) / 3.0;
01373 b[6] = b1[1];
01374 b[7] = ( 2.0 * b1[1] + b1[2] ) / 3.0;
01375 b[8] = ( 2.0 * b1[2] + b1[1] ) / 3.0;
01376 b[9] = b1[2];
01377 }
01378
01379 free2d( A );
01380 }
01381 else if ( q == 0 )
01382 {
01383 double ** A = alloc2d( 1, npoints, sizeof ( double ) );
01384 double w[1];
01385
01386 for ( ii = 0; ii < npoints; ++ii )
01387 A[ii][0] = 1.0;
01388
01389 lsq( A, 1, npoints, z, w, b1 );
01390
01391 ok = 1;
01392 b[0] = b1[0];
01393 b[1] = b1[0];
01394 b[2] = b1[0];
01395 b[3] = b1[0];
01396 b[4] = b1[0];
01397 b[5] = b1[0];
01398 b[6] = b1[0];
01399 b[7] = b1[0];
01400 b[8] = b1[0];
01401 b[9] = b1[0];
01402
01403 free2d( A );
01404 }
01405 } while ( !ok );
01406
01407 n[q]++;
01408 t->order = q;
01409
01410 {
01411 square* s = t->parent;
01412 double* coeffs = s->coeffs;
01413
01414 coeffs[12] = b[0];
01415 coeffs[9] = b[1];
01416 coeffs[6] = b[3];
01417 coeffs[3] = b[6];
01418 coeffs[2] = b[7];
01419 coeffs[1] = b[8];
01420 coeffs[0] = b[9];
01421 coeffs[4] = b[5];
01422 coeffs[8] = b[2];
01423 coeffs[5] = b[4];
01424 }
01425
01426 free( z );
01427 }
01428
01429 if ( csa_verbose )
01430 {
01431 fprintf( stderr, "\n 3rd order -- %d sets\n", n[3] );
01432 fprintf( stderr, " 2nd order -- %d sets\n", n[2] );
01433 fprintf( stderr, " 1st order -- %d sets\n", n[1] );
01434 fprintf( stderr, " 0th order -- %d sets\n", n[0] );
01435 fflush( stderr );
01436 }
01437
01438 if ( csa_verbose == 2 )
01439 {
01440 int j;
01441
01442 fprintf( stderr, " j\\i" );
01443 for ( i = 0; i < a->ni; ++i )
01444 fprintf( stderr, "%2d ", i );
01445 fprintf( stderr, "\n" );
01446 for ( j = a->nj - 1; j >= 0; --j )
01447 {
01448 fprintf( stderr, "%2d ", j );
01449 for ( i = 0; i < a->ni; ++i )
01450 {
01451 square* s = a->squares[j][i];
01452
01453 if ( s->triangles[0]->primary )
01454 fprintf( stderr, "%2d ", s->triangles[0]->order );
01455 else
01456 fprintf( stderr, " . " );
01457 }
01458 fprintf( stderr, "\n" );
01459 }
01460 }
01461 }
01462
01463
01464
01465
01466 static void csa_findsecondarycoeffs( csa* a )
01467 {
01468 square*** squares = a->squares;
01469 int ni = a->ni;
01470 int nj = a->nj;
01471 int ii;
01472
01473 if ( csa_verbose )
01474 {
01475 fprintf( stderr, "propagating spline coefficients to the remaining triangles:\n" );
01476 fflush( stderr );
01477 }
01478
01479
01480
01481
01482 for ( ii = 0; ii < a->npt; ++ii )
01483 {
01484 triangle* t = a->pt[ii];
01485 square * s = t->parent;
01486 int i = s->i;
01487 int j = s->j;
01488 double * c = s->coeffs;
01489 double * cl = ( i > 0 ) ? squares[j][i - 1]->coeffs : NULL;
01490 double * cb = ( j > 0 ) ? squares[j - 1][i]->coeffs : NULL;
01491 double * cbl = ( i > 0 && j > 0 ) ? squares[j - 1][i - 1]->coeffs : NULL;
01492 double * ca = ( j < nj - 1 ) ? squares[j + 1][i]->coeffs : NULL;
01493 double * cal = ( j < nj - 1 && i > 0 ) ? squares[j + 1][i - 1]->coeffs : NULL;
01494
01495 c[7] = 2.0 * c[4] - c[1];
01496 c[11] = 2.0 * c[8] - c[5];
01497 c[15] = 2.0 * c[12] - c[9];
01498
01499 c[10] = 2.0 * c[6] - c[2];
01500 c[13] = 2.0 * c[9] - c[5];
01501 c[16] = 2.0 * c[12] - c[8];
01502
01503 c[19] = 2.0 * c[15] - c[11];
01504
01505 if ( cl != NULL )
01506 {
01507 cl[21] = c[0];
01508 cl[22] = c[1];
01509 cl[23] = c[2];
01510 cl[24] = c[3];
01511
01512 cl[18] = c[0] + c[1] - c[4];
01513 cl[19] = c[1] + c[2] - c[5];
01514 cl[20] = c[2] + c[3] - c[6];
01515
01516 cl[17] = 2.0 * cl[20] - cl[23];
01517 cl[14] = 2.0 * cl[18] - cl[22];
01518 }
01519
01520 if ( cb != NULL )
01521 {
01522 cb[3] = c[0];
01523 cb[10] = c[7];
01524
01525 cb[6] = c[0] + c[7] - c[4];
01526 cb[2] = 2.0 * cb[6] - cb[10];
01527 }
01528
01529 if ( cbl != NULL )
01530 {
01531 cbl[23] = cb[2];
01532 cbl[24] = cb[3];
01533
01534 cbl[20] = cb[2] + cb[3] - cb[6];
01535 cbl[17] = cl[14];
01536 }
01537
01538 if ( ca != NULL )
01539 {
01540 ca[0] = c[3];
01541 ca[7] = c[10];
01542
01543 ca[4] = c[3] + c[10] - c[6];
01544 ca[1] = 2.0 * ca[4] - ca[7];
01545 }
01546
01547 if ( cal != NULL )
01548 {
01549 cal[21] = c[3];
01550 cal[22] = ca[1];
01551
01552 cal[18] = ca[0] + ca[1] - ca[4];
01553 cal[14] = cl[17];
01554 }
01555 }
01556
01557
01558
01559
01560 for ( ii = 0; ii < a->npt; ++ii )
01561 {
01562 triangle* t = a->pt[ii];
01563 square * s = t->parent;
01564 int i = s->i;
01565 int j = s->j;
01566 double * c = s->coeffs;
01567 double * cr = ( i < ni - 1 ) ? squares[j][i + 1]->coeffs : NULL;
01568 double * car = ( i < ni - 1 && j < nj - 1 ) ? squares[j + 1][i + 1]->coeffs : NULL;
01569 double * cbr = ( i < ni - 1 && j > 0 ) ? squares[j - 1][i + 1]->coeffs : NULL;
01570
01571 if ( car != NULL )
01572 cr[13] = car[7] + car[14] - car[11];
01573
01574 if ( cbr != NULL )
01575 cr[11] = cbr[10] + cbr[17] - cbr[13];
01576
01577 if ( cr != NULL )
01578 cr[5] = c[22] + c[23] - c[19];
01579 }
01580
01581
01582
01583
01584 for ( ii = 0; ii < a->npt; ++ii )
01585 {
01586 triangle* t = a->pt[ii];
01587 square * s = t->parent;
01588 int i = s->i;
01589 int j = s->j;
01590 double * cr = ( i < ni - 1 ) ? squares[j][i + 1]->coeffs : NULL;
01591
01592 if ( cr != NULL )
01593 {
01594 cr[9] = ( cr[5] + cr[13] ) / 2.0;
01595 cr[8] = ( cr[5] + cr[11] ) / 2.0;
01596 cr[15] = ( cr[11] + cr[19] ) / 2.0;
01597 cr[16] = ( cr[13] + cr[19] ) / 2.0;
01598 cr[12] = ( cr[8] + cr[16] ) / 2.0;
01599 }
01600 }
01601
01602 if ( csa_verbose )
01603 {
01604 fprintf( stderr, "checking that all coefficients have been set:\n" );
01605 fflush( stderr );
01606 }
01607
01608 for ( ii = 0; ii < ni * nj; ++ii )
01609 {
01610 square* s = squares[0][ii];
01611 double* c = s->coeffs;
01612 int i;
01613
01614 if ( s->npoints == 0 )
01615 continue;
01616 for ( i = 0; i < 25; ++i )
01617 if ( isnan( c[i] ) )
01618 fprintf( stderr, " squares[%d][%d]->coeffs[%d] = NaN\n", s->j, s->i, i );
01619 }
01620 }
01621
01622 static int i300[] = { 12, 12, 12, 12 };
01623 static int i030[] = { 3, 24, 21, 0 };
01624 static int i003[] = { 0, 3, 24, 21 };
01625 static int i210[] = { 9, 16, 15, 8 };
01626 static int i021[] = { 2, 17, 22, 7 };
01627 static int i102[] = { 4, 6, 20, 18 };
01628 static int i120[] = { 6, 20, 18, 4 };
01629 static int i012[] = { 1, 10, 23, 14 };
01630 static int i201[] = { 8, 9, 16, 15 };
01631 static int i111[] = { 5, 13, 19, 11 };
01632
01633 static int * iall[] = { i300, i030, i003, i210, i021, i102, i120, i012, i201, i111 };
01634
01635 static void csa_sethascoeffsflag( csa* a )
01636 {
01637 int i, j;
01638
01639 for ( j = 0; j < a->nj; ++j )
01640 {
01641 for ( i = 0; i < a->ni; ++i )
01642 {
01643 square* s = a->squares[j][i];
01644 double* coeffs = s->coeffs;
01645 int ii;
01646
01647 for ( ii = 0; ii < 4; ++ii )
01648 {
01649 triangle* t = s->triangles[ii];
01650 int cc;
01651
01652 for ( cc = 0; cc < 10; ++cc )
01653 if ( isnan( coeffs[iall[cc][ii]] ) )
01654 break;
01655 if ( cc == 10 )
01656 t->hascoeffs = 1;
01657 }
01658 }
01659 }
01660 }
01661
01662 void csa_calculatespline( csa* a )
01663 {
01664 csa_squarize( a );
01665 csa_attachpoints( a );
01666 csa_findprimarycoeffs( a );
01667 csa_findsecondarycoeffs( a );
01668 csa_sethascoeffsflag( a );
01669 }
01670
01671 void csa_approximate_point( csa* a, point* p )
01672 {
01673 double h = a->h;
01674 double ii = ( p->x - a->xmin ) / h + 1.0;
01675 double jj = ( p->y - a->ymin ) / h + 1.0;
01676 int i, j;
01677 square * s;
01678 double fi, fj;
01679 int ti;
01680 triangle* t;
01681 double bc[3];
01682
01683 if ( ii < 0.0 || jj < 0.0 || ii > (double) a->ni - 1.0 || jj > (double) a->nj - 1.0 )
01684 {
01685 p->z = NaN;
01686 return;
01687 }
01688
01689 i = (int) floor( ii );
01690 j = (int) floor( jj );
01691 s = a->squares[j][i];
01692 fi = ii - i;
01693 fj = jj - j;
01694
01695 if ( fj < fi )
01696 {
01697 if ( fi + fj < 1.0 )
01698 ti = 3;
01699 else
01700 ti = 2;
01701 }
01702 else
01703 {
01704 if ( fi + fj < 1.0 )
01705 ti = 0;
01706 else
01707 ti = 1;
01708 }
01709
01710 t = s->triangles[ti];
01711 if ( !t->hascoeffs )
01712 {
01713 p->z = NaN;
01714 return;
01715 }
01716 triangle_calculatebc( t, p, bc );
01717
01718 {
01719 double * c = s->coeffs;
01720 double bc1 = bc[0];
01721 double bc2 = bc[1];
01722 double bc3 = bc[2];
01723 double tmp1 = bc1 * bc1;
01724 double tmp2 = bc2 * bc2;
01725 double tmp3 = bc3 * bc3;
01726
01727 switch ( ti )
01728 {
01729 case 0:
01730 p->z = c[12] * bc1 * tmp1 + c[3] * bc2 * tmp2 + c[0] * bc3 * tmp3 + 3.0 * ( c[9] * tmp1 * bc2 + c[2] * tmp2 * bc3 + c[4] * tmp3 * bc1 + c[6] * bc1 * tmp2 + c[1] * bc2 * tmp3 + c[8] * tmp1 * bc3 ) + 6.0 * c[5] * bc1 * bc2 * bc3;
01731 break;
01732 case 1:
01733 p->z = c[12] * bc1 * tmp1 + c[24] * bc2 * tmp2 + c[3] * bc3 * tmp3 + 3.0 * ( c[16] * tmp1 * bc2 + c[17] * tmp2 * bc3 + c[6] * tmp3 * bc1 + c[20] * bc1 * tmp2 + c[10] * bc2 * tmp3 + c[9] * tmp1 * bc3 ) + 6.0 * c[13] * bc1 * bc2 * bc3;
01734 break;
01735 case 2:
01736 p->z = c[12] * bc1 * tmp1 + c[21] * bc2 * tmp2 + c[24] * bc3 * tmp3 + 3.0 * ( c[15] * tmp1 * bc2 + c[22] * tmp2 * bc3 + c[20] * tmp3 * bc1 + c[18] * bc1 * tmp2 + c[23] * bc2 * tmp3 + c[16] * tmp1 * bc3 ) + 6.0 * c[19] * bc1 * bc2 * bc3;
01737 break;
01738 default:
01739 p->z = c[12] * bc1 * tmp1 + c[0] * bc2 * tmp2 + c[21] * bc3 * tmp3 + 3.0 * ( c[8] * tmp1 * bc2 + c[7] * tmp2 * bc3 + c[18] * tmp3 * bc1 + c[4] * bc1 * tmp2 + c[14] * bc2 * tmp3 + c[15] * tmp1 * bc3 ) + 6.0 * c[11] * bc1 * bc2 * bc3;
01740 }
01741 }
01742 }
01743
01744 void csa_approximate_points( csa* a, int n, point* points )
01745 {
01746 int ii;
01747
01748 for ( ii = 0; ii < n; ++ii )
01749 csa_approximate_point( a, &points[ii] );
01750 }
01751
01752 void csa_setnpmin( csa* a, int npmin )
01753 {
01754 a->npmin = npmin;
01755 }
01756
01757 void csa_setnpmax( csa* a, int npmax )
01758 {
01759 a->npmax = npmax;
01760 }
01761
01762 void csa_setk( csa* a, int k )
01763 {
01764 a->k = k;
01765 }
01766
01767 void csa_setnppc( csa* a, double nppc )
01768 {
01769 a->nppc = (int) nppc;
01770 }
01771
01772 #if defined ( STANDALONE )
01773
01774 #include "minell.h"
01775
01776 #define NIMAX 2048
01777 #define BUFSIZE 10240
01778 #define STRBUFSIZE 64
01779
01780 static void points_generate( double xmin, double xmax, double ymin, double ymax, int nx, int ny, int* nout, point** pout )
01781 {
01782 double stepx, stepy;
01783 double x0, xx, yy;
01784 int i, j, ii;
01785
01786 if ( nx < 1 || ny < 1 )
01787 {
01788 *pout = NULL;
01789 *nout = 0;
01790 return;
01791 }
01792
01793 *nout = nx * ny;
01794 *pout = malloc( *nout * sizeof ( point ) );
01795
01796 stepx = ( nx > 1 ) ? ( xmax - xmin ) / ( nx - 1 ) : 0.0;
01797 stepy = ( ny > 1 ) ? ( ymax - ymin ) / ( ny - 1 ) : 0.0;
01798 x0 = ( nx > 1 ) ? xmin : ( xmin + xmax ) / 2.0;
01799 yy = ( ny > 1 ) ? ymin : ( ymin + ymax ) / 2.0;
01800
01801 ii = 0;
01802 for ( j = 0; j < ny; ++j )
01803 {
01804 xx = x0;
01805 for ( i = 0; i < nx; ++i )
01806 {
01807 point* p = &( *pout )[ii];
01808
01809 p->x = xx;
01810 p->y = yy;
01811 xx += stepx;
01812 ii++;
01813 }
01814 yy += stepy;
01815 }
01816 }
01817
01818 static int str2double( char* token, double* value )
01819 {
01820 char* end = NULL;
01821
01822 if ( token == NULL )
01823 {
01824 *value = NaN;
01825 return 0;
01826 }
01827
01828 *value = strtod( token, &end );
01829
01830 if ( end == token )
01831 {
01832 *value = NaN;
01833 return 0;
01834 }
01835
01836 return 1;
01837 }
01838
01839 #define NALLOCATED_START 1024
01840
01841
01842
01843
01844
01845
01846
01847
01848 void points_read( char* fname, int dim, int* n, point** points )
01849 {
01850 FILE * f = NULL;
01851 int nallocated = NALLOCATED_START;
01852 char buf[BUFSIZE];
01853 char seps[] = " ,;\t";
01854 char * token;
01855
01856 if ( dim < 2 || dim > 3 )
01857 {
01858 *n = 0;
01859 *points = NULL;
01860 return;
01861 }
01862
01863 if ( fname == NULL )
01864 f = stdin;
01865 else
01866 {
01867 if ( strcmp( fname, "stdin" ) == 0 || strcmp( fname, "-" ) == 0 )
01868 f = stdin;
01869 else
01870 {
01871 f = fopen( fname, "r" );
01872 if ( f == NULL )
01873 csa_quit( "%s: %s\n", fname, strerror( errno ) );
01874 }
01875 }
01876
01877 *points = malloc( nallocated * sizeof ( point ) );
01878 *n = 0;
01879 while ( fgets( buf, BUFSIZE, f ) != NULL )
01880 {
01881 point* p;
01882
01883 if ( *n == nallocated )
01884 {
01885 nallocated *= 2;
01886 *points = realloc( *points, nallocated * sizeof ( point ) );
01887 }
01888
01889 p = &( *points )[*n];
01890
01891 if ( buf[0] == '#' )
01892 continue;
01893 if ( ( token = strtok( buf, seps ) ) == NULL )
01894 continue;
01895 if ( !str2double( token, &p->x ) )
01896 continue;
01897 if ( ( token = strtok( NULL, seps ) ) == NULL )
01898 continue;
01899 if ( !str2double( token, &p->y ) )
01900 continue;
01901 if ( dim == 2 )
01902 p->z = NaN;
01903 else
01904 {
01905 if ( ( token = strtok( NULL, seps ) ) == NULL )
01906 continue;
01907 if ( !str2double( token, &p->z ) )
01908 continue;
01909 }
01910 ( *n )++;
01911 }
01912
01913 if ( *n == 0 )
01914 {
01915 free( *points );
01916 *points = NULL;
01917 }
01918 else
01919 *points = realloc( *points, *n * sizeof ( point ) );
01920
01921 if ( f != stdin )
01922 if ( fclose( f ) != 0 )
01923 csa_quit( "%s: %s\n", fname, strerror( errno ) );
01924 }
01925
01926 static void points_write( int n, point* points )
01927 {
01928 int i;
01929
01930 if ( csa_verbose )
01931 printf( "Output:\n" );
01932
01933 for ( i = 0; i < n; ++i )
01934 {
01935 point* p = &points[i];
01936
01937 printf( "%.15g %.15g %.15g\n", p->x, p->y, p->z );
01938 }
01939 }
01940
01941 static double points_scaletosquare( int n, point* points )
01942 {
01943 double xmin, ymin, xmax, ymax;
01944 double k;
01945 int i;
01946
01947 if ( n <= 0 )
01948 return NaN;
01949
01950 xmin = xmax = points[0].x;
01951 ymin = ymax = points[0].y;
01952
01953 for ( i = 1; i < n; ++i )
01954 {
01955 point* p = &points[i];
01956
01957 if ( p->x < xmin )
01958 xmin = p->x;
01959 else if ( p->x > xmax )
01960 xmax = p->x;
01961 if ( p->y < ymin )
01962 ymin = p->y;
01963 else if ( p->y > ymax )
01964 ymax = p->y;
01965 }
01966
01967 if ( xmin == xmax || ymin == ymax )
01968 return NaN;
01969 else
01970 k = ( ymax - ymin ) / ( xmax - xmin );
01971
01972 for ( i = 0; i < n; ++i )
01973 points[i].y /= k;
01974
01975 return k;
01976 }
01977
01978 static void points_scale( int n, point* points, double k )
01979 {
01980 int i;
01981
01982 for ( i = 0; i < n; ++i )
01983 points[i].y /= k;
01984 }
01985
01986 static void usage()
01987 {
01988 printf( "Usage: csabathy -i <XYZ file> {-o <XY file>|-n <nx>x<ny> [-c|-s] [-z <zoom>]}\n" );
01989 printf( " [-v|-V] [-P nppc=<value>] [-P k=<value>]\n" );
01990 printf( "Options:\n" );
01991 printf( " -c -- scale internally so that the minimal ellipse turns into a\n" );
01992 printf( " circle (this produces results invariant to affine\n" );
01993 printf( " transformations)\n" );
01994 printf( " -i <XYZ file> -- three-column file with points to approximate from (use\n" );
01995 printf( " \"-i stdin\" or \"-i -\" for standard input)\n" );
01996 printf( " -n <nx>x<ny> -- generate <nx>x<ny> output rectangular grid\n" );
01997 printf( " -o <XY file> -- two-column file with points to approximate in (use\n" );
01998 printf( " \"-o stdin\" or \"-o -\" for standard input)\n" );
01999 printf( " -s -- scale internally so that Xmax - Xmin = Ymax - Ymin\n" );
02000 printf( " -v -- verbose / version\n" );
02001 printf( " -z <zoom> -- zoom in (if <zoom> < 1) or out (<zoom> > 1)\n" );
02002 printf( " -P nppc=<value> -- set the average number of points per cell (default = 5\n" );
02003 printf( " increase if the point distribution is strongly non-uniform\n" );
02004 printf( " to get larger cells)\n" );
02005 printf( " -P k=<value> -- set the spline sensitivity (default = 140, reduce to get\n" );
02006 printf( " smoother results)\n" );
02007 printf( " -V -- very verbose / version\n" );
02008 printf( "Description:\n" );
02009 printf( " `csabathy' approximates irregular scalar 2D data in specified points using\n" );
02010 printf( " C1-continuous bivariate cubic spline. The calculated values are written to\n" );
02011 printf( " standard output.\n" );
02012
02013 exit( 0 );
02014 }
02015
02016 static void version()
02017 {
02018 printf( "csa version %s\n", csa_version );
02019 exit( 0 );
02020 }
02021
02022 static void parse_commandline( int argc, char* argv[], char** fdata, char** fpoints, int* invariant, int* square, int* generate_points, int* nx, int* ny, int* nppc, int* k, double* zoom )
02023 {
02024 int i;
02025
02026 if ( argc < 2 )
02027 usage();
02028
02029 i = 1;
02030 while ( i < argc )
02031 {
02032 if ( argv[i][0] != '-' )
02033 usage();
02034
02035 switch ( argv[i][1] )
02036 {
02037 case 'c':
02038 i++;
02039 *invariant = 1;
02040 *square = 0;
02041
02042 break;
02043 case 'i':
02044 i++;
02045 if ( i >= argc )
02046 csa_quit( "no file name found after -i\n" );
02047 *fdata = argv[i];
02048 i++;
02049 break;
02050 case 'n':
02051 i++;
02052 *fpoints = NULL;
02053 *generate_points = 1;
02054 if ( i >= argc )
02055 csa_quit( "no grid dimensions found after -n\n" );
02056 if ( sscanf( argv[i], "%dx%d", nx, ny ) != 2 )
02057 csa_quit( "could not read grid dimensions after \"-n\"\n" );
02058 if ( *nx <= 0 || *nx > NIMAX || *ny <= 0 || *ny > NIMAX )
02059 csa_quit( "invalid size for output grid\n" );
02060 i++;
02061 break;
02062 case 'o':
02063 i++;
02064 if ( i >= argc )
02065 csa_quit( "no file name found after -o\n" );
02066 *fpoints = argv[i];
02067 i++;
02068 break;
02069 case 's':
02070 i++;
02071 *square = 1;
02072
02073 *invariant = 0;
02074 break;
02075 case 'v':
02076 i++;
02077 csa_verbose = 1;
02078 break;
02079 case 'z':
02080 i++;
02081 if ( i >= argc )
02082 csa_quit( "no zoom value found after -z\n" );
02083 *zoom = atof( argv[i] );
02084 i++;
02085 break;
02086 case 'P': {
02087 char delim[] = "=";
02088 char prmstr[STRBUFSIZE] = "";
02089 char * token;
02090
02091 i++;
02092 if ( i >= argc )
02093 csa_quit( "no input found after -P\n" );
02094
02095 if ( strlen( argv[i] ) >= STRBUFSIZE )
02096 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02097
02098 strcpy( prmstr, argv[i] );
02099 token = strtok( prmstr, delim );
02100 if ( token == NULL )
02101 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02102
02103 if ( strcmp( token, "nppc" ) == 0 )
02104 {
02105 long int n;
02106
02107 token = strtok( NULL, delim );
02108 if ( token == NULL )
02109 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02110
02111 n = strtol( token, NULL, 10 );
02112 if ( n == LONG_MIN || n == LONG_MAX )
02113 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02114 else if ( n <= 0 )
02115 csa_quit( "non-sensible value for \"nppc\" parameter\n" );
02116 *nppc = (int) n;
02117 }
02118 else if ( strcmp( token, "k" ) == 0 )
02119 {
02120 long int n;
02121
02122 token = strtok( NULL, delim );
02123 if ( token == NULL )
02124 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02125
02126 n = strtol( token, NULL, 10 );
02127 if ( n == LONG_MIN || n == LONG_MAX )
02128 csa_quit( "could not interpret \"%s\" after -P option\n", argv[i] );
02129 else if ( n <= 0 )
02130 csa_quit( "non-sensible value for \"k\" parameter\n" );
02131 *k = (int) n;
02132 }
02133 else
02134 usage();
02135
02136 i++;
02137 break;
02138 }
02139 case 'V':
02140 i++;
02141 csa_verbose = 2;
02142 break;
02143 default:
02144 usage();
02145 break;
02146 }
02147 }
02148
02149 if ( csa_verbose && argc == 2 )
02150 version();
02151 }
02152
02153 int main( int argc, char* argv[] )
02154 {
02155 char * fdata = NULL;
02156 char * fpoints = NULL;
02157 int nin = 0;
02158 point * pin = NULL;
02159 int invariant = 0;
02160 minell * me = NULL;
02161 int square = 0;
02162 int nout = 0;
02163 int generate_points = 0;
02164 point * pout = NULL;
02165 int nx = -1;
02166 int ny = -1;
02167 csa * a = NULL;
02168 int nppc = -1;
02169 int k = -1;
02170 double ks = NaN;
02171 double zoom = NaN;
02172
02173 parse_commandline( argc, argv, &fdata, &fpoints, &invariant, &square, &generate_points, &nx, &ny, &nppc, &k, &zoom );
02174
02175 if ( fdata == NULL )
02176 csa_quit( "no input data\n" );
02177
02178 if ( !generate_points && fpoints == NULL )
02179 csa_quit( "no output grid specified\n" );
02180
02181 points_read( fdata, 3, &nin, &pin );
02182
02183 if ( nin < 3 )
02184 return 0;
02185
02186 if ( invariant )
02187 {
02188 me = minell_build( nin, pin );
02189 minell_scalepoints( me, nin, pin );
02190 }
02191 else if ( square )
02192 ks = points_scaletosquare( nin, pin );
02193
02194 a = csa_create();
02195 csa_addpoints( a, nin, pin );
02196 if ( nppc > 0 )
02197 csa_setnppc( a, nppc );
02198 if ( k > 0 )
02199 csa_setk( a, k );
02200 csa_calculatespline( a );
02201
02202 if ( generate_points )
02203 {
02204 if ( isnan( zoom ) )
02205 points_generate( a->xmin - a->h, a->xmax + a->h, a->ymin - a->h, a->ymax + a->h, nx, ny, &nout, &pout );
02206 else
02207 {
02208 double xdiff2 = ( a->xmax - a->xmin ) / 2.0;
02209 double ydiff2 = ( a->ymax - a->ymin ) / 2.0;
02210 double xav = ( a->xmax + a->xmin ) / 2.0;
02211 double yav = ( a->ymax + a->ymin ) / 2.0;
02212
02213 points_generate( xav - xdiff2 * zoom, xav + xdiff2 * zoom, yav - ydiff2 * zoom, yav + ydiff2 * zoom, nx, ny, &nout, &pout );
02214 }
02215 }
02216 else
02217 {
02218 points_read( fpoints, 2, &nout, &pout );
02219 if ( invariant )
02220 minell_scalepoints( me, nout, pout );
02221 else if ( square )
02222 points_scale( nout, pout, ks );
02223 }
02224
02225 csa_approximate_points( a, nout, pout );
02226
02227 if ( invariant )
02228 minell_rescalepoints( me, nout, pout );
02229 else if ( square )
02230 points_scale( nout, pout, 1.0 / ks );
02231
02232 points_write( nout, pout );
02233
02234 csa_destroy( a );
02235 free( pin );
02236 free( pout );
02237
02238 return 0;
02239 }
02240
02241 #endif // STANDALONE