1 files changed, 16 insertions, 9 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 6d0db2e..82fd2d2 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -138,7 +138,8 @@ public class BSVMotor : BSMotor
         CurrentValue = TargetValue = Vector3.Zero;
     }
 
-    // Compute the next step and return the new current value
+    // Compute the next step and return the new current value.
+    // Returns the correction needed to move 'current' to 'target'.
     public virtual Vector3 Step(float timeStep)
     {
         if (!Enabled) return TargetValue;
@@ -150,7 +151,7 @@ public class BSVMotor : BSMotor
         Vector3 error = TargetValue - CurrentValue;
         if (!ErrorIsZero(error))
         {
-            correction = Step(timeStep, error);
+            correction = StepError(timeStep, error);
 
             CurrentValue += correction;
 
@@ -187,14 +188,20 @@ public class BSVMotor : BSMotor
         else
         {
             // Difference between what we have and target is small. Motor is done.
-            CurrentValue = TargetValue;
+            CurrentValue = TargetValue = Vector3.Zero;
             MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
                         BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
         }
 
-        return CurrentValue;
+        return correction;
+    }
+    // version of step that sets the current value before doing the step
+    public virtual Vector3 Step(float timeStep, Vector3 current)
+    {
+        CurrentValue = current;
+        return Step(timeStep);
     }
-    public virtual Vector3 Step(float timeStep, Vector3 error)
+    public virtual Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
 
@@ -304,7 +311,7 @@ public class BSFMotor : BSMotor
         float error = TargetValue - CurrentValue;
         if (!ErrorIsZero(error))
         {
-            correction = Step(timeStep, error);
+            correction = StepError(timeStep, error);
 
             CurrentValue += correction;
 
@@ -347,7 +354,7 @@ public class BSFMotor : BSMotor
         return CurrentValue;
     }
 
-    public virtual float Step(float timeStep, float error)
+    public virtual float StepError(float timeStep, float error)
     {
         if (!Enabled) return 0f;
 
@@ -440,8 +447,8 @@ 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)
+    // Advance the PID computation on this error.
+    public override Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;