@@ -194,7 +194,7 @@ public void Reset()
194194 /// <param name="timeSpan">Integration step or timespan in seconds</param>
195195 /// <param name="value">Value to integrate</param>
196196 /// <returns>Value of integration in the next step (t + timeSpan)</returns>
197- public float Integrate ( float timeSpan , float value )
197+ public float Integrate ( float timeSpan , Func < float , float > value )
198198 {
199199 float step = 0.0f ;
200200 float end = timeSpan ;
@@ -207,43 +207,28 @@ public float Integrate(float timeSpan, float value)
207207 {
208208 return integralValue ;
209209 }
210-
211- //if (timeSpan > MinStep)
212210 if ( Math . Abs ( prevDerivation ) > Error )
213211 {
214- //count = 2 * Convert.ToInt32(Math.Round((timeSpan) / MinStep, 0));
215212 count = ++ numOfSubstepsPS ;
216213 if ( count > MaxSubsteps )
217214 count = MaxSubsteps ;
218215 waitBeforeSpeedingUp = 100 ;
219- //if (numOfSubstepsPS > (MaxSubsteps / 2))
220- // Method = IntegratorMethods.EulerBackMod;
221- //else
222- // Method = IntegratorMethods.RungeKutta4;
223216 }
224217 else
225218 {
226219 if ( -- waitBeforeSpeedingUp <= 0 ) //wait for a while before speeding up the integration
227220 {
228- count = -- numOfSubstepsPS ;
229- if ( count < 1 )
230- count = 1 ;
221+ count = Math . Max ( -- numOfSubstepsPS , 1 ) ;
231222
232223 waitBeforeSpeedingUp = 10 ; //not so fast ;)
233224 }
234225 else
235226 count = numOfSubstepsPS ;
236- //IsStepDividing = false;
237227 }
238228
239229 timeSpan = timeSpan / ( float ) count ;
240- IsStepDividing = true ;
241230 numOfSubstepsPS = count ;
242-
243- if ( count > 1 )
244- IsStepDividing = true ;
245- else
246- IsStepDividing = false ;
231+ IsStepDividing = count > 1 ;
247232
248233
249234 #region SOLVERS
@@ -253,28 +238,26 @@ public float Integrate(float timeSpan, float value)
253238 switch ( Method )
254239 {
255240 case IntegratorMethods . EulerBackward :
256- integralValue += ( derivation = timeSpan * value ) ;
241+ integralValue += ( derivation = timeSpan * value ( integralValue ) ) ;
257242 break ;
258243 case IntegratorMethods . EulerBackMod :
259- integralValue += ( derivation = timeSpan / 2.0f * ( previousValues [ 0 ] + value ) ) ;
260- previousValues [ 0 ] = value ;
244+ integralValue += ( derivation = timeSpan / 2.0f * ( previousValues [ 0 ] + value ( integralValue ) ) ) ;
245+ previousValues [ 0 ] = value ( integralValue ) ;
261246 break ;
262247 case IntegratorMethods . EulerForward :
263248 throw new NotImplementedException ( "Not implemented yet!" ) ;
264249
265250 case IntegratorMethods . RungeKutta2 :
266- //throw new NotImplementedException("Not implemented yet!");
267- k1 = integralValue + timeSpan / 2 * value ;
251+ k1 = integralValue + timeSpan / 2 * value ( integralValue ) ;
268252 k2 = 2 * ( k1 - integralValue ) / timeSpan ;
269253 integralValue += ( derivation = timeSpan * k2 ) ;
270254 break ;
271255 case IntegratorMethods . RungeKutta4 :
272- //throw new NotImplementedException("Not implemented yet!");
273- k1 = timeSpan * value ;
274- k2 = k1 + timeSpan / 2.0f * value ;
275- k3 = k1 + timeSpan / 2.0f * k2 ;
276- k4 = timeSpan * k3 ;
277- integralValue += ( derivation = ( k1 + 2.0f * k2 + 2.0f * k3 + k4 ) / 6.0f ) ;
256+ k1 = value ( integralValue ) ;
257+ k2 = value ( integralValue + k1 * timeSpan / 2.0f ) ;
258+ k3 = value ( integralValue + k2 * timeSpan / 2.0f ) ;
259+ k4 = value ( integralValue + k3 * timeSpan ) ;
260+ integralValue += ( derivation = ( k1 + 2.0f * k2 + 2.0f * k3 + k4 ) * timeSpan / 6.0f ) ;
278261 break ;
279262 case IntegratorMethods . NewtonRhapson :
280263 throw new NotImplementedException ( "Not implemented yet!" ) ;
@@ -289,7 +272,7 @@ public float Integrate(float timeSpan, float value)
289272 previousStep [ i ] = previousStep [ i - 1 ] ;
290273 previousValues [ i ] = previousValues [ i - 1 ] ;
291274 }
292- previousValues [ 0 ] = value ;
275+ previousValues [ 0 ] = value ( integralValue ) ;
293276 previousStep [ 0 ] = timeSpan ;
294277 break ;
295278 default :
@@ -313,24 +296,6 @@ public float Integrate(float timeSpan, float value)
313296 else
314297 return integralValue ;
315298 }
316- /// <summary>
317- /// Integrates given value in time. TimeSpan (integration step) is computed internally.
318- /// </summary>
319- /// <param name="clockSeconds">Time value in seconds</param>
320- /// <param name="value">Value to integrate</param>
321- /// <returns>Value of integration in elapsedClockSeconds time</returns>
322- public float TimeIntegrate ( float clockSeconds , float value )
323- {
324- float timeSpan = clockSeconds - oldTime ;
325- oldTime = clockSeconds ;
326- integralValue += timeSpan * value ;
327- if ( isLimited )
328- {
329- return ( integralValue <= min ) ? min : ( ( integralValue >= max ) ? max : integralValue ) ;
330- }
331- else
332- return integralValue ;
333- }
334299
335300 public void Save ( BinaryWriter outf )
336301 {
0 commit comments