diff options
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs')
-rwxr-xr-x | OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 170 |
1 files changed, 145 insertions, 25 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index 817a5f7..6d0db2e 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | |||
@@ -59,10 +59,7 @@ public abstract class BSMotor | |||
59 | { | 59 | { |
60 | if (PhysicsScene != null) | 60 | if (PhysicsScene != null) |
61 | { | 61 | { |
62 | if (PhysicsScene.VehicleLoggingEnabled) | 62 | PhysicsScene.DetailLog(msg, parms); |
63 | { | ||
64 | PhysicsScene.DetailLog(msg, parms); | ||
65 | } | ||
66 | } | 63 | } |
67 | } | 64 | } |
68 | } | 65 | } |
@@ -100,10 +97,13 @@ public class BSVMotor : BSMotor | |||
100 | public virtual Vector3 CurrentValue { get; protected set; } | 97 | public virtual Vector3 CurrentValue { get; protected set; } |
101 | public virtual Vector3 LastError { get; protected set; } | 98 | public virtual Vector3 LastError { get; protected set; } |
102 | 99 | ||
103 | public virtual bool ErrorIsZero | 100 | public virtual bool ErrorIsZero() |
104 | { get { | 101 | { |
105 | return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold); | 102 | return ErrorIsZero(LastError); |
106 | } | 103 | } |
104 | public virtual bool ErrorIsZero(Vector3 err) | ||
105 | { | ||
106 | return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)); | ||
107 | } | 107 | } |
108 | 108 | ||
109 | public BSVMotor(string useName) | 109 | public BSVMotor(string useName) |
@@ -148,7 +148,7 @@ public class BSVMotor : BSMotor | |||
148 | 148 | ||
149 | Vector3 correction = Vector3.Zero; | 149 | Vector3 correction = Vector3.Zero; |
150 | Vector3 error = TargetValue - CurrentValue; | 150 | Vector3 error = TargetValue - CurrentValue; |
151 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | 151 | if (!ErrorIsZero(error)) |
152 | { | 152 | { |
153 | correction = Step(timeStep, error); | 153 | correction = Step(timeStep, error); |
154 | 154 | ||
@@ -200,7 +200,7 @@ public class BSVMotor : BSMotor | |||
200 | 200 | ||
201 | LastError = error; | 201 | LastError = error; |
202 | Vector3 returnCorrection = Vector3.Zero; | 202 | Vector3 returnCorrection = Vector3.Zero; |
203 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | 203 | if (!ErrorIsZero()) |
204 | { | 204 | { |
205 | // correction = error / secondsItShouldTakeToCorrect | 205 | // correction = error / secondsItShouldTakeToCorrect |
206 | Vector3 correctionAmount; | 206 | Vector3 correctionAmount; |
@@ -246,32 +246,139 @@ public class BSVMotor : BSMotor | |||
246 | } | 246 | } |
247 | } | 247 | } |
248 | 248 | ||
249 | // ============================================================================ | ||
250 | // ============================================================================ | ||
249 | public class BSFMotor : BSMotor | 251 | public class BSFMotor : BSMotor |
250 | { | 252 | { |
251 | public float TimeScale { get; set; } | 253 | public virtual float TimeScale { get; set; } |
252 | public float DecayTimeScale { get; set; } | 254 | public virtual float TargetValueDecayTimeScale { get; set; } |
253 | public float Friction { get; set; } | 255 | public virtual float FrictionTimescale { get; set; } |
254 | public float Efficiency { get; set; } | 256 | public virtual float Efficiency { get; set; } |
257 | |||
258 | public virtual float ErrorZeroThreshold { get; set; } | ||
255 | 259 | ||
256 | public float Target { get; private set; } | 260 | public virtual float TargetValue { get; protected set; } |
257 | public float CurrentValue { get; private set; } | 261 | public virtual float CurrentValue { get; protected set; } |
262 | public virtual float LastError { get; protected set; } | ||
263 | |||
264 | public virtual bool ErrorIsZero() | ||
265 | { | ||
266 | return ErrorIsZero(LastError); | ||
267 | } | ||
268 | public virtual bool ErrorIsZero(float err) | ||
269 | { | ||
270 | return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold); | ||
271 | } | ||
258 | 272 | ||
259 | public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) | 273 | public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) |
260 | : base(useName) | 274 | : base(useName) |
261 | { | 275 | { |
276 | TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; | ||
277 | Efficiency = 1f; | ||
278 | FrictionTimescale = BSMotor.Infinite; | ||
279 | CurrentValue = TargetValue = 0f; | ||
280 | ErrorZeroThreshold = 0.01f; | ||
262 | } | 281 | } |
263 | public void SetCurrent(float target) | 282 | public void SetCurrent(float current) |
264 | { | 283 | { |
284 | CurrentValue = current; | ||
265 | } | 285 | } |
266 | public void SetTarget(float target) | 286 | public void SetTarget(float target) |
267 | { | 287 | { |
288 | TargetValue = target; | ||
268 | } | 289 | } |
290 | public override void Zero() | ||
291 | { | ||
292 | base.Zero(); | ||
293 | CurrentValue = TargetValue = 0f; | ||
294 | } | ||
295 | |||
269 | public virtual float Step(float timeStep) | 296 | public virtual float Step(float timeStep) |
270 | { | 297 | { |
271 | return 0f; | 298 | if (!Enabled) return TargetValue; |
299 | |||
300 | float origTarget = TargetValue; // DEBUG | ||
301 | float origCurrVal = CurrentValue; // DEBUG | ||
302 | |||
303 | float correction = 0f; | ||
304 | float error = TargetValue - CurrentValue; | ||
305 | if (!ErrorIsZero(error)) | ||
306 | { | ||
307 | correction = Step(timeStep, error); | ||
308 | |||
309 | CurrentValue += correction; | ||
310 | |||
311 | // The desired value reduces to zero which also reduces the difference with current. | ||
312 | // If the decay time is infinite, don't decay at all. | ||
313 | float decayFactor = 0f; | ||
314 | if (TargetValueDecayTimeScale != BSMotor.Infinite) | ||
315 | { | ||
316 | decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep; | ||
317 | TargetValue *= (1f - decayFactor); | ||
318 | } | ||
319 | |||
320 | // The amount we can correct the error is reduced by the friction | ||
321 | float frictionFactor = 0f; | ||
322 | if (FrictionTimescale != BSMotor.Infinite) | ||
323 | { | ||
324 | // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; | ||
325 | // Individual friction components can be 'infinite' so compute each separately. | ||
326 | frictionFactor = 1f / FrictionTimescale; | ||
327 | frictionFactor *= timeStep; | ||
328 | CurrentValue *= (1f - frictionFactor); | ||
329 | } | ||
330 | |||
331 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", | ||
332 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, | ||
333 | timeStep, error, correction); | ||
334 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", | ||
335 | BSScene.DetailLogZero, UseName, | ||
336 | TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor, | ||
337 | TargetValue, CurrentValue); | ||
338 | } | ||
339 | else | ||
340 | { | ||
341 | // Difference between what we have and target is small. Motor is done. | ||
342 | CurrentValue = TargetValue; | ||
343 | MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", | ||
344 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); | ||
345 | } | ||
346 | |||
347 | return CurrentValue; | ||
348 | } | ||
349 | |||
350 | public virtual float Step(float timeStep, float error) | ||
351 | { | ||
352 | if (!Enabled) return 0f; | ||
353 | |||
354 | LastError = error; | ||
355 | float returnCorrection = 0f; | ||
356 | if (!ErrorIsZero()) | ||
357 | { | ||
358 | // correction = error / secondsItShouldTakeToCorrect | ||
359 | float correctionAmount; | ||
360 | if (TimeScale == 0f || TimeScale == BSMotor.Infinite) | ||
361 | correctionAmount = error * timeStep; | ||
362 | else | ||
363 | correctionAmount = error / TimeScale * timeStep; | ||
364 | |||
365 | returnCorrection = correctionAmount; | ||
366 | MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}", | ||
367 | BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount); | ||
368 | } | ||
369 | return returnCorrection; | ||
370 | } | ||
371 | |||
372 | public override string ToString() | ||
373 | { | ||
374 | return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", | ||
375 | UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); | ||
272 | } | 376 | } |
377 | |||
273 | } | 378 | } |
274 | 379 | ||
380 | // ============================================================================ | ||
381 | // ============================================================================ | ||
275 | // Proportional, Integral, Derivitive Motor | 382 | // Proportional, Integral, Derivitive Motor |
276 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. | 383 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. |
277 | public class BSPIDVMotor : BSVMotor | 384 | public class BSPIDVMotor : BSVMotor |
@@ -281,6 +388,12 @@ public class BSPIDVMotor : BSVMotor | |||
281 | public Vector3 integralFactor { get; set; } | 388 | public Vector3 integralFactor { get; set; } |
282 | public Vector3 derivFactor { get; set; } | 389 | public Vector3 derivFactor { get; set; } |
283 | 390 | ||
391 | // The factors are vectors for the three dimensions. This is the proportional of each | ||
392 | // that is applied. This could be multiplied through the actual factors but it | ||
393 | // is sometimes easier to manipulate the factors and their mix separately. | ||
394 | // to | ||
395 | public Vector3 FactorMix; | ||
396 | |||
284 | // Arbritrary factor range. | 397 | // Arbritrary factor range. |
285 | // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct. | 398 | // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct. |
286 | public float EfficiencyHigh = 0.4f; | 399 | public float EfficiencyHigh = 0.4f; |
@@ -295,6 +408,7 @@ public class BSPIDVMotor : BSVMotor | |||
295 | proportionFactor = new Vector3(1.00f, 1.00f, 1.00f); | 408 | proportionFactor = new Vector3(1.00f, 1.00f, 1.00f); |
296 | integralFactor = new Vector3(1.00f, 1.00f, 1.00f); | 409 | integralFactor = new Vector3(1.00f, 1.00f, 1.00f); |
297 | derivFactor = new Vector3(1.00f, 1.00f, 1.00f); | 410 | derivFactor = new Vector3(1.00f, 1.00f, 1.00f); |
411 | FactorMix = new Vector3(0.5f, 0.25f, 0.25f); | ||
298 | RunningIntegration = Vector3.Zero; | 412 | RunningIntegration = Vector3.Zero; |
299 | LastError = Vector3.Zero; | 413 | LastError = Vector3.Zero; |
300 | } | 414 | } |
@@ -310,15 +424,19 @@ public class BSPIDVMotor : BSVMotor | |||
310 | set | 424 | set |
311 | { | 425 | { |
312 | base.Efficiency = Util.Clamp(value, 0f, 1f); | 426 | base.Efficiency = Util.Clamp(value, 0f, 1f); |
427 | |||
313 | // Compute factors based on efficiency. | 428 | // Compute factors based on efficiency. |
314 | // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. | 429 | // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. |
315 | // If efficiency is low (0f), use a factor value that overcorrects. | 430 | // If efficiency is low (0f), use a factor value that overcorrects. |
316 | // TODO: might want to vary contribution of different factor depending on efficiency. | 431 | // TODO: might want to vary contribution of different factor depending on efficiency. |
317 | float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; | 432 | float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; |
318 | // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; | 433 | // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; |
434 | |||
319 | proportionFactor = new Vector3(factor, factor, factor); | 435 | proportionFactor = new Vector3(factor, factor, factor); |
320 | integralFactor = new Vector3(factor, factor, factor); | 436 | integralFactor = new Vector3(factor, factor, factor); |
321 | derivFactor = new Vector3(factor, factor, factor); | 437 | derivFactor = new Vector3(factor, factor, factor); |
438 | |||
439 | MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor); | ||
322 | } | 440 | } |
323 | } | 441 | } |
324 | 442 | ||
@@ -331,15 +449,17 @@ public class BSPIDVMotor : BSVMotor | |||
331 | RunningIntegration += error * timeStep; | 449 | RunningIntegration += error * timeStep; |
332 | 450 | ||
333 | // A simple derivitive is the rate of change from the last error. | 451 | // A simple derivitive is the rate of change from the last error. |
334 | Vector3 derivFactor = (error - LastError) * timeStep; | 452 | Vector3 derivitive = (error - LastError) * timeStep; |
335 | LastError = error; | 453 | LastError = error; |
336 | 454 | ||
337 | // Correction = -(proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) | 455 | // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) |
338 | Vector3 ret = -( | 456 | Vector3 ret = error * timeStep * proportionFactor * FactorMix.X |
339 | error * proportionFactor | 457 | + RunningIntegration * integralFactor * FactorMix.Y |
340 | + RunningIntegration * integralFactor | 458 | + derivitive * derivFactor * FactorMix.Z |
341 | + derivFactor * derivFactor | 459 | ; |
342 | ); | 460 | |
461 | MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", | ||
462 | BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); | ||
343 | 463 | ||
344 | return ret; | 464 | return ret; |
345 | } | 465 | } |