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