mirror of
https://github.com/dhewm/dhewm3.git
synced 2024-12-11 21:40:49 +00:00
736ec20d4d
Don't include the lazy precompiled.h everywhere, only what's required for the compilation unit. platform.h needs to be included instead to provide all essential defines and types. All includes use the relative path to the neo or the game specific root. Move all idlib related includes from idlib/Lib.h to precompiled.h. precompiled.h still exists for the MFC stuff in tools/. Add some missing header guards.
356 lines
8.8 KiB
C++
356 lines
8.8 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
Doom 3 GPL Source Code
|
|
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
|
|
|
|
This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
|
|
|
|
Doom 3 Source Code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Doom 3 Source Code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
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.
|
|
|
|
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.
|
|
|
|
===========================================================================
|
|
*/
|
|
|
|
#include "sys/platform.h"
|
|
#include "idlib/math/Math.h"
|
|
|
|
#include "idlib/math/Ode.h"
|
|
|
|
//===============================================================
|
|
//
|
|
// idODE_Euler
|
|
//
|
|
//===============================================================
|
|
|
|
/*
|
|
=============
|
|
idODE_Euler::idODE_Euler
|
|
=============
|
|
*/
|
|
idODE_Euler::idODE_Euler( const int dim, deriveFunction_t dr, const void *ud ) {
|
|
dimension = dim;
|
|
derivatives = new float[dim];
|
|
derive = dr;
|
|
userData = ud;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_Euler::~idODE_Euler
|
|
=============
|
|
*/
|
|
idODE_Euler::~idODE_Euler( void ) {
|
|
delete[] derivatives;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_Euler::Evaluate
|
|
=============
|
|
*/
|
|
float idODE_Euler::Evaluate( const float *state, float *newState, float t0, float t1 ) {
|
|
float delta;
|
|
int i;
|
|
|
|
derive( t0, userData, state, derivatives );
|
|
delta = t1 - t0;
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
newState[i] = state[i] + delta * derivatives[i];
|
|
}
|
|
return delta;
|
|
}
|
|
|
|
//===============================================================
|
|
//
|
|
// idODE_Midpoint
|
|
//
|
|
//===============================================================
|
|
|
|
/*
|
|
=============
|
|
idODE_Midpoint::idODE_Midpoint
|
|
=============
|
|
*/
|
|
idODE_Midpoint::idODE_Midpoint( const int dim, deriveFunction_t dr, const void *ud ) {
|
|
dimension = dim;
|
|
tmpState = new float[dim];
|
|
derivatives = new float[dim];
|
|
derive = dr;
|
|
userData = ud;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_Midpoint::~idODE_Midpoint
|
|
=============
|
|
*/
|
|
idODE_Midpoint::~idODE_Midpoint( void ) {
|
|
delete tmpState;
|
|
delete derivatives;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_Midpoint::~Evaluate
|
|
=============
|
|
*/
|
|
float idODE_Midpoint::Evaluate( const float *state, float *newState, float t0, float t1 ) {
|
|
double delta, halfDelta;
|
|
int i;
|
|
|
|
delta = t1 - t0;
|
|
halfDelta = delta * 0.5;
|
|
// first step
|
|
derive( t0, userData, state, derivatives );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * derivatives[i];
|
|
}
|
|
// second step
|
|
derive( t0 + halfDelta, userData, tmpState, derivatives );
|
|
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
newState[i] = state[i] + delta * derivatives[i];
|
|
}
|
|
return delta;
|
|
}
|
|
|
|
//===============================================================
|
|
//
|
|
// idODE_RK4
|
|
//
|
|
//===============================================================
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4::idODE_RK4
|
|
=============
|
|
*/
|
|
idODE_RK4::idODE_RK4( const int dim, deriveFunction_t dr, const void *ud ) {
|
|
dimension = dim;
|
|
derive = dr;
|
|
userData = ud;
|
|
tmpState = new float[dim];
|
|
d1 = new float[dim];
|
|
d2 = new float[dim];
|
|
d3 = new float[dim];
|
|
d4 = new float[dim];
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4::~idODE_RK4
|
|
=============
|
|
*/
|
|
idODE_RK4::~idODE_RK4( void ) {
|
|
delete tmpState;
|
|
delete d1;
|
|
delete d2;
|
|
delete d3;
|
|
delete d4;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4::Evaluate
|
|
=============
|
|
*/
|
|
float idODE_RK4::Evaluate( const float *state, float *newState, float t0, float t1 ) {
|
|
double delta, halfDelta, sixthDelta;
|
|
int i;
|
|
|
|
delta = t1 - t0;
|
|
halfDelta = delta * 0.5;
|
|
// first step
|
|
derive( t0, userData, state, d1 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d1[i];
|
|
}
|
|
// second step
|
|
derive( t0 + halfDelta, userData, tmpState, d2 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d2[i];
|
|
}
|
|
// third step
|
|
derive( t0 + halfDelta, userData, tmpState, d3 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + delta * d3[i];
|
|
}
|
|
// fourth step
|
|
derive( t0 + delta, userData, tmpState, d4 );
|
|
|
|
sixthDelta = delta * (1.0/6.0);
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
|
|
}
|
|
return delta;
|
|
}
|
|
|
|
//===============================================================
|
|
//
|
|
// idODE_RK4Adaptive
|
|
//
|
|
//===============================================================
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4Adaptive::idODE_RK4Adaptive
|
|
=============
|
|
*/
|
|
idODE_RK4Adaptive::idODE_RK4Adaptive( const int dim, deriveFunction_t dr, const void *ud ) {
|
|
dimension = dim;
|
|
derive = dr;
|
|
userData = ud;
|
|
maxError = 0.01f;
|
|
tmpState = new float[dim];
|
|
d1 = new float[dim];
|
|
d1half = new float [dim];
|
|
d2 = new float[dim];
|
|
d3 = new float[dim];
|
|
d4 = new float[dim];
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4Adaptive::~idODE_RK4Adaptive
|
|
=============
|
|
*/
|
|
idODE_RK4Adaptive::~idODE_RK4Adaptive( void ) {
|
|
delete tmpState;
|
|
delete d1;
|
|
delete d1half;
|
|
delete d2;
|
|
delete d3;
|
|
delete d4;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4Adaptive::SetMaxError
|
|
=============
|
|
*/
|
|
void idODE_RK4Adaptive::SetMaxError( const float err ) {
|
|
if ( err > 0.0f ) {
|
|
maxError = err;
|
|
}
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idODE_RK4Adaptive::Evaluate
|
|
=============
|
|
*/
|
|
float idODE_RK4Adaptive::Evaluate( const float *state, float *newState, float t0, float t1 ) {
|
|
double delta, halfDelta, fourthDelta, sixthDelta;
|
|
double error, max;
|
|
int i, n;
|
|
|
|
delta = t1 - t0;
|
|
|
|
for ( n = 0; n < 4; n++ ) {
|
|
|
|
halfDelta = delta * 0.5;
|
|
fourthDelta = delta * 0.25;
|
|
|
|
// first step of first half delta
|
|
derive( t0, userData, state, d1 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + fourthDelta * d1[i];
|
|
}
|
|
// second step of first half delta
|
|
derive( t0 + fourthDelta, userData, tmpState, d2 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + fourthDelta * d2[i];
|
|
}
|
|
// third step of first half delta
|
|
derive( t0 + fourthDelta, userData, tmpState, d3 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d3[i];
|
|
}
|
|
// fourth step of first half delta
|
|
derive( t0 + halfDelta, userData, tmpState, d4 );
|
|
|
|
sixthDelta = halfDelta * (1.0/6.0);
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
|
|
}
|
|
|
|
// first step of second half delta
|
|
derive( t0 + halfDelta, userData, tmpState, d1half );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + fourthDelta * d1half[i];
|
|
}
|
|
// second step of second half delta
|
|
derive( t0 + halfDelta + fourthDelta, userData, tmpState, d2 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + fourthDelta * d2[i];
|
|
}
|
|
// third step of second half delta
|
|
derive( t0 + halfDelta + fourthDelta, userData, tmpState, d3 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d3[i];
|
|
}
|
|
// fourth step of second half delta
|
|
derive( t0 + delta, userData, tmpState, d4 );
|
|
|
|
sixthDelta = halfDelta * (1.0/6.0);
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
newState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
|
|
}
|
|
|
|
// first step of full delta
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d1[i];
|
|
}
|
|
// second step of full delta
|
|
derive( t0 + halfDelta, userData, tmpState, d2 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + halfDelta * d2[i];
|
|
}
|
|
// third step of full delta
|
|
derive( t0 + halfDelta, userData, tmpState, d3 );
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + delta * d3[i];
|
|
}
|
|
// fourth step of full delta
|
|
derive( t0 + delta, userData, tmpState, d4 );
|
|
|
|
sixthDelta = delta * (1.0/6.0);
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
tmpState[i] = state[i] + sixthDelta * (d1[i] + 2.0 * (d2[i] + d3[i]) + d4[i]);
|
|
}
|
|
|
|
// get max estimated error
|
|
max = 0.0;
|
|
for ( i = 0; i < dimension; i++ ) {
|
|
error = idMath::Fabs( (newState[i] - tmpState[i]) / (delta * d1[i] + 1e-10) );
|
|
if ( error > max ) {
|
|
max = error;
|
|
}
|
|
}
|
|
error = max / maxError;
|
|
|
|
if ( error <= 1.0f ) {
|
|
return delta * 4.0;
|
|
}
|
|
if ( delta <= 1e-7 ) {
|
|
return delta;
|
|
}
|
|
delta *= 0.25;
|
|
}
|
|
return delta;
|
|
}
|