neo/idlib/math/Polynomial.h

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #ifndef __MATH_POLYNOMIAL_H__
  30 #define __MATH_POLYNOMIAL_H__
  31
  32 /*
  33 ===============================================================================
  34
  35         Polynomial of arbitrary degree with real coefficients.
  36
  37 ===============================================================================
  38 */
  39
  40
  41 class idPolynomial {
  42 public:
  43                                         idPolynomial( void );
  44                                         explicit idPolynomial( int d );
  45                                         explicit idPolynomial( float a, float b );
  46                                         explicit idPolynomial( float a, float b, float c );
  47                                         explicit idPolynomial( float a, float b, float c, float d );
  48                                         explicit idPolynomial( float a, float b, float c, float d, float e );
  49
  50         float                   operator[]( int index ) const;
  51         float &                 operator[]( int index );
  52
  53         idPolynomial    operator-() const;
  54         idPolynomial &  operator=( const idPolynomial &p );
  55
  56         idPolynomial    operator+( const idPolynomial &p ) const;
  57         idPolynomial    operator-( const idPolynomial &p ) const;
  58         idPolynomial    operator*( const float s ) const;
  59         idPolynomial    operator/( const float s ) const;
  60
  61         idPolynomial &  operator+=( const idPolynomial &p );
  62         idPolynomial &  operator-=( const idPolynomial &p );
  63         idPolynomial &  operator*=( const float s );
  64         idPolynomial &  operator/=( const float s );
  65
  66         bool                    Compare( const idPolynomial &p ) const;                                         // exact compare, no epsilon
  67         bool                    Compare( const idPolynomial &p, const float epsilon ) const;// compare with epsilon
  68         bool                    operator==(     const idPolynomial &p ) const;                                  // exact compare, no epsilon
  69         bool                    operator!=(     const idPolynomial &p ) const;                                  // exact compare, no epsilon
  70
  71         void                    Zero( void );
  72         void                    Zero( int d );
  73
  74         int                             GetDimension( void ) const;                                                                     // get the degree of the polynomial
  75         int                             GetDegree( void ) const;                                                                        // get the degree of the polynomial
  76         float                   GetValue( const float x ) const;                                                        // evaluate the polynomial with the given real value
  77         idComplex               GetValue( const idComplex &x ) const;                                           // evaluate the polynomial with the given complex value
  78         idPolynomial    GetDerivative( void ) const;                                                            // get the first derivative of the polynomial
  79         idPolynomial    GetAntiDerivative( void ) const;                                                        // get the anti derivative of the polynomial
  80
  81         int                             GetRoots( idComplex *roots ) const;                                                     // get all roots
  82         int                             GetRoots( float *roots ) const;                                                         // get the real roots
  83
  84         static int              GetRoots1( float a, float b, float *roots );
  85         static int              GetRoots2( float a, float b, float c, float *roots );
  86         static int              GetRoots3( float a, float b, float c, float d, float *roots );
  87         static int              GetRoots4( float a, float b, float c, float d, float e, float *roots );
  88
  89         const float *   ToFloatPtr( void ) const;
  90         float *                 ToFloatPtr( void );
  91         const char *    ToString( int precision = 2 ) const;
  92
  93         static void             Test( void );
  94
  95 private:
  96         int                             degree;
  97         int                             allocated;
  98         float *                 coefficient;
  99
 100         void                    Resize( int d, bool keep );
 101         int                             Laguer( const idComplex *coef, const int degree, idComplex &r ) const;
 102 };
 103
 104 ID_INLINE idPolynomial::idPolynomial( void ) {
 105         degree = -1;
 106         allocated = 0;
 107         coefficient = NULL;
 108 }
 109
 110 ID_INLINE idPolynomial::idPolynomial( int d ) {
 111         degree = -1;
 112         allocated = 0;
 113         coefficient = NULL;
 114         Resize( d, false );
 115 }
 116
 117 ID_INLINE idPolynomial::idPolynomial( float a, float b ) {
 118         degree = -1;
 119         allocated = 0;
 120         coefficient = NULL;
 121         Resize( 1, false );
 122         coefficient[0] = b;
 123         coefficient[1] = a;
 124 }
 125
 126 ID_INLINE idPolynomial::idPolynomial( float a, float b, float c ) {
 127         degree = -1;
 128         allocated = 0;
 129         coefficient = NULL;
 130         Resize( 2, false );
 131         coefficient[0] = c;
 132         coefficient[1] = b;
 133         coefficient[2] = a;
 134 }
 135
 136 ID_INLINE idPolynomial::idPolynomial( float a, float b, float c, float d ) {
 137         degree = -1;
 138         allocated = 0;
 139         coefficient = NULL;
 140         Resize( 3, false );
 141         coefficient[0] = d;
 142         coefficient[1] = c;
 143         coefficient[2] = b;
 144         coefficient[3] = a;
 145 }
 146
 147 ID_INLINE idPolynomial::idPolynomial( float a, float b, float c, float d, float e ) {
 148         degree = -1;
 149         allocated = 0;
 150         coefficient = NULL;
 151         Resize( 4, false );
 152         coefficient[0] = e;
 153         coefficient[1] = d;
 154         coefficient[2] = c;
 155         coefficient[3] = b;
 156         coefficient[4] = a;
 157 }
 158
 159 ID_INLINE float idPolynomial::operator[]( int index ) const {
 160         assert( index >= 0 && index <= degree );
 161         return coefficient[ index ];
 162 }
 163
 164 ID_INLINE float& idPolynomial::operator[]( int index ) {
 165         assert( index >= 0 && index <= degree );
 166         return coefficient[ index ];
 167 }
 168
 169 ID_INLINE idPolynomial idPolynomial::operator-() const {
 170         int i;
 171         idPolynomial n;
 172
 173         n = *this;
 174         for ( i = 0; i <= degree; i++ ) {
 175                 n[i] = -n[i];
 176         }
 177         return n;
 178 }
 179
 180 ID_INLINE idPolynomial &idPolynomial::operator=( const idPolynomial &p ) {
 181         Resize( p.degree, false );
 182         for ( int i = 0; i <= degree; i++ ) {
 183                 coefficient[i] = p.coefficient[i];
 184         }
 185         return *this;
 186 }
 187
 188 ID_INLINE idPolynomial idPolynomial::operator+( const idPolynomial &p ) const {
 189         int i;
 190         idPolynomial n;
 191
 192         if ( degree > p.degree ) {
 193                 n.Resize( degree, false );
 194                 for ( i = 0; i <= p.degree; i++ ) {
 195                         n.coefficient[i] = coefficient[i] + p.coefficient[i];
 196                 }
 197                 for ( ; i <= degree; i++ ) {
 198                         n.coefficient[i] = coefficient[i];
 199                 }
 200                 n.degree = degree;
 201         } else if ( p.degree > degree ) {
 202                 n.Resize( p.degree, false );
 203                 for ( i = 0; i <= degree; i++ ) {
 204                         n.coefficient[i] = coefficient[i] + p.coefficient[i];
 205                 }
 206                 for ( ; i <= p.degree; i++ ) {
 207                         n.coefficient[i] = p.coefficient[i];
 208                 }
 209                 n.degree = p.degree;
 210         } else {
 211                 n.Resize( degree, false );
 212                 n.degree = 0;
 213                 for ( i = 0; i <= degree; i++ ) {
 214                         n.coefficient[i] = coefficient[i] + p.coefficient[i];
 215                         if ( n.coefficient[i] != 0.0f ) {
 216                                 n.degree = i;
 217                         }
 218                 }
 219         }
 220         return n;
 221 }
 222
 223 ID_INLINE idPolynomial idPolynomial::operator-( const idPolynomial &p ) const {
 224         int i;
 225         idPolynomial n;
 226
 227         if ( degree > p.degree ) {
 228                 n.Resize( degree, false );
 229                 for ( i = 0; i <= p.degree; i++ ) {
 230                         n.coefficient[i] = coefficient[i] - p.coefficient[i];
 231                 }
 232                 for ( ; i <= degree; i++ ) {
 233                         n.coefficient[i] = coefficient[i];
 234                 }
 235                 n.degree = degree;
 236         } else if ( p.degree >= degree ) {
 237                 n.Resize( p.degree, false );
 238                 for ( i = 0; i <= degree; i++ ) {
 239                         n.coefficient[i] = coefficient[i] - p.coefficient[i];
 240                 }
 241                 for ( ; i <= p.degree; i++ ) {
 242                         n.coefficient[i] = - p.coefficient[i];
 243                 }
 244                 n.degree = p.degree;
 245         } else {
 246                 n.Resize( degree, false );
 247                 n.degree = 0;
 248                 for ( i = 0; i <= degree; i++ ) {
 249                         n.coefficient[i] = coefficient[i] - p.coefficient[i];
 250                         if ( n.coefficient[i] != 0.0f ) {
 251                                 n.degree = i;
 252                         }
 253                 }
 254         }
 255         return n;
 256 }
 257
 258 ID_INLINE idPolynomial idPolynomial::operator*( const float s ) const {
 259         idPolynomial n;
 260
 261         if ( s == 0.0f ) {
 262                 n.degree = 0;
 263         } else {
 264                 n.Resize( degree, false );
 265                 for ( int i = 0; i <= degree; i++ ) {
 266                         n.coefficient[i] = coefficient[i] * s;
 267                 }
 268         }
 269         return n;
 270 }
 271
 272 ID_INLINE idPolynomial idPolynomial::operator/( const float s ) const {
 273         float invs;
 274         idPolynomial n;
 275
 276         assert( s != 0.0f );
 277         n.Resize( degree, false );
 278         invs = 1.0f / s;
 279         for ( int i = 0; i <= degree; i++ ) {
 280                 n.coefficient[i] = coefficient[i] * invs;
 281         }
 282         return n;
 283 }
 284
 285 ID_INLINE idPolynomial &idPolynomial::operator+=( const idPolynomial &p ) {
 286         int i;
 287
 288         if ( degree > p.degree ) {
 289                 for ( i = 0; i <= p.degree; i++ ) {
 290                         coefficient[i] += p.coefficient[i];
 291                 }
 292         } else if ( p.degree > degree ) {
 293                 Resize( p.degree, true );
 294                 for ( i = 0; i <= degree; i++ ) {
 295                         coefficient[i] += p.coefficient[i];
 296                 }
 297                 for ( ; i <= p.degree; i++ ) {
 298                         coefficient[i] = p.coefficient[i];
 299                 }
 300         } else {
 301                 for ( i = 0; i <= degree; i++ ) {
 302                         coefficient[i] += p.coefficient[i];
 303                         if ( coefficient[i] != 0.0f ) {
 304                                 degree = i;
 305                         }
 306                 }
 307         }
 308         return *this;
 309 }
 310
 311 ID_INLINE idPolynomial &idPolynomial::operator-=( const idPolynomial &p ) {
 312         int i;
 313
 314         if ( degree > p.degree ) {
 315                 for ( i = 0; i <= p.degree; i++ ) {
 316                         coefficient[i] -= p.coefficient[i];
 317                 }
 318         } else if ( p.degree > degree ) {
 319                 Resize( p.degree, true );
 320                 for ( i = 0; i <= degree; i++ ) {
 321                         coefficient[i] -= p.coefficient[i];
 322                 }
 323                 for ( ; i <= p.degree; i++ ) {
 324                         coefficient[i] = - p.coefficient[i];
 325                 }
 326         } else {
 327                 for ( i = 0; i <= degree; i++ ) {
 328                         coefficient[i] -= p.coefficient[i];
 329                         if ( coefficient[i] != 0.0f ) {
 330                                 degree = i;
 331                         }
 332                 }
 333         }
 334         return *this;
 335 }
 336
 337 ID_INLINE idPolynomial &idPolynomial::operator*=( const float s ) {
 338         if ( s == 0.0f ) {
 339                 degree = 0;
 340         } else {
 341                 for ( int i = 0; i <= degree; i++ ) {
 342                         coefficient[i] *= s;
 343                 }
 344         }
 345         return *this;
 346 }
 347
 348 ID_INLINE idPolynomial &idPolynomial::operator/=( const float s ) {
 349         float invs;
 350
 351         assert( s != 0.0f );
 352         invs = 1.0f / s;
 353         for ( int i = 0; i <= degree; i++ ) {
 354                 coefficient[i] = invs;
 355         }
 356         return *this;;
 357 }
 358
 359 ID_INLINE bool idPolynomial::Compare( const idPolynomial &p ) const {
 360         if ( degree != p.degree ) {
 361                 return false;
 362         }
 363         for ( int i = 0; i <= degree; i++ ) {
 364                 if ( coefficient[i] != p.coefficient[i] ) {
 365                         return false;
 366                 }
 367         }
 368         return true;
 369 }
 370
 371 ID_INLINE bool idPolynomial::Compare( const idPolynomial &p, const float epsilon ) const {
 372         if ( degree != p.degree ) {
 373                 return false;
 374         }
 375         for ( int i = 0; i <= degree; i++ ) {
 376                 if ( idMath::Fabs( coefficient[i] - p.coefficient[i] ) > epsilon ) {
 377                         return false;
 378                 }
 379         }
 380         return true;
 381 }
 382
 383 ID_INLINE bool idPolynomial::operator==( const idPolynomial &p ) const {
 384         return Compare( p );
 385 }
 386
 387 ID_INLINE bool idPolynomial::operator!=( const idPolynomial &p ) const {
 388         return !Compare( p );
 389 }
 390
 391 ID_INLINE void idPolynomial::Zero( void ) {
 392         degree = 0;
 393 }
 394
 395 ID_INLINE void idPolynomial::Zero( int d ) {
 396         Resize( d, false );
 397         for ( int i = 0; i <= degree; i++ ) {
 398                 coefficient[i] = 0.0f;
 399         }
 400 }
 401
 402 ID_INLINE int idPolynomial::GetDimension( void ) const {
 403         return degree;
 404 }
 405
 406 ID_INLINE int idPolynomial::GetDegree( void ) const {
 407         return degree;
 408 }
 409
 410 ID_INLINE float idPolynomial::GetValue( const float x ) const {
 411         float y, z;
 412         y = coefficient[0];
 413         z = x;
 414         for ( int i = 1; i <= degree; i++ ) {
 415                 y += coefficient[i] * z;
 416                 z *= x;
 417         }
 418         return y;
 419 }
 420
 421 ID_INLINE idComplex idPolynomial::GetValue( const idComplex &x ) const {
 422         idComplex y, z;
 423         y.Set( coefficient[0], 0.0f );
 424         z = x;
 425         for ( int i = 1; i <= degree; i++ ) {
 426                 y += coefficient[i] * z;
 427                 z *= x;
 428         }
 429         return y;
 430 }
 431
 432 ID_INLINE idPolynomial idPolynomial::GetDerivative( void ) const {
 433         idPolynomial n;
 434
 435         if ( degree == 0 ) {
 436                 return n;
 437         }
 438         n.Resize( degree - 1, false );
 439         for ( int i = 1; i <= degree; i++ ) {
 440                 n.coefficient[i-1] = i * coefficient[i];
 441         }
 442         return n;
 443 }
 444
 445 ID_INLINE idPolynomial idPolynomial::GetAntiDerivative( void ) const {
 446         idPolynomial n;
 447
 448         if ( degree == 0 ) {
 449                 return n;
 450         }
 451         n.Resize( degree + 1, false );
 452         n.coefficient[0] = 0.0f;
 453         for ( int i = 0; i <= degree; i++ ) {
 454                 n.coefficient[i+1] = coefficient[i] / ( i + 1 );
 455         }
 456         return n;
 457 }
 458
 459 ID_INLINE int idPolynomial::GetRoots1( float a, float b, float *roots ) {
 460         assert( a != 0.0f );
 461         roots[0] = - b / a;
 462         return 1;
 463 }
 464
 465 ID_INLINE int idPolynomial::GetRoots2( float a, float b, float c, float *roots ) {
 466         float inva, ds;
 467
 468         if ( a != 1.0f ) {
 469                 assert( a != 0.0f );
 470                 inva = 1.0f / a;
 471                 c *= inva;
 472                 b *= inva;
 473         }
 474         ds = b * b - 4.0f * c;
 475         if ( ds < 0.0f ) {
 476                 return 0;
 477         } else if ( ds > 0.0f ) {
 478                 ds = idMath::Sqrt( ds );
 479                 roots[0] = 0.5f * ( -b - ds );
 480                 roots[1] = 0.5f * ( -b + ds );
 481                 return 2;
 482         } else {
 483                 roots[0] = 0.5f * -b;
 484                 return 1;
 485         }
 486 }
 487
 488 ID_INLINE int idPolynomial::GetRoots3( float a, float b, float c, float d, float *roots ) {
 489         float inva, f, g, halfg, ofs, ds, dist, angle, cs, ss, t;
 490
 491         if ( a != 1.0f ) {
 492                 assert( a != 0.0f );
 493                 inva = 1.0f / a;
 494                 d *= inva;
 495                 c *= inva;
 496                 b *= inva;
 497         }
 498
 499         f = ( 1.0f / 3.0f ) * ( 3.0f * c - b * b );
 500         g = ( 1.0f / 27.0f ) * ( 2.0f * b * b * b - 9.0f * c * b + 27.0f * d );
 501         halfg = 0.5f * g;
 502         ofs = ( 1.0f / 3.0f ) * b;
 503         ds = 0.25f * g * g + ( 1.0f / 27.0f ) * f * f * f;
 504
 505         if ( ds < 0.0f ) {
 506                 dist = idMath::Sqrt( ( -1.0f / 3.0f ) * f );
 507                 angle = ( 1.0f / 3.0f ) * idMath::ATan( idMath::Sqrt( -ds ), -halfg );
 508                 cs = idMath::Cos( angle );
 509                 ss = idMath::Sin( angle );
 510                 roots[0] = 2.0f * dist * cs - ofs;
 511                 roots[1] = -dist * ( cs + idMath::SQRT_THREE * ss ) - ofs;
 512                 roots[2] = -dist * ( cs - idMath::SQRT_THREE * ss ) - ofs;
 513                 return 3;
 514         } else if ( ds > 0.0f )  {
 515                 ds = idMath::Sqrt( ds );
 516                 t = -halfg + ds;
 517                 if ( t >= 0.0f ) {
 518                         roots[0] = idMath::Pow( t, ( 1.0f / 3.0f ) );
 519                 } else {
 520                         roots[0] = -idMath::Pow( -t, ( 1.0f / 3.0f ) );
 521                 }
 522                 t = -halfg - ds;
 523                 if ( t >= 0.0f ) {
 524                         roots[0] += idMath::Pow( t, ( 1.0f / 3.0f ) );
 525                 } else {
 526                         roots[0] -= idMath::Pow( -t, ( 1.0f / 3.0f ) );
 527                 }
 528                 roots[0] -= ofs;
 529                 return 1;
 530         } else {
 531                 if ( halfg >= 0.0f ) {
 532                         t = -idMath::Pow( halfg, ( 1.0f / 3.0f ) );
 533                 } else {
 534                         t = idMath::Pow( -halfg, ( 1.0f / 3.0f ) );
 535                 }
 536                 roots[0] = 2.0f * t - ofs;
 537                 roots[1] = -t - ofs;
 538                 roots[2] = roots[1];
 539                 return 3;
 540         }
 541 }
 542
 543 ID_INLINE int idPolynomial::GetRoots4( float a, float b, float c, float d, float e, float *roots ) {
 544         int count;
 545         float inva, y, ds, r, s1, s2, t1, t2, tp, tm;
 546         float roots3[3];
 547
 548         if ( a != 1.0f ) {
 549                 assert( a != 0.0f );
 550                 inva = 1.0f / a;
 551                 e *= inva;
 552                 d *= inva;
 553                 c *= inva;
 554                 b *= inva;
 555         }
 556
 557         count = 0;
 558
 559         GetRoots3( 1.0f, -c, b * d - 4.0f * e, -b * b * e + 4.0f * c * e - d * d, roots3 );
 560         y = roots3[0];
 561         ds = 0.25f * b * b - c + y;
 562
 563         if ( ds < 0.0f ) {
 564                 return 0;
 565         } else if ( ds > 0.0f ) {
 566                 r = idMath::Sqrt( ds );
 567                 t1 = 0.75f * b * b - r * r - 2.0f * c;
 568                 t2 = ( 4.0f * b * c - 8.0f * d - b * b * b ) / ( 4.0f * r );
 569                 tp = t1 + t2;
 570                 tm = t1 - t2;
 571
 572                 if ( tp >= 0.0f ) {
 573                         s1 = idMath::Sqrt( tp );
 574                         roots[count++] = -0.25f * b + 0.5f * ( r + s1 );
 575                         roots[count++] = -0.25f * b + 0.5f * ( r - s1 );
 576                 }
 577                 if ( tm >= 0.0f ) {
 578                         s2 = idMath::Sqrt( tm );
 579                         roots[count++] = -0.25f * b + 0.5f * ( s2 - r );
 580                         roots[count++] = -0.25f * b - 0.5f * ( s2 + r );
 581                 }
 582                 return count;
 583         } else {
 584                 t2 = y * y - 4.0f * e;
 585                 if ( t2 >= 0.0f ) {
 586                         t2 = 2.0f * idMath::Sqrt( t2 );
 587                         t1 = 0.75f * b * b - 2.0f * c;
 588                         if ( t1 + t2 >= 0.0f ) {
 589                                 s1 = idMath::Sqrt( t1 + t2 );
 590                                 roots[count++] = -0.25f * b + 0.5f * s1;
 591                                 roots[count++] = -0.25f * b - 0.5f * s1;
 592                         }
 593                         if ( t1 - t2 >= 0.0f ) {
 594                                 s2 = idMath::Sqrt( t1 - t2 );
 595                                 roots[count++] = -0.25f * b + 0.5f * s2;
 596                                 roots[count++] = -0.25f * b - 0.5f * s2;
 597                         }
 598                 }
 599                 return count;
 600         }
 601 }
 602
 603 ID_INLINE const float *idPolynomial::ToFloatPtr( void ) const {
 604         return coefficient;
 605 }
 606
 607 ID_INLINE float *idPolynomial::ToFloatPtr( void ) {
 608         return coefficient;
 609 }
 610
 611 ID_INLINE void idPolynomial::Resize( int d, bool keep ) {
 612         int alloc = ( d + 1 + 3 ) & ~3;
 613         if ( alloc > allocated ) {
 614                 float *ptr = (float *) Mem_Alloc16( alloc * sizeof( float ) );
 615                 if ( coefficient != NULL ) {
 616                         if ( keep ) {
 617                                 for ( int i = 0; i <= degree; i++ ) {
 618                                         ptr[i] = coefficient[i];
 619                                 }
 620                         }
 621                         Mem_Free16( coefficient );
 622                 }
 623                 allocated = alloc;
 624                 coefficient = ptr;
 625         }
 626         degree = d;
 627 }
 628
 629 #endif /* !__MATH_POLYNOMIAL_H__ */