Merge branch 'master' into careminster

1 files changed, 26 insertions, 6 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 34a87c6..9e1a9ba 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -43,7 +43,9 @@ public abstract class BSMotor
     {
         UseName = useName;
         PhysicsScene = null;
+        Enabled = true;
     }
+    public virtual bool Enabled { get; set; }
     public virtual void Reset() { }
     public virtual void Zero() { }
     public virtual void GenerateTestOutput(float timeStep) { }
@@ -98,6 +100,12 @@ 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 BSVMotor(string useName)
         : base(useName)
     {
@@ -105,7 +113,7 @@ public class BSVMotor : BSMotor
         Efficiency = 1f;
         FrictionTimescale = BSMotor.InfiniteVector;
         CurrentValue = TargetValue = Vector3.Zero;
-        ErrorZeroThreshold = 0.01f;
+        ErrorZeroThreshold = 0.001f;
     }
     public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
         : this(useName)
@@ -133,6 +141,8 @@ public class BSVMotor : BSMotor
     // Compute the next step and return the new current value
     public virtual Vector3 Step(float timeStep)
     {
+        if (!Enabled) return TargetValue;
+
         Vector3 origTarget = TargetValue;       // DEBUG
         Vector3 origCurrVal = CurrentValue;     // DEBUG
 
@@ -178,7 +188,7 @@ public class BSVMotor : BSMotor
         {
             // Difference between what we have and target is small. Motor is done.
             CurrentValue = TargetValue;
-            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}",
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
                         BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
         }
 
@@ -186,6 +196,8 @@ public class BSVMotor : BSMotor
     }
     public virtual Vector3 Step(float timeStep, Vector3 error)
     {
+        if (!Enabled) return Vector3.Zero;
+
         LastError = error;
         Vector3 returnCorrection = Vector3.Zero;
         if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
@@ -268,12 +280,14 @@ public class BSPIDVMotor : BSVMotor
     public Vector3 proportionFactor { get; set; }
     public Vector3 integralFactor { get; set; }
     public Vector3 derivFactor { get; set; }
+
     // Arbritrary factor range.
     // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
     public float EfficiencyHigh = 0.4f;
     public float EfficiencyLow = 4.0f;
 
-    Vector3 IntegralFactor { get; set; }
+    // Running integration of the error
+    Vector3 RunningIntegration { get; set; }
 
     public BSPIDVMotor(string useName)
         : base(useName)
@@ -281,7 +295,7 @@ public class BSPIDVMotor : BSVMotor
         proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
         integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
         derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
-        IntegralFactor = Vector3.Zero;
+        RunningIntegration = Vector3.Zero;
         LastError = Vector3.Zero;
     }
 
@@ -311,15 +325,21 @@ public class BSPIDVMotor : BSVMotor
     // Ignore Current and Target Values and just advance the PID computation on this error.
     public override Vector3 Step(float timeStep, Vector3 error)
     {
+        if (!Enabled) return Vector3.Zero;
+
         // Add up the error so we can integrate over the accumulated errors
-        IntegralFactor += error * timeStep;
+        RunningIntegration += error * timeStep;
 
         // A simple derivitive is the rate of change from the last error.
         Vector3 derivFactor = (error - LastError) * timeStep;
         LastError = error;
 
         // Correction = -(proportionOfPresentError +      accumulationOfPastError    +     rateOfChangeOfError)
-        Vector3 ret   = -(error * proportionFactor + IntegralFactor * integralFactor + derivFactor * derivFactor);
+        Vector3 ret   = -(
+                          error * proportionFactor
+                        + RunningIntegration * integralFactor 
+                        + derivFactor * derivFactor
+                        );
 
         return ret;
     }