aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs147
1 files changed, 129 insertions, 18 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 817a5f7..91255bd 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// ============================================================================
249public class BSFMotor : BSMotor 251public 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; }
259
260 public virtual float TargetValue { get; protected set; }
261 public virtual float CurrentValue { get; protected set; }
262 public virtual float LastError { get; protected set; }
255 263
256 public float Target { get; private set; } 264 public virtual bool ErrorIsZero()
257 public float CurrentValue { get; private set; } 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;
289 }
290 public override void Zero()
291 {
292 base.Zero();
293 CurrentValue = TargetValue = 0f;
268 } 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.
277public class BSPIDVMotor : BSVMotor 384public class BSPIDVMotor : BSVMotor
@@ -319,6 +426,7 @@ public class BSPIDVMotor : BSVMotor
319 proportionFactor = new Vector3(factor, factor, factor); 426 proportionFactor = new Vector3(factor, factor, factor);
320 integralFactor = new Vector3(factor, factor, factor); 427 integralFactor = new Vector3(factor, factor, factor);
321 derivFactor = new Vector3(factor, factor, factor); 428 derivFactor = new Vector3(factor, factor, factor);
429 MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
322 } 430 }
323 } 431 }
324 432
@@ -341,6 +449,9 @@ public class BSPIDVMotor : BSVMotor
341 + derivFactor * derivFactor 449 + derivFactor * derivFactor
342 ); 450 );
343 451
452 MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},derivFact={4},ret={5}",
453 BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivFactor, ret);
454
344 return ret; 455 return ret;
345 } 456 }
346} 457}