neo/idlib/math/Ode.cpp

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