10 files changed, 372 insertions, 96 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
index 8c6e7d6..14de2eb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -530,12 +530,12 @@ public override void SetForceUpdateAllAabbs(BulletWorld world, bool force)
 // btDynamicsWorld entries
 public override bool AddObjectToWorld(BulletWorld world, BulletBody obj)
 {
-    // Bullet resets several variables when an object is added to the world.
-    //   Gravity is reset to world default depending on the static/dynamic
-    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
 
+    // Bullet resets several variables when an object is added to the world.
+    //   Gravity is reset to world default depending on the static/dynamic
+    //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
     Vector3 origGrav = BSAPICPP.GetGravity2(bodyu.ptr);
 
     bool ret = BSAPICPP.AddObjectToWorld2(worldu.ptr, bodyu.ptr);
@@ -921,6 +921,7 @@ public override void SetCenterOfMassByPosRot(BulletBody obj, Vector3 pos, Quater
 }
 
 // Add a force to the object as if its mass is one.
+// Deep down in Bullet: m_totalForce += force*m_linearFactor;
 public override void ApplyCentralForce(BulletBody obj, Vector3 force)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -964,6 +965,7 @@ public override void SetSleepingThresholds(BulletBody obj, float lin_threshold,
     BSAPICPP.SetSleepingThresholds2(bodyu.ptr, lin_threshold, ang_threshold);
 }
 
+// Deep down in Bullet: m_totalTorque += torque*m_angularFactor;
 public override void ApplyTorque(BulletBody obj, Vector3 torque)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -971,6 +973,8 @@ public override void ApplyTorque(BulletBody obj, Vector3 torque)
 }
 
 // Apply force at the given point. Will add torque to the object.
+// Deep down in Bullet: applyCentralForce(force);
+//                              applyTorque(rel_pos.cross(force*m_linearFactor));
 public override void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -978,6 +982,7 @@ public override void ApplyForce(BulletBody obj, Vector3 force, Vector3 pos)
 }
 
 // Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
+// Deep down in Bullet: m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
 public override void ApplyCentralImpulse(BulletBody obj, Vector3 imp)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -985,6 +990,7 @@ public override void ApplyCentralImpulse(BulletBody obj, Vector3 imp)
 }
 
 // Apply impulse to the object's torque. Force is scaled by object's mass.
+// Deep down in Bullet: m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
 public override void ApplyTorqueImpulse(BulletBody obj, Vector3 imp)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -992,6 +998,8 @@ public override void ApplyTorqueImpulse(BulletBody obj, Vector3 imp)
 }
 
 // Apply impulse at the point given. For is scaled by object's mass and effects both linear and angular forces.
+// Deep down in Bullet: applyCentralImpulse(impulse);
+//                              applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
 public override void ApplyImpulse(BulletBody obj, Vector3 imp, Vector3 pos)
 {
     BulletBodyUnman bodyu = obj as BulletBodyUnman;
@@ -1259,6 +1267,16 @@ public override void DumpPhysicsStatistics(BulletWorld world)
     BulletWorldUnman worldu = world as BulletWorldUnman;
     BSAPICPP.DumpPhysicsStatistics2(worldu.ptr);
 }
+public override void ResetBroadphasePool(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetBroadphasePool(worldu.ptr);
+}
+public override void ResetConstraintSolver(BulletWorld world)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BSAPICPP.ResetConstraintSolver(worldu.ptr);
+}
 
 // =====================================================================================
 // =====================================================================================
@@ -1832,6 +1850,12 @@ public static extern void DumpAllInfo2(IntPtr sim);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpPhysicsStatistics2(IntPtr sim);
 
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetBroadphasePool(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ResetConstraintSolver(IntPtr sim);
+
 }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index 30a7bee..0c7f315 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -232,21 +232,25 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
     {
+        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
+        CollisionObject cbody = ((BulletBodyXNA)pBody).body;
+        RigidBody rbody = cbody as RigidBody;
+
         // Bullet resets several variables when an object is added to the world. In particular,
         //   BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
         //   type. Of course, the collision flags in the broadphase proxy are initialized to default.
-        DiscreteDynamicsWorld world = ((BulletWorldXNA)pWorld).world;
-        RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
-
-        IndexedMatrix origPos = body.GetWorldTransform();
-        IndexedVector3 origGrav = body.GetGravity();
-
-        //if (!(body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE && body.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE))
-
-        world.AddRigidBody(body);
-
-        body.SetWorldTransform(origPos);
-        body.SetGravity(origGrav);
+        IndexedMatrix origPos = cbody.GetWorldTransform();
+        if (rbody != null)
+        {
+            IndexedVector3 origGrav = rbody.GetGravity();
+            world.AddRigidBody(rbody);
+            rbody.SetGravity(origGrav);
+        }
+        else
+        {
+            world.AddCollisionObject(rbody);
+        }
+        cbody.SetWorldTransform(origPos);
 
         pBody.ApplyCollisionMask(pWorld.physicsScene);
 
@@ -773,35 +777,6 @@ private sealed class BulletConstraintXNA : BulletConstraint
         body.ApplyTorqueImpulse(ref fSum);
     }
 
-    public override void DumpRigidBody(BulletWorld p, BulletBody p_2)
-    {
-        //TODO:
-    }
-
-    public override void DumpCollisionShape(BulletWorld p, BulletShape p_2)
-    {
-        //TODO:
-    }
-    public override void DumpConstraint(BulletWorld world, BulletConstraint constrain)
-    {
-        //TODO:
-    }
-
-    public override void DumpActivationInfo(BulletWorld world)
-    {
-        //TODO:
-    }
-
-    public override void DumpAllInfo(BulletWorld world)
-    {
-        //TODO:
-    }
-
-    public override void DumpPhysicsStatistics(BulletWorld world)
-    {
-        //TODO:
-    }
-
     public override void DestroyObject(BulletWorld p, BulletBody p_2)
     {
         //TODO:
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index 8ad78ca..befb076 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -646,17 +646,21 @@ public abstract float GetMargin(BulletShape shape);
 
 // =====================================================================================
 // Debugging
-public abstract void DumpRigidBody(BulletWorld sim, BulletBody collisionObject);
+public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { }
 
-public abstract void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape);
+public virtual void DumpCollisionShape(BulletWorld sim, BulletShape collisionShape) { }
 
-public abstract void DumpConstraint(BulletWorld sim, BulletConstraint constrain);
+public virtual void DumpConstraint(BulletWorld sim, BulletConstraint constrain) { }
 
-public abstract void DumpActivationInfo(BulletWorld sim);
+public virtual void DumpActivationInfo(BulletWorld sim) { }
 
-public abstract void DumpAllInfo(BulletWorld sim);
+public virtual void DumpAllInfo(BulletWorld sim) { }
 
-public abstract void DumpPhysicsStatistics(BulletWorld sim);
+public virtual void DumpPhysicsStatistics(BulletWorld sim) { }
+
+public virtual void ResetBroadphasePool(BulletWorld sim) { }
+
+public virtual void ResetConstraintSolver(BulletWorld sim) { }
 
 };
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 103d8fc..c215e3a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -214,7 +214,7 @@ public sealed class BSCharacter : BSPhysObject
             }
 
             // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
-            OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass / PhysicsScene.LastTimeStep;
+            OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
 
             /*
             // If moveForce is very small, zero things so we don't keep sending microscopic updates to the user
@@ -231,7 +231,7 @@ public sealed class BSCharacter : BSPhysObject
             }
             */
             // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
-            AddForce(moveForce, false, true);
+            PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
         });
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 817a5f7..6d0db2e 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 float Target { get; private set; }
-    public float CurrentValue { get; private set; }
+    public virtual float TargetValue { get; protected set; }
+    public virtual float CurrentValue { get; protected set; }
+    public virtual float LastError { get; protected 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
@@ -281,6 +388,12 @@ public class BSPIDVMotor : BSVMotor
     public Vector3 integralFactor { get; set; }
     public Vector3 derivFactor { get; set; }
 
+    // The factors are vectors for the three dimensions. This is the proportional of each
+    //    that is applied. This could be multiplied through the actual factors but it
+    //    is sometimes easier to manipulate the factors and their mix separately.
+    //      to 
+    public Vector3 FactorMix;
+
     // Arbritrary factor range.
     // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
     public float EfficiencyHigh = 0.4f;
@@ -295,6 +408,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);
+        FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
         RunningIntegration = Vector3.Zero;
         LastError = Vector3.Zero;
     }
@@ -310,15 +424,19 @@ public class BSPIDVMotor : BSVMotor
         set
         {
             base.Efficiency = Util.Clamp(value, 0f, 1f);
+
             // Compute factors based on efficiency.
             // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
             // If efficiency is low (0f), use a factor value that overcorrects.
             // TODO: might want to vary contribution of different factor depending on efficiency.
             float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
             // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
+
             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);
         }
     }
 
@@ -331,15 +449,17 @@ public class BSPIDVMotor : BSVMotor
         RunningIntegration += error * timeStep;
 
         // A simple derivitive is the rate of change from the last error.
-        Vector3 derivFactor = (error - LastError) * timeStep;
+        Vector3 derivitive = (error - LastError) * timeStep;
         LastError = error;
 
-        // Correction = -(proportionOfPresentError +      accumulationOfPastError    +     rateOfChangeOfError)
-        Vector3 ret   = -(
-                          error * proportionFactor
-                        + RunningIntegration * integralFactor 
-                        + derivFactor * derivFactor
-                        );
+        // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
+        Vector3 ret   =   error * timeStep   * proportionFactor * FactorMix.X
+                        + RunningIntegration * integralFactor   * FactorMix.Y
+                        + derivitive         * derivFactor      * FactorMix.Z
+                        ;
+
+        MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
+                        BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret);
 
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 69ac8cd..b9bd0bf 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -497,6 +497,16 @@ public static class BSParam
             (s,cf,p,v) => { s.PhysicsMetricDumpFrames = cf.GetFloat(p, (int)v); },
             (s) => { return (float)s.PhysicsMetricDumpFrames; },
             (s,p,l,v) => { s.PhysicsMetricDumpFrames = (int)v; } ),
+        new ParameterDefn("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool",
+            0f,
+            (s,cf,p,v) => { ; },
+            (s) => { return 0f; },
+            (s,p,l,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ),
+        new ParameterDefn("ResetConstraintSolver", "Setting this is any value resets the constraint solver",
+            0f,
+            (s,cf,p,v) => { ; },
+            (s) => { return 0f; },
+            (s,p,l,v) => { BSParam.ResetConstraintSolverTainted(s, v); } ),
     };
 
     // Convert a boolean to our numeric true and false values
@@ -511,6 +521,24 @@ public static class BSParam
         return (b == ConfigurationParameters.numericTrue ? true : false);
     }
 
+    private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetBroadphasePoolTainted", delegate()
+        {
+            physScene.PE.ResetBroadphasePool(physScene.World);
+        });
+    }
+
+    private static void ResetConstraintSolverTainted(BSScene pPhysScene, float v)
+    {
+        BSScene physScene = pPhysScene;
+        physScene.TaintedObject("BSParam.ResetConstraintSolver", delegate()
+        {
+            physScene.PE.ResetConstraintSolver(physScene.World);
+        });
+    }
+
     // Search through the parameter definitions and return the matching
     //    ParameterDefn structure.
     // Case does not matter as names are compared after converting to lower case.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index e7cb3e0..534f929 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -343,6 +343,10 @@ public abstract class BSPhysObject : PhysicsActor
     protected void RegisterPreStepAction(string op, uint id, BSScene.PreStepAction actn)
     {
         string identifier = op + "-" + id.ToString();
+
+        // Clean out any existing action
+        UnRegisterPreStepAction(op, id);
+
         RegisteredActions[identifier] = actn;
         PhysicsScene.BeforeStep += actn;
         DetailLog("{0},BSPhysObject.RegisterPreStepAction,id={1}", LocalID, identifier);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 826261c..94b63e5 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -66,9 +66,6 @@ public sealed class BSPrim : BSPhysObject
     private float _restitution;
     private bool _setAlwaysRun;
     private bool _throttleUpdates;
-    private bool _isColliding;
-    private bool _collidingGround;
-    private bool _collidingObj;
     private bool _floatOnWater;
     private OMV.Vector3 _rotationalVelocity;
     private bool _kinematic;
@@ -76,13 +73,14 @@ public sealed class BSPrim : BSPhysObject
 
     private BSDynamics _vehicle;
 
+    private BSVMotor _targetMotor;
     private OMV.Vector3 _PIDTarget;
-    private bool _usePID;
     private float _PIDTau;
-    private bool _useHoverPID;
+
+    private BSFMotor _hoverMotor;
     private float _PIDHoverHeight;
     private PIDHoverType _PIDHoverType;
-    private float _PIDHoverTao;
+    private float _PIDHoverTau;
 
     public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -564,6 +562,11 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
+    public OMV.Vector3 RawVelocity
+    {
+        get { return _velocity; }
+        set { _velocity = value; }
+    }
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
@@ -1004,13 +1007,99 @@ public sealed class BSPrim : BSPhysObject
         set { _PIDTau = value; }
     }
     public override bool PIDActive {
-        set { _usePID = value; }
+        set {
+            if (value)
+            {
+                // We're taking over after this.
+                ZeroMotion(true);
+
+                _targetMotor = new BSVMotor("BSPrim.PIDTarget",
+                                            _PIDTau,                    // timeScale
+                                            BSMotor.Infinite,           // decay time scale
+                                            BSMotor.InfiniteVector,     // friction timescale
+                                            1f                          // efficiency
+                );
+                _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+                _targetMotor.SetTarget(_PIDTarget);
+                _targetMotor.SetCurrent(RawPosition);
+                /*
+                _targetMotor = new BSPIDVMotor("BSPrim.PIDTarget");
+                _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
+                _targetMotor.SetTarget(_PIDTarget);
+                _targetMotor.SetCurrent(RawPosition);
+                _targetMotor.TimeScale = _PIDTau;
+                _targetMotor.Efficiency = 1f;
+                 */
+
+                RegisterPreStepAction("BSPrim.PIDTarget", LocalID, delegate(float timeStep)
+                {
+                    OMV.Vector3 origPosition = RawPosition;     // DEBUG DEBUG (for printout below)
+
+                    // 'movePosition' is where we'd like the prim to be at this moment.
+                    OMV.Vector3 movePosition = _targetMotor.Step(timeStep);
+
+                    // If we are very close to our target, turn off the movement motor.
+                    if (_targetMotor.ErrorIsZero())
+                    {
+                        DetailLog("{0},BSPrim.PIDTarget,zeroMovement,movePos={1},pos={2},mass={3}",
+                                                LocalID, movePosition, RawPosition, Mass);
+                        ForcePosition = _targetMotor.TargetValue;
+                        _targetMotor.Enabled = false;
+                    }
+                    else
+                    {
+                        ForcePosition = movePosition;
+                    }
+                    DetailLog("{0},BSPrim.PIDTarget,move,fromPos={1},movePos={2}", LocalID, origPosition, movePosition);
+                });
+            }
+            else
+            {
+                // Stop any targetting
+                UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID);
+            }
+        }
     }
 
     // Used for llSetHoverHeight and maybe vehicle height
     // Hover Height will override MoveTo target's Z
     public override bool PIDHoverActive {
-        set { _useHoverPID = value; }
+        set {
+            if (value)
+            {
+                // Turning the target on
+                _hoverMotor = new BSFMotor("BSPrim.Hover",
+                                            _PIDHoverTau,               // timeScale
+                                            BSMotor.Infinite,           // decay time scale
+                                            BSMotor.Infinite,           // friction timescale
+                                            1f                          // efficiency
+                );
+                _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight());
+                _hoverMotor.SetCurrent(RawPosition.Z);
+                _hoverMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
+                RegisterPreStepAction("BSPrim.Hover", LocalID, delegate(float timeStep)
+                {
+                    _hoverMotor.SetCurrent(RawPosition.Z);
+                    _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight());
+                    float targetHeight = _hoverMotor.Step(timeStep);
+
+                    // 'targetHeight' is where we'd like the Z of the prim to be at this moment.
+                    // Compute the amount of force to push us there.
+                    float moveForce = (targetHeight - RawPosition.Z) * Mass;
+                    // Undo anything the object thinks it's doing at the moment
+                    moveForce = -RawVelocity.Z * Mass;
+
+                    PhysicsScene.PE.ApplyCentralImpulse(PhysBody, new OMV.Vector3(0f, 0f, moveForce));
+                    DetailLog("{0},BSPrim.Hover,move,targHt={1},moveForce={2},mass={3}", LocalID, targetHeight, moveForce, Mass);
+                });
+            }
+            else
+            {
+                UnRegisterPreStepAction("BSPrim.Hover", LocalID);
+            }
+        }
     }
     public override float PIDHoverHeight {
         set { _PIDHoverHeight = value; }
@@ -1019,8 +1108,35 @@ public sealed class BSPrim : BSPhysObject
         set { _PIDHoverType = value; }
     }
     public override float PIDHoverTau {
-        set { _PIDHoverTao = value; }
+        set { _PIDHoverTau = value; }
     }
+    // Based on current position, determine what we should be hovering at now.
+    // Must recompute often. What if we walked offa cliff>
+    private float ComputeCurrentPIDHoverHeight()
+    {
+        float ret = _PIDHoverHeight;
+        float groundHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
+
+        switch (_PIDHoverType)
+        {
+            case PIDHoverType.Ground:
+                ret = groundHeight + _PIDHoverHeight;
+                break;
+            case PIDHoverType.GroundAndWater:
+                float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
+                if (groundHeight > waterHeight)
+                {
+                    ret = groundHeight + _PIDHoverHeight;
+                }
+                else
+                {
+                    ret = waterHeight + _PIDHoverHeight;
+                }
+                break;
+        }
+        return ret;
+    }
+
 
     // For RotLookAt
     public override OMV.Quaternion APIDTarget { set { return; } }
@@ -1037,7 +1153,7 @@ public sealed class BSPrim : BSPhysObject
     // This added force will only last the next simulation tick.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
-        if (force.IsFinite())
+        if (!IsStatic && force.IsFinite())
         {
             float magnitude = force.Length();
             if (magnitude > BSParam.MaxAddForceMagnitude)
@@ -1047,7 +1163,7 @@ public sealed class BSPrim : BSPhysObject
             }
 
             OMV.Vector3 addForce = force;
-            DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+            // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
 
             PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
             {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index a7855f0..b4f764b 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -453,6 +453,7 @@ public sealed class BSShapeCollection : IDisposable
         // If the prim attributes are simple, this could be a simple Bullet native shape
         if (!haveShape
                 && pbs != null
+                && !pbs.SculptEntry
                 && nativeShapePossible
                 && ((pbs.SculptEntry && !BSParam.ShouldMeshSculptedPrim)
                     || (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index a8a4ff5..facf720 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,22 +1,26 @@
 CURRENT PRIORITIES
 =================================================
-Redo BulletSimAPI to allow native C# implementation of Bullet option.
+Redo BulletSimAPI to allow native C# implementation of Bullet option (DONE)
+Meshes rendering as bounding boxes
+llMoveToTarget
+Vehicle movement on terrain smoothness
+limitMotorUp calibration (more down?)
+Preferred orientatino angular correction fix
+Surfboard go wonky when turning
+        Angular motor direction is global coordinates rather than local coordinates?
+Boats float low in the water
 Avatar movement
-        flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle
+        flying into a wall doesn't stop avatar who keeps appearing to move through the obstacle (DONE)
         walking up stairs is not calibrated correctly (stairs out of Kepler cabin)
-        avatar capsule rotation completed
-llMoveToTarget
+        avatar capsule rotation completed (NOT DONE - Bullet's capsule shape is not the solution)
 Enable vehicle border crossings (at least as poorly as ODE)
         Terrain skirts
         Avatar created in previous region and not new region when crossing border
         Vehicle recreated in new sim at small Z value (offset from root value?) (DONE)
-Vehicle movement on terrain smoothness
 Vehicle script tuning/debugging
         Avanti speed script
         Weapon shooter script
-limitMotorUp calibration (more down?)
-Boats float low in the water
-Add material densities to the material types.
+Add material densities to the material types
 
 CRASHES
 =================================================