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 "common.h"
00029
00030 #include <stdio.h>
00031 #include <math.h>
00032 #include <signal.h>
00033
00034 #include "bu.h"
00035 #include "vmath.h"
00036 #include "bn.h"
00037
00038
00039 #define Max(a, b) ((a) > (b) ? (a) : (b))
00040
00041 #define PI_DIV_3 (M_PI/3.0)
00042
00043 #define SQRT3 1.732050808
00044 #define THIRD 0.333333333333333333333333333
00045 #define INV_TWENTYSEVEN 0.037037037037037037037037037
00046 #define CUBEROOT(a) (((a) >= 0.0) ? pow(a, THIRD) : -pow(-(a), THIRD))
00047
00048 static const struct bn_poly bn_Zero_poly = { BN_POLY_MAGIC, 0, {0.0} };
00049 static int bn_expecting_fpe = 0;
00050 static jmp_buf bn_abort_buf;
00051
00052
00053 HIDDEN void bn_catch_FPE(int sig)
00054 {
00055 if (sig != SIGFPE)
00056 bu_bomb("bn_catch_FPE() unexpected signal!");
00057 if (!bn_expecting_fpe)
00058 bu_bomb("bn_catch_FPE() unexpected SIGFPE!");
00059 if (!bu_is_parallel())
00060 (void)signal(SIGFPE, bn_catch_FPE);
00061 longjmp(bn_abort_buf, 1);
00062 }
00063
00064
00065
00066
00067
00068
00069
00070 struct bn_poly *
00071 bn_poly_mul(register struct bn_poly *product, register const struct bn_poly *m1, register const struct bn_poly *m2)
00072 {
00073 if (m1->dgr == 1 && m2->dgr == 1) {
00074 product->dgr = 2;
00075 product->cf[0] = m1->cf[0] * m2->cf[0];
00076 product->cf[1] = m1->cf[0] * m2->cf[1] +
00077 m1->cf[1] * m2->cf[0];
00078 product->cf[2] = m1->cf[1] * m2->cf[1];
00079 return product;
00080 }
00081 if (m1->dgr == 2 && m2->dgr == 2) {
00082 product->dgr = 4;
00083 product->cf[0] = m1->cf[0] * m2->cf[0];
00084 product->cf[1] = m1->cf[0] * m2->cf[1] +
00085 m1->cf[1] * m2->cf[0];
00086 product->cf[2] = m1->cf[0] * m2->cf[2] +
00087 m1->cf[1] * m2->cf[1] +
00088 m1->cf[2] * m2->cf[0];
00089 product->cf[3] = m1->cf[1] * m2->cf[2] +
00090 m1->cf[2] * m2->cf[1];
00091 product->cf[4] = m1->cf[2] * m2->cf[2];
00092 return product;
00093 }
00094
00095
00096 {
00097 register size_t ct1, ct2;
00098
00099 *product = bn_Zero_poly;
00100
00101
00102
00103
00104
00105 if ((product->dgr = m1->dgr + m2->dgr) > BN_MAX_POLY_DEGREE)
00106 return BN_POLY_NULL;
00107
00108 for (ct1=0; ct1 <= m1->dgr; ++ct1) {
00109 for (ct2=0; ct2 <= m2->dgr; ++ct2) {
00110 product->cf[ct1+ct2] +=
00111 m1->cf[ct1] * m2->cf[ct2];
00112 }
00113 }
00114 }
00115 return product;
00116 }
00117
00118
00119
00120
00121
00122
00123
00124 struct bn_poly *
00125 bn_poly_scale(register struct bn_poly *eqn, double factor)
00126 {
00127 register size_t cnt;
00128
00129 for (cnt=0; cnt <= eqn->dgr; ++cnt) {
00130 eqn->cf[cnt] *= factor;
00131 }
00132 return eqn;
00133 }
00134
00135
00136
00137
00138
00139
00140
00141 struct bn_poly *
00142 bn_poly_add(register struct bn_poly *sum, register const struct bn_poly *poly1, register const struct bn_poly *poly2)
00143 {
00144 struct bn_poly tmp;
00145 register size_t i, offset;
00146
00147 offset = abs((long)poly1->dgr - (long)poly2->dgr);
00148
00149 tmp = bn_Zero_poly;
00150
00151 if (poly1->dgr >= poly2->dgr) {
00152 *sum = *poly1;
00153 for (i=0; i <= poly2->dgr; ++i) {
00154 tmp.cf[i+offset] = poly2->cf[i];
00155 }
00156 } else {
00157 *sum = *poly2;
00158 for (i=0; i <= poly1->dgr; ++i) {
00159 tmp.cf[i+offset] = poly1->cf[i];
00160 }
00161 }
00162
00163 for (i=0; i <= sum->dgr; ++i) {
00164 sum->cf[i] += tmp.cf[i];
00165 }
00166 return sum;
00167 }
00168
00169
00170
00171
00172
00173
00174
00175 struct bn_poly *
00176 bn_poly_sub(register struct bn_poly *diff, register const struct bn_poly *poly1, register const struct bn_poly *poly2)
00177 {
00178 struct bn_poly tmp;
00179 register size_t i, offset;
00180
00181 offset = abs((long)poly1->dgr - (long)poly2->dgr);
00182
00183 *diff = bn_Zero_poly;
00184 tmp = bn_Zero_poly;
00185
00186 if (poly1->dgr >= poly2->dgr) {
00187 *diff = *poly1;
00188 for (i=0; i <= poly2->dgr; ++i) {
00189 tmp.cf[i+offset] = poly2->cf[i];
00190 }
00191 } else {
00192 diff->dgr = poly2->dgr;
00193 for (i=0; i <= poly1->dgr; ++i) {
00194 diff->cf[i+offset] = poly1->cf[i];
00195 }
00196 tmp = *poly2;
00197 }
00198
00199 for (i=0; i <= diff->dgr; ++i) {
00200 diff->cf[i] -= tmp.cf[i];
00201 }
00202 return diff;
00203 }
00204
00205
00206
00207
00208
00209
00210
00211
00212 void
00213 bn_poly_synthetic_division(register struct bn_poly *quo, register struct bn_poly *rem, register const struct bn_poly *dvdend, register const struct bn_poly *dvsor)
00214 {
00215 register size_t divisor;
00216 register size_t n;
00217
00218 *quo = *dvdend;
00219 *rem = bn_Zero_poly;
00220
00221 if (dvsor->dgr > dvdend->dgr) {
00222 quo->dgr = -1;
00223 } else {
00224 quo->dgr = dvdend->dgr - dvsor->dgr;
00225 }
00226 if ((rem->dgr = dvsor->dgr - 1) > dvdend->dgr)
00227 rem->dgr = dvdend->dgr;
00228
00229 for (n=0; n <= quo->dgr; ++n) {
00230 quo->cf[n] /= dvsor->cf[0];
00231 for (divisor=1; divisor <= dvsor->dgr; ++divisor) {
00232 quo->cf[n+divisor] -= quo->cf[n] * dvsor->cf[divisor];
00233 }
00234 }
00235 for (n=1; n<=(rem->dgr+1); ++n) {
00236 rem->cf[n-1] = quo->cf[quo->dgr+n];
00237 quo->cf[quo->dgr+n] = 0;
00238 }
00239 }
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251 int
00252 bn_poly_quadratic_roots(register struct bn_complex *roots, register const struct bn_poly *quadrat)
00253 {
00254 fastf_t discrim, denom, rad;
00255 const fastf_t small = SMALL_FASTF;
00256
00257 if (NEAR_ZERO(quadrat->cf[0], small)) {
00258
00259 if (NEAR_ZERO(quadrat->cf[1], small)) {
00260
00261
00262 return 0;
00263 }
00264
00265 roots[0].re = roots[1].re = -quadrat->cf[2]/quadrat->cf[1];
00266 roots[0].im = roots[1].im = 0.0;
00267 return 1;
00268 }
00269
00270 discrim = quadrat->cf[1]*quadrat->cf[1] - 4.0* quadrat->cf[0]*quadrat->cf[2];
00271 denom = 0.5 / quadrat->cf[0];
00272
00273 if (discrim > 0.0) {
00274 rad = sqrt(discrim);
00275
00276 if (NEAR_ZERO(quadrat->cf[1], small)) {
00277 double r = fabs(rad * denom);
00278 roots[0].re = r;
00279 roots[1].re = -r;
00280 } else {
00281 double t, r1, r2;
00282
00283 if (quadrat->cf[1] > 0.0) {
00284 t = -0.5 * (quadrat->cf[1] + rad);
00285 } else {
00286 t = -0.5 * (quadrat->cf[1] - rad);
00287 }
00288 r1 = t / quadrat->cf[0];
00289 r2 = quadrat->cf[2] / t;
00290
00291 if (r1 < r2) {
00292 roots[0].re = r1;
00293 roots[1].re = r2;
00294 } else {
00295 roots[0].re = r2;
00296 roots[1].re = r1;
00297 }
00298 }
00299 roots[1].im = roots[0].im = 0.0;
00300 } else if (NEAR_ZERO(discrim, small)) {
00301 roots[1].re = roots[0].re = -quadrat->cf[1] * denom;
00302 roots[1].im = roots[0].im = 0.0;
00303 } else {
00304 roots[1].re = roots[0].re = -quadrat->cf[1] * denom;
00305 roots[1].im = -(roots[0].im = sqrt(-discrim) * denom);
00306 }
00307 return 1;
00308 }
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341 int
00342 bn_poly_cubic_roots(register struct bn_complex *roots, register const struct bn_poly *eqn)
00343 {
00344 fastf_t a, b, c1, c1_3rd, delta;
00345 register int i;
00346 static int first_time = 1;
00347
00348 if (!bu_is_parallel()) {
00349
00350 if (first_time) {
00351 first_time = 0;
00352 (void)signal(SIGFPE, bn_catch_FPE);
00353 }
00354 bn_expecting_fpe = 1;
00355 if (setjmp(bn_abort_buf)) {
00356 (void)signal(SIGFPE, bn_catch_FPE);
00357 bu_log("bn_poly_cubic_roots() Floating Point Error\n");
00358 return 0;
00359 }
00360 }
00361
00362 c1 = eqn->cf[1];
00363 if (abs(c1) > SQRT_MAX_FASTF) return 0;
00364
00365 c1_3rd = c1 * THIRD;
00366 a = eqn->cf[2] - c1*c1_3rd;
00367 if (abs(a) > SQRT_MAX_FASTF) return 0;
00368 b = (2.0*c1*c1*c1 - 9.0*c1*eqn->cf[2] + 27.0*eqn->cf[3])*INV_TWENTYSEVEN;
00369 if (abs(b) > SQRT_MAX_FASTF) return 0;
00370
00371 if ((delta = a*a) > SQRT_MAX_FASTF) return 0;
00372 delta = b*b*0.25 + delta*a*INV_TWENTYSEVEN;
00373
00374 if (delta > 0.0) {
00375 fastf_t r_delta, A, B;
00376
00377 r_delta = sqrt(delta);
00378 A = B = -0.5 * b;
00379 A += r_delta;
00380 B -= r_delta;
00381
00382 A = CUBEROOT(A);
00383 B = CUBEROOT(B);
00384
00385 roots[2].re = roots[1].re = -0.5 * (roots[0].re = A + B);
00386
00387 roots[0].im = 0.0;
00388 roots[2].im = -(roots[1].im = (A - B)*SQRT3*0.5);
00389 } else if (ZERO(delta)) {
00390 fastf_t b_2;
00391 b_2 = -0.5 * b;
00392
00393 roots[0].re = 2.0* CUBEROOT(b_2);
00394 roots[2].re = roots[1].re = -0.5 * roots[0].re;
00395 roots[2].im = roots[1].im = roots[0].im = 0.0;
00396 } else {
00397 fastf_t phi, fact;
00398 fastf_t cs_phi, sn_phi_s3;
00399
00400 if (a >= 0.0) {
00401 fact = 0.0;
00402 phi = 0.0;
00403 cs_phi = 1.0;
00404 sn_phi_s3 = 0.0;
00405 } else {
00406 register fastf_t f;
00407 a *= -THIRD;
00408 fact = sqrt(a);
00409 if ((f = b * (-0.5) / (a*fact)) >= 1.0) {
00410 phi = 0.0;
00411 cs_phi = 1.0;
00412 sn_phi_s3 = 0.0;
00413 } else if (f <= -1.0) {
00414 phi = PI_DIV_3;
00415 cs_phi = cos(phi);
00416 sn_phi_s3 = sin(phi) * SQRT3;
00417 } else {
00418 phi = acos(f) * THIRD;
00419 cs_phi = cos(phi);
00420 sn_phi_s3 = sin(phi) * SQRT3;
00421 }
00422 }
00423
00424 roots[0].re = 2.0*fact*cs_phi;
00425 roots[1].re = fact*( sn_phi_s3 - cs_phi);
00426 roots[2].re = fact*(-sn_phi_s3 - cs_phi);
00427 roots[2].im = roots[1].im = roots[0].im = 0.0;
00428 }
00429 for (i=0; i < 3; ++i)
00430 roots[i].re -= c1_3rd;
00431
00432 if (!bu_is_parallel())
00433 bn_expecting_fpe = 0;
00434
00435 return 1;
00436 }
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448 int
00449 bn_poly_quartic_roots(register struct bn_complex *roots, register const struct bn_poly *eqn)
00450 {
00451 struct bn_poly cube, quad1, quad2;
00452 bn_complex_t u[3];
00453 fastf_t U, p, q, q1, q2;
00454
00455
00456 const fastf_t small = 1.0e-8;
00457
00458 #define Max3(a, b, c) ((c)>((a)>(b)?(a):(b)) ? (c) : ((a)>(b)?(a):(b)))
00459
00460 cube.dgr = 3;
00461 cube.cf[0] = 1.0;
00462 cube.cf[1] = -eqn->cf[2];
00463 cube.cf[2] = eqn->cf[3]*eqn->cf[1]
00464 - 4*eqn->cf[4];
00465 cube.cf[3] = -eqn->cf[3]*eqn->cf[3]
00466 - eqn->cf[4]*eqn->cf[1]*eqn->cf[1]
00467 + 4*eqn->cf[4]*eqn->cf[2];
00468
00469 if (!bn_poly_cubic_roots(u, &cube)) {
00470 return 0;
00471 }
00472 if (!ZERO(u[1].im)) {
00473 U = u[0].re;
00474 } else {
00475 U = Max3(u[0].re, u[1].re, u[2].re);
00476 }
00477
00478 p = eqn->cf[1]*eqn->cf[1]*0.25 + U - eqn->cf[2];
00479 U *= 0.5;
00480 q = U*U - eqn->cf[4];
00481 if (p < 0) {
00482 if (p < -small) {
00483 return 0;
00484 }
00485 p = 0;
00486 } else {
00487 p = sqrt(p);
00488 }
00489 if (q < 0) {
00490 if (q < -small) {
00491 return 0;
00492 }
00493 q = 0;
00494 } else {
00495 q = sqrt(q);
00496 }
00497
00498 quad1.dgr = quad2.dgr = 2;
00499 quad1.cf[0] = quad2.cf[0] = 1.0;
00500 quad1.cf[1] = eqn->cf[1]*0.5;
00501 quad2.cf[1] = quad1.cf[1] + p;
00502 quad1.cf[1] -= p;
00503
00504 q1 = U - q;
00505 q2 = U + q;
00506
00507 p = quad1.cf[1]*q2 + quad2.cf[1]*q1 - eqn->cf[3];
00508 if (NEAR_ZERO(p, small)) {
00509 quad1.cf[2] = q1;
00510 quad2.cf[2] = q2;
00511 } else {
00512 q = quad1.cf[1]*q1 + quad2.cf[1]*q2 - eqn->cf[3];
00513 if (NEAR_ZERO(q, small)) {
00514 quad1.cf[2] = q2;
00515 quad2.cf[2] = q1;
00516 } else {
00517 return 0;
00518 }
00519 }
00520
00521 bn_poly_quadratic_roots(&roots[0], &quad1);
00522 bn_poly_quadratic_roots(&roots[2], &quad2);
00523 return 1;
00524 }
00525
00526
00527
00528
00529
00530
00531
00532 void
00533 bn_pr_poly(const char *title, register const struct bn_poly *eqn)
00534 {
00535 register size_t n;
00536 register size_t exponent;
00537 struct bu_vls str = BU_VLS_INIT_ZERO;
00538 char buf[48];
00539
00540 bu_vls_extend(&str, 196);
00541 bu_vls_strcat(&str, title);
00542 snprintf(buf, 48, " polynomial, degree = %lu\n", (long unsigned int)eqn->dgr);
00543 bu_vls_strcat(&str, buf);
00544
00545 exponent = eqn->dgr;
00546 for (n=0; n<=eqn->dgr; n++, exponent--) {
00547 register double coeff = eqn->cf[n];
00548 if (n > 0) {
00549 if (coeff < 0) {
00550 bu_vls_strcat(&str, " - ");
00551 coeff = -coeff;
00552 } else {
00553 bu_vls_strcat(&str, " + ");
00554 }
00555 }
00556 bu_vls_printf(&str, "%g", coeff);
00557 if (exponent > 1) {
00558 bu_vls_printf(&str, " *X^%zu", exponent);
00559 } else if (exponent == 1) {
00560
00561 bu_vls_strcat(&str, " *X");
00562 } else {
00563
00564 }
00565 }
00566 bu_vls_strcat(&str, "\n");
00567 bu_log("%s", bu_vls_addr(&str));
00568 bu_vls_free(&str);
00569 }
00570
00571
00572
00573
00574
00575
00576 void
00577 bn_pr_roots(const char *title, const struct bn_complex *roots, int n)
00578 {
00579 register int i;
00580
00581 bu_log("%s: %d roots:\n", title, n);
00582 for (i=0; i<n; i++) {
00583 bu_log("%4d %e + i * %e\n", i, roots[i].re, roots[i].im);
00584 }
00585 }
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596