BulletSim: complete vector motor. Correct line endings.

OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs

@@ -1,4 +1,4 @@
-using System;
+using System;
 using System.Collections.Generic;
 using System.Text;
 using OpenMetaverse;
@@ -7,8 +7,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public abstract class BSMotor
 {
+    public BSMotor()
+    {
+        PhysicsScene = null;
+    }
     public virtual void Reset() { }
     public virtual void Zero() { }
+
+    // Used only for outputting debug information. Might not be set so check for null.
+    public BSScene PhysicsScene { get; set; }
+    protected void MDetailLog(string msg, params Object[] parms)
+    {
+        if (PhysicsScene != null)
+        {
+            if (PhysicsScene.VehicleLoggingEnabled)
+            {
+                PhysicsScene.DetailLog(msg, parms);
+            }
+        }
+    }
 }
 // Can all the incremental stepping be replaced with motor classes?
 public class BSVMotor : BSMotor
@@ -24,14 +41,13 @@ public class BSVMotor : BSMotor
     public Vector3 TargetValue { get; private set; }
     public Vector3 CurrentValue { get; private set; }
 
-
+    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) : base()
-
-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
     {
         TimeScale = timeScale;
         TargetValueDecayTimeScale = decayTimeScale;
         CurrentValueReductionTimescale = frictionTimeScale;
         Efficiency = efficiency;
+        CurrentValue = TargetValue = Vector3.Zero;
     }
     public void SetCurrent(Vector3 current)
     {
@@ -43,30 +59,39 @@ public class BSVMotor : BSMotor
     }
     public Vector3 Step(float timeStep)
     {
+        Vector3 returnCurrent = Vector3.Zero;
         if (CurrentValue.LengthSquared() > 0.001f)
         {
             // Vector3 origDir = Target;       // DEBUG
             // Vector3 origVel = CurrentValue;   // DEBUG
 
-            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete
+            // Add (desiredVector - currentAppliedVector) / howLongItShouldTakeToComplete
-            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;
+            Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
             CurrentValue += addAmount;
+            returnCurrent = CurrentValue;
 
+            // The desired value reduces to zero when also reduces the difference with current.
             float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
             TargetValue *= (1f - decayFactor);
 
             Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;
             CurrentValue *= (Vector3.One - frictionFactor);
+
+            MDetailLog("{0},BSVMotor.Step,nonZero,curr={1},target={2},add={3},decay={4},frict={5},ret={6}",
+                                    BSScene.DetailLogZero, TargetValue, CurrentValue, 
+                                    addAmount, decayFactor, frictionFactor, returnCurrent);
         }
         else
         {
-            // if what remains of direction is very small, zero it.
+            // Difference between what we have and target is small. Motor is done.
-            TargetValue = Vector3.Zero;
             CurrentValue = Vector3.Zero;
+            TargetValue = Vector3.Zero;
+
+            MDetailLog("{0},BSVMotor.Step,zero,curr={1},target={2},ret={3}",
+                                    BSScene.DetailLogZero, TargetValue, CurrentValue, returnCurrent);
 
-            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
         }
-        return CurrentValue;
+        return returnCurrent;
     }
 }
 
@@ -80,7 +105,7 @@ public class BSFMotor : BSMotor
     public float Target { get; private set; }
     public float CurrentValue { get; private set; }
 
-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)
+    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency) : base()
     {
     }
     public void SetCurrent(float target)
@@ -97,7 +122,7 @@ public class BSFMotor : BSMotor
 public class BSPIDMotor : BSMotor
 {
     // TODO: write and use this one
-    BSPIDMotor()
+    BSPIDMotor() : base()
     {
     }
 }