00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
#include <volume_io/internal_volume_io.h>
00016
#include <bicpl/numerical.h>
00017
#include <limits.h>
00018
00019
#ifndef lint
00020
static char rcsid[] =
"$Header: /software/source//libraries/bicpl/Numerical/numerical.c,v 1.24 2000/02/06 15:30:41 stever Exp $";
00021
#endif
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #define SMALLEST 1.0e-20
00039
00040 public BOOLEAN
numerically_close(
00041 Real n1,
00042 Real n2,
00043 Real threshold_ratio )
00044 {
00045
double avg, diff, abs_n1, abs_n2;
00046
00047 diff = n1 - n2;
00048
if( diff < 0.0 ) diff = -diff;
00049
00050 abs_n1 = FABS( n1 );
00051 abs_n2 = FABS( n2 );
00052
00053
if( abs_n1 <
SMALLEST || abs_n2 <
SMALLEST )
00054
return( abs_n1 < threshold_ratio && abs_n2 < threshold_ratio );
00055
00056 avg = (n1 + n2) / 2.0;
00057
00058
if( avg == 0.0 )
00059
return( diff <= (
double) threshold_ratio );
00060
00061
if( avg < 0.0 ) avg = -avg;
00062
00063
return( (diff / avg) <= (
double) threshold_ratio );
00064 }
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080 public Real
get_good_round_value(
00081 Real value )
00082 {
00083 Real rounded, sign, power_of_ten, power_of_ten_times_5;
00084
00085
if( value < 0.0 )
00086 {
00087 sign = -1.0;
00088 value = -value;
00089 }
00090
else
00091 sign = 1.0;
00092
00093
if( value == 0.0 )
00094 rounded = 0.0;
00095
else
00096 {
00097 power_of_ten = pow( 10.0, (
double) (
int) log10( (
double) value ) );
00098 power_of_ten_times_5 = 5.0 * power_of_ten;
00099
if( power_of_ten_times_5 > value )
00100 power_of_ten_times_5 /= 10.0;
00101
00102 rounded = MAX( power_of_ten, power_of_ten_times_5 );
00103 }
00104
00105
return( sign * rounded );
00106 }
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122 public Real
round_to_nearest_multiple(
00123 Real value,
00124 Real multiple_value )
00125 {
00126
int i;
00127 Real factor, nearest;
00128
00129 multiple_value = FABS( multiple_value );
00130
00131 factor = value / multiple_value;
00132
if( factor > (Real) INT_MAX || factor < (Real) INT_MIN )
00133 nearest = value;
00134
else
00135 {
00136 i = ROUND( value / multiple_value );
00137 nearest = (Real) i * multiple_value;
00138 }
00139
00140
return( nearest );
00141 }
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 public int solve_quadratic(
00160 Real a,
00161 Real b,
00162 Real c,
00163 Real *solution1,
00164 Real *solution2 )
00165 {
00166
double inside_sqrt;
00167
int n_solutions;
00168
00169
if( a == 0.0 )
00170 {
00171
if( b == 0.0 )
00172 n_solutions = 0;
00173
else
00174 {
00175 n_solutions = 1;
00176 *solution1 = -c / b;
00177 }
00178 }
00179
else
00180 {
00181 inside_sqrt = (b * b - 4.0 * a * c);
00182
00183
if( inside_sqrt < 0.0 )
00184 {
00185 n_solutions = 0;
00186 }
00187
else if( inside_sqrt == 0.0 )
00188 {
00189 n_solutions = 1;
00190 *solution1 = - b / 2.0 / a;
00191 }
00192
else
00193 {
00194 n_solutions = 2;
00195 inside_sqrt = sqrt( (
double) inside_sqrt );
00196 *solution1 = (- b - inside_sqrt) / 2.0 / a;
00197 *solution2 = (- b + inside_sqrt) / 2.0 / a;
00198 }
00199 }
00200
00201
return( n_solutions );
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232 #define EQN_EPS 1e-9
00233 #define IsZero(x) ((x) > -EQN_EPS && (x) < EQN_EPS)
00234
00235
#ifdef NO_CBRT
00236
#define cbrt(x) ((x) > 0.0 ? pow((double)(x), 1.0/3.0) : \
00237
((x) < 0.0 ? -pow((double)-(x), 1.0/3.0) : 0.0))
00238
#endif
00239
00240 #define COS_60 0.5
00241 #define SIN_60 0.86602540378443864676
00242
00243
00244 public int solve_cubic(
00245 Real a,
00246 Real b,
00247 Real c,
00248 Real d,
00249 Real s[ 3 ] )
00250 {
00251
int num;
00252
double A, B, C, a3, cs, sn;
00253
double sq_A3, p, q;
00254
double cb_p, D, phi, t, sr_p, tc;
00255
00256
if(
IsZero(a) )
00257 {
00258
return(
solve_quadratic( b, c, d, &s[0], &s[1] ) );
00259 }
00260
00261
00262
00263 A = b / a;
00264 B = c / a;
00265 C = d / a;
00266
00267
00268
00269
00270 a3 = A / 3.0;
00271 sq_A3 = a3 * a3;
00272 p = B / 3.0 - sq_A3 ;
00273 q = a3 * sq_A3 + (C - a3 * B) / 2.0;
00274
00275
00276
00277 cb_p = p * p * p;
00278 D = q * q + cb_p;
00279
00280
if (
IsZero(D))
00281 {
00282
if (
IsZero(q))
00283 {
00284 s[ 0 ] = -a3;
00285 num = 1;
00286 }
00287
else
00288 {
00289
double u = cbrt(-q);
00290 s[ 0 ] = 2.0 * u - a3;
00291 s[ 1 ] = - u - a3;
00292 num = 2;
00293 }
00294 }
00295
else if (D < 0.0)
00296 {
00297 sr_p = sqrt( -p );
00298 phi = acos(-q / (sr_p*sr_p*sr_p) ) / 3.0;
00299 t = 2.0 * sr_p;
00300
00301 tc = t * cos( phi );
00302 cs = - tc *
COS_60 - a3;
00303 sn = t *
SIN_60 * sin( phi );
00304
00305 s[ 0 ] = tc - a3;
00306 s[ 1 ] = cs + sn;
00307 s[ 2 ] = cs - sn;
00308 num = 3;
00309 }
00310
else
00311 {
00312
double sqrt_D = sqrt(D);
00313
double u = cbrt(sqrt_D - q);
00314
double v = - cbrt(sqrt_D + q);
00315
00316 s[ 0 ] = u + v - a3;
00317 num = 1;
00318 }
00319
00320
return num;
00321 }
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340 private int get_roots_of_cubic(
00341
int n,
00342 Real poly[],
00343 Real u_min,
00344 Real u_max,
00345 Real roots[] )
00346 {
00347
int n_roots, i, j, k, n_inside;
00348 Real cubic[4], tmp;
00349
00350 for_less( i, 0, 4 )
00351 {
00352
if( i >= n )
00353 cubic[i] = 0.0;
00354
else
00355 cubic[i] = poly[i];
00356 }
00357
00358
if( n < 4 || cubic[3] == 0.0 )
00359 {
00360 n_roots =
solve_quadratic( cubic[2], cubic[1], cubic[0],
00361 &roots[0], &roots[1] );
00362 }
00363
else
00364 n_roots =
solve_cubic( cubic[3], cubic[2], cubic[1], cubic[0], roots );
00365
00366 n_inside = 0;
00367 for_less( i, 0, n_roots )
00368 {
00369
if( u_min > u_max || (u_min <= roots[i] && roots[i] <= u_max) )
00370 {
00371 roots[n_inside] = roots[i];
00372 ++n_inside;
00373 }
00374 }
00375
00376 for_less( i, 0, n_inside-1 )
00377 {
00378 k = i;
00379 for_less( j, i+1, n_inside )
00380 {
00381
if( roots[j] < roots[k] )
00382 k = j;
00383 }
00384
00385
if( k != i )
00386 {
00387 tmp = roots[i];
00388 roots[i] = roots[k];
00389 roots[k] = tmp;
00390 }
00391 }
00392
00393
return( n_inside );
00394 }
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411 public Real
evaluate_polynomial(
00412
int n,
00413 Real poly[],
00414 Real u )
00415 {
00416
int i;
00417 Real val;
00418
00419 val = 0.0;
00420
00421
for( i = n-1; i >= 0; --i )
00422 val = val * u + poly[i];
00423
00424
return( val );
00425 }
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448 private void interval_value_range(
00449
int n,
00450 Real poly[],
00451 Real u_min,
00452 Real u_max,
00453 Real *min_val_ptr,
00454 Real *max_val_ptr )
00455 {
00456
int i;
00457 Real min_val, max_val, t1, t2, t3, t4;
00458
00459 min_val = 0.0;
00460 max_val = 0.0;
00461
00462
if( u_min >= 0.0 )
00463 {
00464
for( i = n-1; i >= 0; --i )
00465 {
00466
if( i != n-1 )
00467 {
00468
if( min_val < 0.0 )
00469 {
00470 min_val *= u_max;
00471
if( max_val < 0.0 )
00472 max_val *= u_min;
00473
else
00474 max_val *= u_max;
00475 }
00476
else
00477 {
00478 min_val *= u_min;
00479 max_val *= u_max;
00480 }
00481
00482 min_val += poly[i];
00483 max_val += poly[i];
00484 }
00485
else
00486 {
00487 min_val = poly[i];
00488 max_val = poly[i];
00489 }
00490 }
00491 }
00492
else
00493 {
00494
for( i = n-1; i >= 0; --i )
00495 {
00496 t1 = min_val * u_min;
00497 t2 = max_val * u_min;
00498 t3 = min_val * u_max;
00499 t4 = max_val * u_max;
00500 min_val = t1;
00501 max_val = t1;
00502
if( t2 < min_val )
00503 min_val = t2;
00504
else if( t2 > max_val )
00505 max_val = t2;
00506
if( t3 < min_val )
00507 min_val = t3;
00508
else if( t3 > max_val )
00509 max_val = t3;
00510
if( t4 < min_val )
00511 min_val = t4;
00512
else if( t4 > max_val )
00513 max_val = t4;
00514 min_val += poly[i];
00515 max_val += poly[i];
00516 }
00517 }
00518
00519 *min_val_ptr = min_val;
00520 *max_val_ptr = max_val;
00521 }
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540 private BOOLEAN
interval_may_contain_zero(
00541
int n,
00542 Real poly[],
00543 Real u_min,
00544 Real u_max )
00545 {
00546 Real min_val, max_val;
00547
00548
interval_value_range( n, poly, u_min, u_max, &min_val, &max_val );
00549
00550
return( min_val <= 0.0 && max_val >= 0.0 );
00551 }
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571 private void check_interval(
00572
int n,
00573 Real poly[],
00574 Real u_min,
00575 Real u_max,
00576 Real accuracy,
00577
int *n_roots,
00578 Real roots[] )
00579 {
00580
int i, which;
00581 Real u_mid, min_diff, diff;
00582
00583
if(
interval_may_contain_zero( n, poly, u_min, u_max ) )
00584 {
00585 u_mid = (u_max + u_min) / 2.0;
00586
if( u_max - u_min > accuracy )
00587 {
00588
check_interval( n, poly, u_min, u_mid, accuracy, n_roots, roots );
00589
check_interval( n, poly, u_mid, u_max, accuracy, n_roots, roots );
00590 }
00591
else
00592 {
00593
if( *n_roots == n-1 )
00594 {
00595 which = 0;
00596 min_diff = 0.0;
00597 for_less( i, 0, *n_roots-1 )
00598 {
00599 diff = roots[i+1] - roots[i];
00600
if( which == 0 || diff < min_diff )
00601 {
00602 min_diff = diff;
00603 which = i;
00604 }
00605 }
00606
00607 for_less( i, which+1, *n_roots-1 )
00608 roots[i] = roots[i+1];
00609 --(*n_roots);
00610 }
00611
00612 roots[*n_roots] = u_mid;
00613 ++(*n_roots);
00614 }
00615 }
00616 }
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635 public int get_roots_of_polynomial(
00636
int n,
00637 Real poly[],
00638 Real u_min,
00639 Real u_max,
00640 Real accuracy,
00641 Real roots[] )
00642 {
00643
int n_roots;
00644
00645
if( n <= 4 )
00646
return(
get_roots_of_cubic( n, poly, u_min, u_max, roots ) );
00647
00648 n_roots = 0;
00649
00650
check_interval( n, poly, u_min, u_max, accuracy, &n_roots, roots );
00651
00652
return( n_roots );
00653 }
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674 private BOOLEAN
polynomial_may_include_range(
00675
int n,
00676 Real poly[],
00677 Real u_min,
00678 Real u_max,
00679 Real min_val,
00680 Real max_val )
00681 {
00682 Real min_range, max_range;
00683
00684
interval_value_range( n, poly, u_min, u_max, &min_range, &max_range );
00685
00686
return( max_val >= min_range && min_val <= max_range );
00687 }
00688
00689 private BOOLEAN
check_range(
00690
int n,
00691 Real poly[],
00692 Real u_min,
00693 Real u_max,
00694 Real min_val,
00695 Real max_val,
00696 Real accuracy,
00697 Real *u_min_range,
00698 Real *u_max_range )
00699 {
00700 Real u_mid;
00701 BOOLEAN left_has_value, right_has_value;
00702
00703
if( u_max - u_min <= accuracy )
00704 {
00705 *u_min_range = u_min;
00706 *u_max_range = u_max;
00707
return(
TRUE );
00708 }
00709
00710 u_mid = (u_min + u_max) / 2.0;
00711
00712 left_has_value =
polynomial_may_include_range( n, poly, u_min, u_mid,
00713 min_val, max_val );
00714 right_has_value =
polynomial_may_include_range( n, poly, u_mid, u_max,
00715 min_val, max_val );
00716
00717
if( left_has_value && right_has_value )
00718 {
00719 *u_min_range = u_min;
00720 *u_max_range = u_max;
00721
return(
TRUE );
00722 }
00723
else if( left_has_value && !right_has_value )
00724 {
00725
return(
check_range( n, poly, u_min, u_mid, min_val, max_val,
00726 accuracy, u_min_range, u_max_range ) );
00727 }
00728
else if( !left_has_value && right_has_value )
00729 {
00730
return(
check_range( n, poly, u_mid, u_max, min_val, max_val,
00731 accuracy, u_min_range, u_max_range ) );
00732 }
00733
else
00734
return(
FALSE );
00735 }
00736
00737 public BOOLEAN
get_range_of_polynomial(
00738
int n,
00739 Real poly[],
00740 Real u_min,
00741 Real u_max,
00742 Real min_val,
00743 Real max_val,
00744 Real accuracy,
00745 Real *u_min_range,
00746 Real *u_max_range )
00747 {
00748 BOOLEAN found;
00749
00750
if(
polynomial_may_include_range( n, poly, u_min, u_max, min_val, max_val ))
00751 {
00752 found =
check_range( n, poly, u_min, u_max, min_val, max_val, accuracy,
00753 u_min_range, u_max_range );
00754 }
00755
else
00756 found =
FALSE;
00757
00758
return( found );
00759 }