6 files changed, 369 insertions, 120 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 4dd6264..57c5898 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -69,6 +69,8 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 _appliedVelocity;   // the last velocity applied to the avatar
     private float _currentFriction;         // the friction currently being used (changed by setVelocity).
 
+    private BSVMotor _velocityMotor;
+
     private OMV.Vector3 _PIDTarget;
     private bool _usePID;
     private float _PIDTau;
@@ -89,6 +91,18 @@ public sealed class BSCharacter : BSPhysObject
         if (_size.X == 0f) _size.X = PhysicsScene.Params.avatarCapsuleDepth;
         if (_size.Y == 0f) _size.Y = PhysicsScene.Params.avatarCapsuleWidth;
 
+        // A motor to control the acceleration and deceleration of the avatar movement.
+        // _velocityMotor = new BSVMotor("BSCharacter.Velocity", 3f, 5f, BSMotor.InfiniteVector, 1f);
+        // _velocityMotor = new BSPIDVMotor("BSCharacter.Velocity", 3f, 5f, BSMotor.InfiniteVector, 1f);
+        // Infinite decay and timescale values so motor only changes current to target values.
+        _velocityMotor = new BSVMotor("BSCharacter.Velocity", 
+                                            0.2f,                       // time scale
+                                            BSMotor.Infinite,           // decay time scale
+                                            BSMotor.InfiniteVector,     // friction timescale
+                                            1f                          // efficiency
+        );
+        _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
@@ -138,6 +152,10 @@ public sealed class BSCharacter : BSPhysObject
         ForcePosition = _position;
         // Set the velocity and compute the proper friction
         ForceVelocity = _velocity;
+        // Setting the current and target in the motor will cause it to start computing any deceleration.
+        _velocityMotor.Reset();
+        _velocityMotor.SetCurrent(_velocity);
+        _velocityMotor.SetTarget(_velocity);
 
         // This will enable or disable the flying buoyancy of the avatar.
         // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
@@ -239,6 +257,7 @@ public sealed class BSCharacter : BSPhysObject
     public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
+        _velocityMotor.Zero();
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
@@ -400,10 +419,38 @@ public sealed class BSCharacter : BSPhysObject
 
     public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } }
     public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
+
+    // Sets the target in the motor. This starts the changing of the avatar's velocity.
+    public override OMV.Vector3 TargetVelocity
+    {
+        get
+        {
+            return _velocityMotor.TargetValue;
+        }
+        set
+        {
+            DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value);
+            OMV.Vector3 targetVel = value;
+            PhysicsScene.TaintedObject("BSCharacter.setTargetVelocity", delegate()
+            {
+                float timeStep = 0.089f;    // DEBUG DEBUG FIX FIX FIX
+                _velocityMotor.Reset();
+                _velocityMotor.SetTarget(targetVel);
+                _velocityMotor.SetCurrent(_velocity);
+                // Compute a velocity value and make sure it gets pushed into the avatar.
+                // This makes sure the avatar will start from a stop.
+                ForceVelocity = _velocityMotor.Step(timeStep);
+            });
+        }
+    }
+    // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
             _velocity = value;
+            _velocityMotor.Reset();
+            _velocityMotor.SetCurrent(_velocity);
+            _velocityMotor.SetTarget(_velocity);
             // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
             PhysicsScene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
@@ -415,6 +462,8 @@ public sealed class BSCharacter : BSPhysObject
     public override OMV.Vector3 ForceVelocity {
         get { return _velocity; }
         set {
+            PhysicsScene.AssertInTaintTime("BSCharacter.ForceVelocity");
+
             // Depending on whether the avatar is moving or not, change the friction
             //    to keep the avatar from slipping around
             if (_velocity.Length() == 0)
@@ -511,6 +560,13 @@ public sealed class BSCharacter : BSPhysObject
         get { return _flying; }
         set {
             _flying = value;
+
+            // Velocity movement is different when flying: flying velocity degrades over time.
+            if (_flying)
+                _velocityMotor.TargetValueDecayTimeScale = 1f;
+            else
+                _velocityMotor.TargetValueDecayTimeScale = BSMotor.Infinite;
+
             // simulate flying by changing the effect of gravity
             Buoyancy = ComputeBuoyancyFromFlying(_flying);
         }
@@ -581,7 +637,10 @@ public sealed class BSCharacter : BSPhysObject
     }
     public override float ForceBuoyancy {
         get { return _buoyancy; }
-        set { _buoyancy = value;
+        set { 
+            PhysicsScene.AssertInTaintTime("BSCharacter.ForceBuoyancy");
+
+            _buoyancy = value;
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
             float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
@@ -698,6 +757,22 @@ public sealed class BSCharacter : BSPhysObject
         LastEntityProperties = CurrentEntityProperties;
         CurrentEntityProperties = entprop;
 
+        // Avatars don't respond to world friction, etc. They only go the speed I tell them too.
+        // Special kludge here for falling. Even though the target velocity might not have a
+        //     Z component, the avatar could be falling (walked off a ledge, stopped flying, ...)
+        //     and that velocity component must be retained.
+        float timeStep = 0.089f;        // DEBUG DEBUG FIX FIX FIX
+        OMV.Vector3 stepVelocity = _velocityMotor.Step(timeStep);
+        // Remove next line so avatars don't fly up forever. DEBUG DEBUG this is only temporary.
+        // stepVelocity.Z += entprop.Velocity.Z;
+        _velocity = stepVelocity;
+        BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+        /*
+        OMV.Vector3 stepVelocity = _velocityMotor.Step(timeStep);
+        OMV.Vector3 avVel = new OMV.Vector3(stepVelocity.X, stepVelocity.Y, entprop.Velocity.Z);
+        _velocity = avVel;
+        BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, avVel);
+        
         if (entprop.Velocity != LastEntityProperties.Velocity)
         {
             // Changes in the velocity are suppressed in avatars.
@@ -706,6 +781,7 @@ public sealed class BSCharacter : BSPhysObject
             _velocity = avVel;
             BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, avVel);
         }
+         */
 
         // Tell the linkset about value changes
         Linkset.UpdateProperties(this, true);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 5887249..a5acd86 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -91,6 +91,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
+        private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
         private float m_angularDeflectionEfficiency = 0;
         private float m_angularDeflectionTimescale = 0;
         private float m_linearDeflectionEfficiency = 0;
@@ -102,6 +103,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_bankingTimescale = 0;
 
         //Hover and Buoyancy properties
+        private BSVMotor m_hoverMotor = new BSVMotor("Hover");
         private float m_VhoverHeight = 0f;
         private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
@@ -118,6 +120,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Timescale > cutoff  means no vert attractor.
         private float m_verticalAttractionTimescale = 510f;
 
+        // Just some recomputed constants:
+        static readonly float PIOverFour = ((float)Math.PI) / 4f;
+        static readonly float PIOverTwo = ((float)Math.PI) / 2f;
+
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
             PhysicsScene = myScene;
@@ -563,9 +569,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Vehicles report collision events so we know when it's on the ground
                 BulletSimAPI.AddToCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
 
-                // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet
-                // Vector3 localInertia = new Vector3(1f, 1f, 1f);
-                // Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
                 Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(Prim.PhysShape.ptr, m_vehicleMass);
                 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
                 BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
@@ -599,21 +602,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         #region Known vehicle value functions
         // Vehicle physical parameters that we buffer from constant getting and setting.
-        // The "m_known*" variables are initialized to 'null', fetched only if referenced
-        //      and stored back into the physics engine only if updated.
+        // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
+        //      Changing is remembered and the parameter is stored back into the physics engine only if updated.
         //      This does two things: 1) saves continuious calls into unmanaged code, and
         //      2) signals when a physics property update must happen back to the simulator
         //      to update values modified for the vehicle.
         private int m_knownChanged;
-        private float? m_knownTerrainHeight;
-        private float? m_knownWaterLevel;
-        private Vector3? m_knownPosition;
-        private Vector3? m_knownVelocity;
+        private int m_knownHas;
+        private float m_knownTerrainHeight;
+        private float m_knownWaterLevel;
+        private Vector3 m_knownPosition;
+        private Vector3 m_knownVelocity;
         private Vector3 m_knownForce;
-        private Quaternion? m_knownOrientation;
-        private Vector3? m_knownRotationalVelocity;
+        private Quaternion m_knownOrientation;
+        private Vector3 m_knownRotationalVelocity;
         private Vector3 m_knownRotationalForce;
-        private float? m_knownForwardSpeed;
+        private Vector3 m_knownForwardVelocity;    // vehicle relative forward speed
 
         private const int m_knownChangedPosition           = 1 << 0;
         private const int m_knownChangedVelocity           = 1 << 1;
@@ -621,18 +625,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private const int m_knownChangedOrientation        = 1 << 3;
         private const int m_knownChangedRotationalVelocity = 1 << 4;
         private const int m_knownChangedRotationalForce    = 1 << 5;
+        private const int m_knownChangedTerrainHeight      = 1 << 6;
+        private const int m_knownChangedWaterLevel         = 1 << 7;
+        private const int m_knownChangedForwardVelocity    = 1 << 8;
 
         private void ForgetKnownVehicleProperties()
         {
-            m_knownTerrainHeight = null;
-            m_knownWaterLevel = null;
-            m_knownPosition = null;
-            m_knownVelocity = null;
-            m_knownForce = Vector3.Zero;
-            m_knownOrientation = null;
-            m_knownRotationalVelocity = null;
-            m_knownRotationalForce = Vector3.Zero;
-            m_knownForwardSpeed = null;
+            m_knownHas = 0;
             m_knownChanged = 0;
         }
         private void PushKnownChanged()
@@ -671,17 +670,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    is used ot fetch the height only once for each vehicle simulation step.
         private float GetTerrainHeight(Vector3 pos)
         {
-            if (m_knownTerrainHeight == null)
+            if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
+            {
                 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-            return (float)m_knownTerrainHeight;
+                m_knownHas |= m_knownChangedTerrainHeight;
+            }
+            return m_knownTerrainHeight;
         }
 
         // Since the computation of water level can be a little involved, this routine
         //    is used ot fetch the level only once for each vehicle simulation step.
         private float GetWaterLevel(Vector3 pos)
         {
-            if (m_knownWaterLevel == null)
+            if ((m_knownHas & m_knownChangedWaterLevel) == 0)
+            {
                 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+                m_knownHas |= m_knownChangedWaterLevel;
+            }
             return (float)m_knownWaterLevel;
         }
 
@@ -689,8 +694,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownPosition == null)
+                if ((m_knownHas & m_knownChangedPosition) == 0)
+                {
                     m_knownPosition = Prim.ForcePosition;
+                    m_knownHas |= m_knownChangedPosition;
+                }
                 return (Vector3)m_knownPosition;
             }
             set
@@ -704,8 +712,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownOrientation == null)
+                if ((m_knownHas & m_knownChangedOrientation) == 0)
+                {
                     m_knownOrientation = Prim.ForceOrientation;
+                    m_knownHas |= m_knownChangedOrientation;
+                }
                 return (Quaternion)m_knownOrientation;
             }
             set
@@ -719,8 +730,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownVelocity == null)
+                if ((m_knownHas & m_knownChangedVelocity) == 0)
+                {
                     m_knownVelocity = Prim.ForceVelocity;
+                    m_knownHas |= m_knownChangedVelocity;
+                }
                 return (Vector3)m_knownVelocity;
             }
             set
@@ -740,8 +754,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownRotationalVelocity == null)
+                if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
+                {
                     m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                    m_knownHas |= m_knownChangedRotationalVelocity;
+                }
                 return (Vector3)m_knownRotationalVelocity;
             }
             set
@@ -755,13 +772,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_knownRotationalForce += aForce;
             m_knownChanged |= m_knownChangedRotationalForce;
         }
+        // Vehicle relative forward velocity
+        private Vector3 VehicleForwardVelocity
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
+                {
+                    m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
+                    m_knownHas |= m_knownChangedForwardVelocity;
+                }
+                return (Vector3)m_knownForwardVelocity;
+            }
+        }
         private float VehicleForwardSpeed
         {
             get
             {
-                if (m_knownForwardSpeed == null)
-                    m_knownForwardSpeed = (VehicleVelocity * Quaternion.Inverse(VehicleOrientation)).X;
-                return (float)m_knownForwardSpeed;
+                return VehicleForwardVelocity.X;
             }
         }
 
@@ -832,13 +860,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // ==================================================================
             // Clamp high or low velocities
             float newVelocityLengthSq = newVelocity.LengthSquared();
-            // if (newVelocityLengthSq > 1e6f)
             if (newVelocityLengthSq > 1000f)
             {
                 newVelocity /= newVelocity.Length();
                 newVelocity *= 1000f;
             }
-            // else if (newVelocityLengthSq < 1e-6f)
             else if (newVelocityLengthSq < 0.001f)
                 newVelocity = Vector3.Zero;
 
@@ -1003,7 +1029,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Not colliding if the vehicle is off the ground
                 if (!Prim.IsColliding)
                 {
-                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
                     ret = new Vector3(0, 0, -distanceAboveGround);
                 }
@@ -1026,7 +1051,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     set directly on the vehicle.
         private void MoveAngular(float pTimestep)
         {
-            // The user wants how many radians per second angular change?
+            // The user wants this many radians per second angular change?
             Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
 
             // ==================================================================
@@ -1135,31 +1160,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //         zero and one.
                 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
 
+                // Y error means needed rotation around X axis and visa versa.
+                // Since the error goes from zero to one, the asin is the corresponding angle.
+                ret.X = (float)Math.Asin(verticalError.Y);
+                // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
+                ret.Y = -(float)Math.Asin(verticalError.X);
+
                 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
                 if (verticalError.Z < 0f)
                 {
-                    verticalError.X = 2f - verticalError.X;
-                    verticalError.Y = 2f - verticalError.Y;
+                    ret.X += PIOverFour;
+                    ret.Y += PIOverFour;
                 }
 
-                // Y error means needed rotation around X axis and visa versa.
-                ret.X =    verticalError.Y;
-                ret.Y =  - verticalError.X;
-                ret.Z = 0f;
-
-                // Scale the correction force by how far we're off from vertical.
-                // Z error of one says little error. As Z gets smaller, the vehicle is leaning farther over.
-                float clampedSqrZError = ClampInRange(0.01f, verticalError.Z * verticalError.Z, 1f);
-                float vertForce = 1f / clampedSqrZError;
-
-                ret *= vertForce;
-
-                // Correction happens over a number of seconds.
+                // 'ret' is now the necessary velocity to correct tilt in one second.
+                //     Correction happens over a number of seconds.
                 Vector3 unscaledContrib = ret;
                 ret /= m_verticalAttractionTimescale;
 
-                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},vertForce={3},eff={4},vertAttr={5}",
-                                Prim.LocalID, verticalError, unscaledContrib, vertForce, m_verticalAttractionEfficiency, ret);
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
+                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
             }
             return ret;
         }
@@ -1172,7 +1192,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public Vector3 ComputeAngularDeflection()
         {
             Vector3 ret = Vector3.Zero;
-            return ret;     // DEBUG DEBUG DEBUG  debug one force at a time
+            return ret;         // DEBUG DEBUG DEBUG
+            // Disable angular deflection for the moment.
+            // Since angularMotorUp and angularDeflection are computed independently, they will calculate
+            //     approximately the same X or Y correction. When added together (when contributions are combined)
+            //     this creates an over-correction and then wabbling as the target is overshot.
+            // TODO: rethink how the different correction computations inter-relate.
 
             if (m_angularDeflectionEfficiency != 0)
             {
@@ -1184,15 +1209,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
                 pointingDirection.Normalize();
 
-                // The difference between what is and what should be
+                // The difference between what is and what should be.
                 Vector3 deflectionError = movingDirection - pointingDirection;
 
+                // Don't try to correct very large errors (not our job)
+                if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
+                if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
+                if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
+
+                // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
+
                 // Scale the correction by recovery timescale and efficiency
-                ret = (-deflectionError * VehicleForwardSpeed) * m_angularDeflectionEfficiency;
+                ret = (-deflectionError) * m_angularDeflectionEfficiency;
                 ret /= m_angularDeflectionTimescale;
 
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
                     Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
+                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
+                    Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
             }
             return ret;
         }
@@ -1308,6 +1342,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float ClampInRange(float low, float val, float high)
         {
             return Math.Max(low, Math.Min(val, high));
+            // return Utils.Clamp(val, low, high);
         }
 
         // Invoke the detailed logger and output something if it's enabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index e0faf4e..34a87c6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -29,13 +29,14 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 using OpenMetaverse;
+using OpenSim.Framework;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public abstract class BSMotor
 {
     // Timescales and other things can be turned off by setting them to 'infinite'.
-    public const float Infinite = 12345f;
+    public const float Infinite = 12345.6f;
     public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
 
     public BSMotor(string useName)
@@ -45,6 +46,7 @@ public abstract class BSMotor
     }
     public virtual void Reset() { }
     public virtual void Zero() { }
+    public virtual void GenerateTestOutput(float timeStep) { }
 
     // A name passed at motor creation for easily identifyable debugging messages.
     public string UseName { get; private set; }
@@ -62,12 +64,16 @@ public abstract class BSMotor
         }
     }
 }
-// Can all the incremental stepping be replaced with motor classes?
 
 // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
 // The TargetValue decays in TargetValueDecayTimeScale and
 //     the CurrentValue will be held back by FrictionTimeScale.
-// TimeScale and TargetDelayTimeScale may be 'infinite' which means go decay.
+// This motor will "zero itself" over time in that the targetValue will
+//    decay to zero and the currentValue will follow it to that zero.
+//    The overall effect is for the returned correction value to go from large
+//    values (the total difference between current and target minus friction)
+//    to small and eventually zero values.
+// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
 
 // For instance, if something is moving at speed X and the desired speed is Y,
 //    CurrentValue is X and TargetValue is Y. As the motor is stepped, new
@@ -81,13 +87,16 @@ public class BSVMotor : BSMotor
     // public Vector3 FrameOfReference { get; set; }
     // public Vector3 Offset { get; set; }
 
-    public float TimeScale { get; set; }
-    public float TargetValueDecayTimeScale { get; set; }
-    public Vector3 FrictionTimescale { get; set; }
-    public float Efficiency { get; set; }
+    public virtual float TimeScale { get; set; }
+    public virtual float TargetValueDecayTimeScale { get; set; }
+    public virtual Vector3 FrictionTimescale { get; set; }
+    public virtual float Efficiency { get; set; }
+
+    public virtual float ErrorZeroThreshold { get; set; }
 
-    public Vector3 TargetValue { get; private set; }
-    public Vector3 CurrentValue { get; private set; }
+    public virtual Vector3 TargetValue { get; protected set; }
+    public virtual Vector3 CurrentValue { get; protected set; }
+    public virtual Vector3 LastError { get; protected set; }
 
     public BSVMotor(string useName)
         : base(useName)
@@ -96,6 +105,7 @@ public class BSVMotor : BSMotor
         Efficiency = 1f;
         FrictionTimescale = BSMotor.InfiniteVector;
         CurrentValue = TargetValue = Vector3.Zero;
+        ErrorZeroThreshold = 0.01f;
     }
     public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
         : this(useName)
@@ -114,25 +124,25 @@ public class BSVMotor : BSMotor
     {
         TargetValue = target;
     }
-
-    // A form of stepping that does not take the time quantum into account.
-    // The caller must do the right thing later.
-    public Vector3 Step()
+    public override void Zero()
     {
-        return Step(1f);
+        base.Zero();
+        CurrentValue = TargetValue = Vector3.Zero;
     }
 
-    public Vector3 Step(float timeStep)
+    // Compute the next step and return the new current value
+    public virtual Vector3 Step(float timeStep)
     {
-        Vector3 returnCurrent = Vector3.Zero;
-        if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
+        Vector3 origTarget = TargetValue;       // DEBUG
+        Vector3 origCurrVal = CurrentValue;     // DEBUG
+
+        Vector3 correction = Vector3.Zero;
+        Vector3 error = TargetValue - CurrentValue;
+        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
         {
-            Vector3 origTarget = TargetValue;       // DEBUG
-            Vector3 origCurrVal = CurrentValue;   // DEBUG
+            correction = Step(timeStep, error);
 
-            // Addition =  (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete
-            Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
-            CurrentValue += addAmount;
+            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.
@@ -143,40 +153,80 @@ public class BSVMotor : BSMotor
                 TargetValue *= (1f - decayFactor);
             }
 
+            // The amount we can correct the error is reduced by the friction
             Vector3 frictionFactor = Vector3.Zero;
             if (FrictionTimescale != BSMotor.InfiniteVector)
             {
                 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
                 // Individual friction components can be 'infinite' so compute each separately.
-                frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep;
-                frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep;
-                frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep;
+                frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
+                frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
+                frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
+                frictionFactor *= timeStep;
                 CurrentValue *= (Vector3.One - frictionFactor);
             }
 
-            returnCurrent = CurrentValue;
-
-            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
                                 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
-                                timeStep, TimeScale, addAmount,
-                                TargetValueDecayTimeScale, decayFactor,
-                                FrictionTimescale, frictionFactor);
-            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},curr={2},target={3},add={4},decay={5},frict={6},ret={7}",
-                                    BSScene.DetailLogZero, UseName, CurrentValue, TargetValue,
-                                    addAmount, decayFactor, frictionFactor, returnCurrent);
+                                timeStep, error, correction);
+            MDetailLog("{0},  BSVMotor.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 = Vector3.Zero;
-            TargetValue = Vector3.Zero;
+            CurrentValue = TargetValue;
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}",
+                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
+        }
+
+        return CurrentValue;
+    }
+    public virtual Vector3 Step(float timeStep, Vector3 error)
+    {
+        LastError = error;
+        Vector3 returnCorrection = Vector3.Zero;
+        if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        {
+            // correction =  error / secondsItShouldTakeToCorrect
+            Vector3 correctionAmount;
+            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
+                correctionAmount = error * timeStep;
+            else
+                correctionAmount = error / TimeScale * timeStep;
 
-            MDetailLog("{0},  BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
-                                    BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
+            returnCorrection = correctionAmount;
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
+                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
+        }
+        return returnCorrection;
+    }
 
+    // The user sets all the parameters and calls this which outputs values until error is zero.
+    public override void GenerateTestOutput(float timeStep)
+    {
+        // maximum number of outputs to generate.
+        int maxOutput = 50;
+        MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
+        MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
+                                BSScene.DetailLogZero, UseName,
+                                TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, 
+                                CurrentValue, TargetValue);
+
+        LastError = BSMotor.InfiniteVector;
+        while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        {
+            Vector3 lastStep = Step(timeStep);
+            MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
+                            BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
         }
-        return returnCurrent;
+        MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
+        
+
     }
+
     public override string ToString()
     {
         return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>",
@@ -204,17 +254,74 @@ public class BSFMotor : BSMotor
     public void SetTarget(float target)
     {
     }
-    public float Step(float timeStep)
+    public virtual float Step(float timeStep)
     {
         return 0f;
     }
 }
-public class BSPIDMotor : BSMotor
+
+// 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
 {
-    // TODO: write and use this one
-    public BSPIDMotor(string useName)
+    // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
+    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; }
+
+    public BSPIDVMotor(string useName)
         : base(useName)
     {
+        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;
+        LastError = Vector3.Zero;
+    }
+
+    public override void Zero()
+    {
+        base.Zero();
+    }
+
+    public override float Efficiency
+    {
+        get { return base.Efficiency; }
+        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);
+        }
+    }
+
+    // Ignore Current and Target Values and just advance the PID computation on this error.
+    public override Vector3 Step(float timeStep, Vector3 error)
+    {
+        // Add up the error so we can integrate over the accumulated errors
+        IntegralFactor += 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);
+
+        return ret;
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 758d92b..68a0db6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -100,10 +100,15 @@ public sealed class BSPrim : BSPhysObject
         BaseShape = pbs;
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
-        _friction = PhysicsScene.Params.defaultFriction;  // TODO: compute based on object material
-        _density = PhysicsScene.Params.defaultDensity;    // TODO: compute based on object material
+
+        // Someday set default attributes based on the material but, for now, we don't know the prim material yet.
+        // MaterialAttributes primMat = BSMaterials.GetAttributes(Material, pisPhysical);
+        _density = PhysicsScene.Params.defaultDensity;
+        _friction = PhysicsScene.Params.defaultFriction;
         _restitution = PhysicsScene.Params.defaultRestitution;
+
         _vehicle = new BSDynamics(PhysicsScene, this);            // add vehicleness
+
         _mass = CalculateMass();
 
         // No body or shape yet
@@ -527,16 +532,18 @@ public sealed class BSPrim : BSPhysObject
             PhysicsScene.TaintedObject("BSPrim.setVelocity", delegate()
             {
                 // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                if (PhysBody.HasPhysicalBody)
-                    BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+                ForceVelocity = _velocity;
             });
         }
     }
     public override OMV.Vector3 ForceVelocity {
         get { return _velocity; }
         set {
+            PhysicsScene.AssertInTaintTime("BSPrim.ForceVelocity");
+
             _velocity = value;
-            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
         }
     }
     public override OMV.Vector3 Torque {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 069cb0d..2ca4912 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -96,6 +96,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     public long SimulationStep { get { return m_simulationStep; } }
     private int m_taintsToProcessPerStep;
 
+    // Avatar parameters
+    public float ParamAvatarFriction { get; private set; }
+    public float ParamAvatarStandingFriction { get; private set; }
+    public float ParamAvatarDensity { get; private set; }
+    public float ParamAvatarRestitution { get; private set; }
+    public float ParamAvatarCapsuleWidth { get; private set; }
+    public float ParamAvatarCapsuleDepth { get; private set; }
+    public float ParamAvatarCapsuleHeight { get; private set; }
+        public float ParamAvatarContactProcessingThreshold { get; private set; }
+
     public delegate void PreStepAction(float timeStep);
     public event PreStepAction BeforeStep;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 0d9a156..c084ab4 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -1,15 +1,16 @@
 CURRENT PRIORITIES
 =================================================
-Eliminate all crashes (DONEish)
-        Editing/deleting physical linkset (DONE)
-        Border crossing of physical linkset (DONE)
+Smooth avatar movement with motor
+        Should motor update be all at taint-time?
 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)
-Calibrate turning radius
+Vehicle movement on terrain smoothness
+Vehicle script tuning/debugging
+        Avanti speed script
+        Weapon shooter script
 limitMotorUp calibration (more down?)
-study PID motors (include 'efficiency' implementation
-        Add to avatar movement
 
 CRASHES
 =================================================
@@ -25,7 +26,6 @@ CRASHES
 VEHICLES TODO LIST:
 =================================================
 Border crossing with linked vehicle causes crash
-Neb vehicle taking > 25ms of physics time!!
 Vehicles (Move smoothly)
 Add vehicle collisions so IsColliding is properly reported.
         Needed for banking, limitMotorUp, movementLimiting, ...
@@ -34,28 +34,25 @@ Cannot edit/move a vehicle being ridden: it jumps back to the origional position
 Neb car jiggling left and right
         Happens on terrain and any other mesh object. Flat cubes are much smoother.
         This has been reduced but not eliminated.
-Light cycle falling over when driving
 Implement referenceFrame for all the motion routines.
 Angular motion around Z moves the vehicle in world Z and not vehicle Z in ODE.
         Verify that angular motion specified around Z moves in the vehicle coordinates.
 Verify llGetVel() is returning a smooth and good value for vehicle movement.
 llGetVel() should return the root's velocity if requested in a child prim.
 Implement function efficiency for lineaar and angular motion.
-Should vehicle angular/linear movement friction happen after all the components
-        or does it only apply to the basic movement?
 After getting off a vehicle, the root prim is phantom (can be walked through)
     Need to force a position update for the root prim after compound shape destruction
 Linkset explosion after three "rides" on Nebadon lite vehicle (LinksetConstraint)
 For limitMotorUp, use raycast down to find if vehicle is in the air.
 Remove vehicle angular velocity zeroing in BSPrim.UpdateProperties().
         A kludge that isn't fixing the real problem of Bullet adding extra motion.
+Incorporate inter-relationship of angular corrections. For instance, angularDeflection
+        and angularMotorUp will compute same X or Y correction. When added together
+        creates over-correction and over-shoot and wabbling.
 
 BULLETSIM TODO LIST:
 =================================================
 Revisit CollisionMargin. Builders notice the 0.04 spacing between prims.
-Avatar height off after unsitting (floats off ground)
-        Editting appearance then moving restores.
-        Must not be initializing height when recreating capsule after unsit.
 Duplicating a physical prim causes old prim to jump away
         Dup a phys prim and the original become unselected and thus interacts w/ selected prim.
 Scenes with hundred of thousands of static objects take a lot of physics CPU time.
@@ -82,6 +79,9 @@ Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
         Also osGetPhysicsEngineVerion() maybe.
 Linkset.Position and Linkset.Orientation requre rewrite to properly return
         child position. LinksetConstraint acts like it's at taint time!!
+Implement LockAngularMotion -- implements llSetStatus(ROTATE_AXIS_*, T/F)
+Should the different PID factors have non-equal contributions for different
+        values of Efficiency?
 
 LINKSETS
 ======================================================
@@ -99,17 +99,16 @@ Disable activity of passive linkset children.
         Since the linkset is a compound object, the old prims are left lying
         around and need to be phantomized so they don't collide, ...
 Speed up creation of large physical linksets
-        For instance, sitting in Neb's car (130 prims) takes several seconds to become physical
+        For instance, sitting in Neb's car (130 prims) takes several seconds to become physical.
+        REALLY bad for very large physical linksets (freezes the sim for many seconds).
 Eliminate collisions between objects in a linkset. (LinksetConstraint)
     Have UserPointer point to struct with localID and linksetID?
     Objects in original linkset still collide with each other?
 
 MORE
 ======================================================
-Find/remove avatar collision with ID=0.
 Test avatar walking up stairs. How does compare with SL.
     Radius of the capsule affects ability to climb edges.
-Tune terrain/object friction to be closer to SL.
 Debounce avatar contact so legs don't keep folding up when standing.
 Implement LSL physics controls. Like STATUS_ROTATE_X.
 Add border extensions to terrain to help region crossings and objects leaving region.
@@ -140,6 +139,8 @@ Consider moving prim/character body and shape destruction in destroy()
         to postTimeTime rather than protecting all the potential sets that
         might have been queued up.
 Remove unused fields from ShapeData (not used in API2)
+Remove unused fields from pinned memory shared parameter block
+        Create parameter variables in BSScene to replace same.
 Breakout code for mesh/hull/compound/native into separate BSShape* classes
     Standardize access to building and reference code.
         The skeleton classes are in the sources but are not complete or linked in.
@@ -202,3 +203,16 @@ Single prim vehicles don't seem to properly vehiclize.
 Add material type linkage and input all the material property definitions.
         Skeleton classes and table are in the sources but are not filled or used.
         (Resolution: 
+Neb vehicle taking > 25ms of physics time!!
+        (Resolution: compound linksets were being rebuild WAY too often)
+Avatar height off after unsitting (floats off ground)
+        Editting appearance then moving restores.
+        Must not be initializing height when recreating capsule after unsit.
+        (Resolution: confusion of scale vs size for native objects removed)
+Light cycle falling over when driving (Resolution: implemented angularMotorUp)
+Should vehicle angular/linear movement friction happen after all the components
+        or does it only apply to the basic movement?
+        (Resolution: friction added before returning newly computed motor value.
+        What is expected by some vehicles (turning up friction to moderate speed))
+Tune terrain/object friction to be closer to SL.
+        (Resolution: added material type with friction and resolution)