From 54321800277b683d76e80b12334900c82e3c6b50 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 3 Jan 2013 19:37:01 -0800 Subject: BulletSim: reorganize motor code a little to pull together common functions. Add BSFMotor. --- OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 147 ++++++++++++++++++++--- 1 file 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 { if (PhysicsScene != null) { - if (PhysicsScene.VehicleLoggingEnabled) - { - PhysicsScene.DetailLog(msg, parms); - } + PhysicsScene.DetailLog(msg, parms); } } } @@ -100,10 +97,13 @@ public class BSVMotor : BSMotor public virtual Vector3 CurrentValue { get; protected set; } public virtual Vector3 LastError { get; protected set; } - public virtual bool ErrorIsZero - { get { - return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold); - } + public virtual bool ErrorIsZero() + { + return ErrorIsZero(LastError); + } + public virtual bool ErrorIsZero(Vector3 err) + { + return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)); } public BSVMotor(string useName) @@ -148,7 +148,7 @@ public class BSVMotor : BSMotor Vector3 correction = Vector3.Zero; Vector3 error = TargetValue - CurrentValue; - if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) + if (!ErrorIsZero(error)) { correction = Step(timeStep, error); @@ -200,7 +200,7 @@ public class BSVMotor : BSMotor LastError = error; Vector3 returnCorrection = Vector3.Zero; - if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) + if (!ErrorIsZero()) { // correction = error / secondsItShouldTakeToCorrect Vector3 correctionAmount; @@ -246,32 +246,139 @@ public class BSVMotor : BSMotor } } +// ============================================================================ +// ============================================================================ public class BSFMotor : BSMotor { - public float TimeScale { get; set; } - public float DecayTimeScale { get; set; } - public float Friction { get; set; } - public float Efficiency { get; set; } + public virtual float TimeScale { get; set; } + public virtual float TargetValueDecayTimeScale { get; set; } + public virtual float FrictionTimescale { get; set; } + public virtual float Efficiency { get; set; } + + public virtual float ErrorZeroThreshold { get; set; } + + public virtual float TargetValue { get; protected set; } + public virtual float CurrentValue { get; protected set; } + public virtual float LastError { get; protected set; } - public float Target { get; private set; } - public float CurrentValue { get; private set; } + public virtual bool ErrorIsZero() + { + return ErrorIsZero(LastError); + } + public virtual bool ErrorIsZero(float err) + { + return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold); + } public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) : base(useName) { + TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; + Efficiency = 1f; + FrictionTimescale = BSMotor.Infinite; + CurrentValue = TargetValue = 0f; + ErrorZeroThreshold = 0.01f; } - public void SetCurrent(float target) + public void SetCurrent(float current) { + CurrentValue = current; } public void SetTarget(float target) { + TargetValue = target; + } + public override void Zero() + { + base.Zero(); + CurrentValue = TargetValue = 0f; } + public virtual float Step(float timeStep) { - return 0f; + if (!Enabled) return TargetValue; + + float origTarget = TargetValue; // DEBUG + float origCurrVal = CurrentValue; // DEBUG + + float correction = 0f; + float error = TargetValue - CurrentValue; + if (!ErrorIsZero(error)) + { + correction = Step(timeStep, error); + + CurrentValue += correction; + + // The desired value reduces to zero which also reduces the difference with current. + // If the decay time is infinite, don't decay at all. + float decayFactor = 0f; + if (TargetValueDecayTimeScale != BSMotor.Infinite) + { + decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep; + TargetValue *= (1f - decayFactor); + } + + // The amount we can correct the error is reduced by the friction + float frictionFactor = 0f; + if (FrictionTimescale != BSMotor.Infinite) + { + // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; + // Individual friction components can be 'infinite' so compute each separately. + frictionFactor = 1f / FrictionTimescale; + frictionFactor *= timeStep; + CurrentValue *= (1f - frictionFactor); + } + + MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", + BSScene.DetailLogZero, UseName, origCurrVal, origTarget, + timeStep, error, correction); + MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", + BSScene.DetailLogZero, UseName, + TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor, + TargetValue, CurrentValue); + } + else + { + // Difference between what we have and target is small. Motor is done. + CurrentValue = TargetValue; + MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", + BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); + } + + return CurrentValue; + } + + public virtual float Step(float timeStep, float error) + { + if (!Enabled) return 0f; + + LastError = error; + float returnCorrection = 0f; + if (!ErrorIsZero()) + { + // correction = error / secondsItShouldTakeToCorrect + float correctionAmount; + if (TimeScale == 0f || TimeScale == BSMotor.Infinite) + correctionAmount = error * timeStep; + else + correctionAmount = error / TimeScale * timeStep; + + returnCorrection = correctionAmount; + MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}", + BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount); + } + return returnCorrection; + } + + public override string ToString() + { + return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", + UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); } + } +// ============================================================================ +// ============================================================================ // Proportional, Integral, Derivitive Motor // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. public class BSPIDVMotor : BSVMotor @@ -319,6 +426,7 @@ public class BSPIDVMotor : BSVMotor proportionFactor = new Vector3(factor, factor, factor); integralFactor = new Vector3(factor, factor, factor); derivFactor = new Vector3(factor, factor, factor); + MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor); } } @@ -341,6 +449,9 @@ public class BSPIDVMotor : BSVMotor + derivFactor * derivFactor ); + MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},derivFact={4},ret={5}", + BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivFactor, ret); + return ret; } } -- cgit v1.1