home *** CD-ROM | disk | FTP | other *** search
/ GameStar 2006 March / Gamestar_82_2006-03_dvd.iso / DVDStar / Editace / quake4_sdkv10.exe / source / idlib / math / Ode.cpp < prev    next >
C/C++ Source or Header  |  2005-11-14  |  7KB  |  329 lines
  1.  
  2. #include "../precompiled.h"
  3. #pragma hdrstop
  4.  
  5. //===============================================================
  6. //
  7. //    idODE_Euler
  8. //
  9. //===============================================================
  10.  
  11. /*
  12. =============
  13. idODE_Euler::idODE_Euler
  14. =============
  15. */
  16. idODE_Euler::idODE_Euler( const int dim, deriveFunction_t dr, const void *ud ) {
  17.     dimension = dim;
  18.     derivatives = new float[dim];
  19.     derive = dr;
  20.     userData = ud;
  21. }
  22.  
  23. /*
  24. =============
  25. idODE_Euler::~idODE_Euler
  26. =============
  27. */
  28. idODE_Euler::~idODE_Euler( void ) {
  29.     delete[] derivatives;
  30. }
  31.  
  32. /*
  33. =============
  34. idODE_Euler::Evaluate
  35. =============
  36. */
  37. float idODE_Euler::Evaluate( const float *state, float *newState, float t0, float t1 ) {
  38.     float delta;
  39.     int i;
  40.  
  41.     derive( t0, userData, state, derivatives );
  42.     delta = t1 - t0;
  43.     for ( i = 0; i < dimension; i++ ) {
  44.         newState[i] = state[i] + delta * derivatives[i];
  45.     }
  46.     return delta;
  47. }
  48.  
  49. //===============================================================
  50. //
  51. //    idODE_Midpoint
  52. //
  53. //===============================================================
  54.  
  55. /*
  56. =============
  57. idODE_Midpoint::idODE_Midpoint
  58. =============
  59. */
  60. idODE_Midpoint::idODE_Midpoint( const int dim, deriveFunction_t dr, const void *ud ) {
  61.     dimension = dim;
  62.     tmpState = new float[dim];
  63.     derivatives = new float[dim];
  64.     derive = dr;
  65.     userData = ud;
  66. }
  67.  
  68. /*
  69. =============
  70. idODE_Midpoint::~idODE_Midpoint
  71. =============
  72. */
  73. idODE_Midpoint::~idODE_Midpoint( void ) {
  74.     delete tmpState;
  75.     delete derivatives;
  76. }
  77.  
  78. /*
  79. =============
  80. idODE_Midpoint::~Evaluate
  81. =============
  82. */
  83. float idODE_Midpoint::Evaluate( const float *state, float *newState, float t0, float t1 ) {
  84.     double delta, halfDelta;
  85.     int i;
  86.  
  87.     delta = t1 - t0;
  88.     halfDelta = delta * 0.5;
  89.     // first step
  90.     derive( t0, userData, state, derivatives );
  91.     for ( i = 0; i < dimension; i++ ) {
  92.         tmpState[i] = state[i] + halfDelta * derivatives[i];
  93.     }
  94.     // second step
  95.     derive( t0 + halfDelta, userData, tmpState, derivatives );
  96.  
  97.     for ( i = 0; i < dimension; i++ ) {
  98.         newState[i] = state[i] + delta * derivatives[i];
  99.     }
  100.     return delta;
  101. }
  102.  
  103. //===============================================================
  104. //
  105. //    idODE_RK4
  106. //
  107. //===============================================================
  108.  
  109. /*
  110. =============
  111. idODE_RK4::idODE_RK4
  112. =============
  113. */
  114. idODE_RK4::idODE_RK4( const int dim, deriveFunction_t dr, const void *ud ) {
  115.     dimension = dim;
  116.     derive = dr;
  117.     userData = ud;
  118.     tmpState = new float[dim];
  119.     d1 = new float[dim];
  120.     d2 = new float[dim];
  121.     d3 = new float[dim];
  122.     d4 = new float[dim];
  123. }
  124.  
  125. /*
  126. =============
  127. idODE_RK4::~idODE_RK4
  128. =============
  129. */
  130. idODE_RK4::~idODE_RK4( void ) {
  131.     delete tmpState;
  132.     delete d1;
  133.     delete d2;
  134.     delete d3;
  135.     delete d4;
  136. }
  137.  
  138. /*
  139. =============
  140. idODE_RK4::Evaluate
  141. =============
  142. */
  143. float idODE_RK4::Evaluate( const float *state, float *newState, float t0, float t1 ) {
  144.     double delta, halfDelta, sixthDelta;
  145.     int i;
  146.  
  147.     delta = t1 - t0;
  148.     halfDelta = delta * 0.5;
  149.     // first step
  150.     derive( t0, userData, state, d1 );
  151.     for ( i = 0; i < dimension; i++ ) {
  152.         tmpState[i] = state[i] + halfDelta * d1[i];
  153.     }
  154.     // second step
  155.     derive( t0 + halfDelta, userData, tmpState, d2 );
  156.     for ( i = 0; i < dimension; i++ ) {
  157.         tmpState[i] = state[i] + halfDelta * d2[i];
  158.     }
  159.     // third step
  160.     derive( t0 + halfDelta, userData, tmpState, d3 );
  161.     for ( i = 0; i < dimension; i++ ) {
  162.         tmpState[i] = state[i] + delta * d3[i];
  163.     }
  164.     // fourth step
  165.     derive( t0 + delta, userData, tmpState, d4 );
  166.  
  167.     sixthDelta = delta * (1.0/6.0);
  168.     for ( i = 0; i < dimension; i++ ) {
  169.         newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
  170.     }
  171.     return delta;
  172. }
  173.  
  174. //===============================================================
  175. //
  176. //    idODE_RK4Adaptive
  177. //
  178. //===============================================================
  179.  
  180. /*
  181. =============
  182. idODE_RK4Adaptive::idODE_RK4Adaptive
  183. =============
  184. */
  185. idODE_RK4Adaptive::idODE_RK4Adaptive( const int dim, deriveFunction_t dr, const void *ud ) {
  186.     dimension = dim;
  187.     derive = dr;
  188.     userData = ud;
  189.     maxError = 0.01f;
  190.     tmpState = new float[dim];
  191.     d1 = new float[dim];
  192.     d1half = new float [dim];
  193.     d2 = new float[dim];
  194.     d3 = new float[dim];
  195.     d4 = new float[dim];
  196. }
  197.  
  198. /*
  199. =============
  200. idODE_RK4Adaptive::~idODE_RK4Adaptive
  201. =============
  202. */
  203. idODE_RK4Adaptive::~idODE_RK4Adaptive( void ) {
  204.     delete tmpState;
  205.     delete d1;
  206.     delete d1half;
  207.     delete d2;
  208.     delete d3;
  209.     delete d4;
  210. }
  211.  
  212. /*
  213. =============
  214. idODE_RK4Adaptive::SetMaxError
  215. =============
  216. */
  217. void idODE_RK4Adaptive::SetMaxError( const float err ) {
  218.     if ( err > 0.0f ) {
  219.         maxError = err;
  220.     }
  221. }
  222.  
  223. /*
  224. =============
  225. idODE_RK4Adaptive::Evaluate
  226. =============
  227. */
  228. float idODE_RK4Adaptive::Evaluate( const float *state, float *newState, float t0, float t1 ) {
  229.     double delta, halfDelta, fourthDelta, sixthDelta;
  230.     double error, max;
  231.     int i, n;
  232.  
  233.     delta = t1 - t0;
  234.  
  235.     for ( n = 0; n < 4; n++ ) {
  236.  
  237.         halfDelta = delta * 0.5;
  238.         fourthDelta = delta * 0.25;
  239.  
  240.         // first step of first half delta
  241.         derive( t0, userData, state, d1 );
  242.         for ( i = 0; i < dimension; i++ ) {
  243.             tmpState[i] = state[i] + fourthDelta * d1[i];
  244.         }
  245.         // second step of first half delta
  246.         derive( t0 + fourthDelta, userData, tmpState, d2 );
  247.         for ( i = 0; i < dimension; i++ ) {
  248.             tmpState[i] = state[i] + fourthDelta * d2[i];
  249.         }
  250.         // third step of first half delta
  251.         derive( t0 + fourthDelta, userData, tmpState, d3 );
  252.         for ( i = 0; i < dimension; i++ ) {
  253.             tmpState[i] = state[i] + halfDelta * d3[i];
  254.         }
  255.         // fourth step of first half delta
  256.         derive( t0 + halfDelta, userData, tmpState, d4 );
  257.  
  258.         sixthDelta = halfDelta * (1.0/6.0);
  259.         for ( i = 0; i < dimension; i++ ) {
  260.             tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
  261.         }
  262.  
  263.         // first step of second half delta
  264.         derive( t0 + halfDelta, userData, tmpState, d1half );
  265.         for ( i = 0; i < dimension; i++ ) {
  266.             tmpState[i] = state[i] + fourthDelta * d1half[i];
  267.         }
  268.         // second step of second half delta
  269.         derive( t0 + halfDelta + fourthDelta, userData, tmpState, d2 );
  270.         for ( i = 0; i < dimension; i++ ) {
  271.             tmpState[i] = state[i] + fourthDelta * d2[i];
  272.         }
  273.         // third step of second half delta
  274.         derive( t0 + halfDelta + fourthDelta, userData, tmpState, d3 );
  275.         for ( i = 0; i < dimension; i++ ) {
  276.             tmpState[i] = state[i] + halfDelta * d3[i];
  277.         }
  278.         // fourth step of second half delta
  279.         derive( t0 + delta, userData, tmpState, d4 );
  280.  
  281.         sixthDelta = halfDelta * (1.0/6.0);
  282.         for ( i = 0; i < dimension; i++ ) {
  283.             newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
  284.         }
  285.  
  286.         // first step of full delta
  287.         for ( i = 0; i < dimension; i++ ) {
  288.             tmpState[i] = state[i] + halfDelta * d1[i];
  289.         }
  290.         // second step of full delta
  291.         derive( t0 + halfDelta, userData, tmpState, d2 );
  292.         for ( i = 0; i < dimension; i++ ) {
  293.             tmpState[i] = state[i] + halfDelta * d2[i];
  294.         }
  295.         // third step of full delta
  296.         derive( t0 + halfDelta, userData, tmpState, d3 );
  297.         for ( i = 0; i < dimension; i++ ) {
  298.             tmpState[i] = state[i] + delta * d3[i];
  299.         }
  300.         // fourth step of full delta
  301.         derive( t0 + delta, userData, tmpState, d4 );
  302.  
  303.         sixthDelta = delta * (1.0/6.0);
  304.         for ( i = 0; i < dimension; i++ ) {
  305.             tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
  306.         }
  307.  
  308.         // get max estimated error
  309.         max = 0.0;
  310.         for ( i = 0; i < dimension; i++ ) {
  311.             error = idMath::Fabs( (newState[i] - tmpState[i]) / (delta * d1[i] + 1e-10) );
  312.             if ( error > max ) {
  313.                 max = error;
  314.             }
  315.         }
  316.         error = max / maxError;
  317.  
  318.         if ( error <= 1.0f ) {
  319.             return delta * 4.0;
  320.         }
  321.         if ( delta <= 1e-7 ) {
  322.             return delta;
  323.         }
  324.         delta *= 0.25;
  325.     }
  326.     return delta;
  327. }
  328.  
  329.