1 files changed, 181 insertions, 330 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 5a9f135..65b38d6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -54,17 +54,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
     public class BSDynamics
     {
-        private int frcount = 0;                                        // Used to limit dynamics debug output to
-                                                                        // every 100th frame
-
-        private BSPrim m_prim;      // the prim this dynamic controller belongs to
+        private BSScene PhysicsScene { get; set; }
+        // the prim this dynamic controller belongs to
+        private BSPrim Prim { get; set; }
 
         // Vehicle properties
-        private Vehicle m_type = Vehicle.TYPE_NONE;                     // If a 'VEHICLE', and what kind
-        public Vehicle Type
-        {
-            get { return m_type; }
-        }
+        public Vehicle Type { get; set; }
+
         // private Quaternion m_referenceFrame = Quaternion.Identity;   // Axis modifier
         private VehicleFlag m_flags = (VehicleFlag) 0;                  // Boolean settings:
                                                                         // HOVER_TERRAIN_ONLY
@@ -74,13 +70,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                                                         // HOVER_UP_ONLY
                                                                         // LIMIT_MOTOR_UP
                                                                         // LIMIT_ROLL_ONLY
-        private VehicleFlag m_Hoverflags = (VehicleFlag)0;
         private Vector3 m_BlockingEndPoint = Vector3.Zero;
         private Quaternion m_RollreferenceFrame = Quaternion.Identity;
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
-        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
+        private Vector3 m_newVelocity = Vector3.Zero;                   // velocity computed to be applied to body
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
         private float m_linearMotorDecayTimescale = 0;
         private float m_linearMotorTimescale = 0;
@@ -124,15 +119,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
 
-        public BSDynamics(BSPrim myPrim)
+        public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
-            m_prim = myPrim;
-            m_type = Vehicle.TYPE_NONE;
+            PhysicsScene = myScene;
+            Prim = myPrim;
+            Type = Vehicle.TYPE_NONE;
         }
 
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
+        // Return 'true' if this vehicle is doing vehicle things
+        public bool IsActive
         {
-            DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            get { return Type != Vehicle.TYPE_NONE; }
+        }
+
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        {
+            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -229,9 +231,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }//end ProcessFloatVehicleParam
 
-        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
-            DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -266,7 +268,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
         {
-            DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
@@ -280,166 +282,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
-            DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
+            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", Prim.LocalID, pParam, remove);
+            VehicleFlag parm = (VehicleFlag)pParam;
             if (remove)
             {
                 if (pParam == -1)
                 {
                     m_flags = (VehicleFlag)0;
-                    m_Hoverflags = (VehicleFlag)0;
-                    return;
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
-                {
-                    if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY)
-                {
-                    if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
-                {
-                    if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.MOUSELOOK_BANK);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
-                {
-                    if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.MOUSELOOK_STEER);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
-                {
-                    if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP);
-                }
-                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
-                {
-                    if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED);
-                }
-                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
-                {
-                    if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_X);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
-                {
-                    if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_Y);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
-                {
-                    if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_Z);
                 }
-                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
-                {
-                    if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
-                {
-                    if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_DEFLECTION);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
+                else
                 {
-                    if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LOCK_ROTATION);
+                    m_flags &= ~parm;
                 }
             }
-            else
-            {
-                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
-                {
-                    m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
-                {
-                    m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
-                {
-                    m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
-                {
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
-                {
-                    m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
-                {
-                    m_flags |= (VehicleFlag.NO_X);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
-                {
-                    m_flags |= (VehicleFlag.NO_Y);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
-                {
-                    m_flags |= (VehicleFlag.NO_Z);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
-                {
-                    m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
-                {
-                    m_flags |= (VehicleFlag.NO_DEFLECTION);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
-                {
-                    m_flags |= (VehicleFlag.LOCK_ROTATION);
-                }
+            else {
+                m_flags |= parm;
             }
         }//end ProcessVehicleFlags
 
         internal void ProcessTypeChange(Vehicle pType)
         {
-            DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
+            VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
             // Set Defaults For Type
-            m_type = pType;
+            Type = pType;
             switch (pType)
             {
                     case Vehicle.TYPE_NONE:
@@ -478,10 +343,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 10;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &=
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags &=
                          ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
                     break;
                 case Vehicle.TYPE_CAR:
                     m_linearFrictionTimescale = new Vector3(100, 2, 1000);
@@ -506,10 +371,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
                                 VehicleFlag.LIMIT_MOTOR_UP);
-                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.HOVER_UP_ONLY);
                     break;
                 case Vehicle.TYPE_BOAT:
                     m_linearFrictionTimescale = new Vector3(10, 3, 2);
@@ -534,12 +399,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.8f;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
                             VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                                 VehicleFlag.LIMIT_MOTOR_UP);
-                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY);
+                    m_flags |= (VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
@@ -564,7 +429,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 2;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                         VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
@@ -592,50 +457,63 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 5;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                         VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
         }//end SetDefaultsForType
 
-        internal void Step(float pTimestep)
+        // Some of the properties of this prim may have changed.
+        // Do any updating needed for a vehicle
+        public void Refresh()
         {
-            if (m_type == Vehicle.TYPE_NONE) return;
+            if (Type == Vehicle.TYPE_NONE) return;
+
+            // Set the prim's inertia to zero. The vehicle code handles that and this
+            //    removes the torque action introduced by Bullet.
+            Vector3 inertia = Vector3.Zero;
+            BulletSimAPI.SetMassProps2(Prim.BSBody.ptr, Prim.MassRaw, inertia);
+            BulletSimAPI.UpdateInertiaTensor2(Prim.BSBody.ptr);
+        }
 
-            frcount++;  // used to limit debug comment output
-            if (frcount > 100)
-                frcount = 0;
+        // One step of the vehicle properties for the next 'pTimestep' seconds.
+        internal void Step(float pTimestep)
+        {
+            if (!IsActive) return;
 
             MoveLinear(pTimestep);
             MoveAngular(pTimestep);
             LimitRotation(pTimestep);
 
-            DetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 
-                    m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
+            // remember the position so next step we can limit absolute movement effects
+            m_lastPositionVector = Prim.Position;
+
+            VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
+                    Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
         }// end Step
 
+        // Apply the effect of the linear motor.
+        // Also does hover and float.
         private void MoveLinear(float pTimestep)
         {
-            // requested m_linearMotorDirection is significant
-            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
-            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
+            // m_linearMotorDirection is the direction we are moving relative to the vehicle coordinates
+            // m_lastLinearVelocityVector is the speed we are moving in that direction
+            if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
                 Vector3 origDir = m_linearMotorDirection;
                 Vector3 origVel = m_lastLinearVelocityVector;
 
                 // add drive to body
-                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
-                // lastLinearVelocityVector is the current body velocity vector?
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale / pTimestep);
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale / pTimestep);
+                // lastLinearVelocityVector is the current body velocity vector
                 // RA: Not sure what the *10 is for. A correction for pTimestep?
-                // m_lastLinearVelocityVector += (addAmount*10);  
-                m_lastLinearVelocityVector += addAmount;  
+                // m_lastLinearVelocityVector += (addAmount*10);
+                m_lastLinearVelocityVector += addAmount;
 
-                // This will work temporarily, but we really need to compare speed on an axis
-                // KF: Limit body velocity to applied velocity?
                 // Limit the velocity vector to less than the last set linear motor direction
                 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                     m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
@@ -644,124 +522,87 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                     m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
 
+                /*
                 // decay applied velocity
-                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
+                Vector3 decayfraction = Vector3.One/(m_linearMotorDecayTimescale / pTimestep);
+                // (RA: do not know where the 0.5f comes from)
                 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-
-                /*
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
-                m_lastLinearVelocityVector += addAmount;
-
-                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
-                m_linearMotorDirection *= decayfraction;
-
                  */
+                float keepfraction = 1.0f - (1.0f / (m_linearMotorDecayTimescale / pTimestep));
+                m_linearMotorDirection *= keepfraction;
 
-                DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
-                    m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
+                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},notDecay={4},dir={5},vel={6}",
+                    Prim.LocalID, origDir, origVel, addAmount, keepfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
             }
             else
             {
-                // if what remains of applied is small, zero it.
-                // if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
-                //     m_lastLinearVelocityVector = Vector3.Zero;
+                // if what remains of direction is very small, zero it.
                 m_linearMotorDirection = Vector3.Zero;
                 m_lastLinearVelocityVector = Vector3.Zero;
+                VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
             }
 
-            // convert requested object velocity to world-referenced vector
-            Quaternion rotq = m_prim.Orientation;
-            m_dir = m_lastLinearVelocityVector * rotq;
+            // convert requested object velocity to object relative vector
+            Quaternion rotq = Prim.Orientation;
+            m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
 
             // add Gravity and Buoyancy
-            // KF: So far I have found no good method to combine a script-requested
-            // .Z velocity and gravity. Therefore only 0g will used script-requested
-            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
-            Vector3 grav = Vector3.Zero;
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Mass * (1f - m_VehicleBuoyancy));
+
+            /*
+             * RA: Not sure why one would do this
             // Preserve the current Z velocity
             Vector3 vel_now = m_prim.Velocity;
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
+             */
 
-            Vector3 pos = m_prim.Position;
-            Vector3 posChange = pos;
+            Vector3 pos = Prim.Position;
 //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
-            double Zchange = Math.Abs(posChange.Z);
-            if (m_BlockingEndPoint != Vector3.Zero)
-            {
-                bool changed = false;
-                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
-                {
-                    pos.X -= posChange.X + 1;
-                    changed = true;
-                }
-                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
-                {
-                    pos.Y -= posChange.Y + 1;
-                    changed = true;
-                }
-                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
-                {
-                    pos.Z -= posChange.Z + 1;
-                    changed = true;
-                }
-                if (pos.X <= 0)
-                {
-                    pos.X += posChange.X + 1;
-                    changed = true;
-                }
-                if (pos.Y <= 0)
-                {
-                    pos.Y += posChange.Y + 1;
-                    changed = true;
-                }
-                if (changed)
-                {
-                    m_prim.Position = pos;
-                    DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
-                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
-                }
-            }
 
             // If below the terrain, move us above the ground a little.
-            if (pos.Z < m_prim.Scene.GetTerrainHeightAtXYZ(pos))
+            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+            // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
+            //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
+            // Vector3 rotatedSize = m_prim.Size * m_prim.Orientation;
+            // if (rotatedSize.Z < terrainHeight)
+            if (pos.Z < terrainHeight)
             {
-                pos.Z = m_prim.Scene.GetTerrainHeightAtXYZ(pos) + 2;
-                m_prim.Position = pos;
-                DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
+                pos.Z = terrainHeight + 2;
+                Prim.Position = pos;
+                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
 
             // Check if hovering
-            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
+            if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
             {
                 // We should hover, get the target height
-                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
                 }
-                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
                 }
-                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
                     m_VhoverTargetHeight = m_VhoverHeight;
                 }
 
-                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
                     // If body is aready heigher, use its height as target height
                     if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
                 }
-                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
+                if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
                     if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                     {
-                        m_prim.Position = pos;
+                        Prim.Position = pos;
                     }
                 }
                 else
@@ -770,85 +611,96 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Replace Vertical speed with correction figure if significant
                     if (Math.Abs(herr0) > 0.01f)
                     {
-                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                        m_newVelocity.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                         //KF: m_VhoverEfficiency is not yet implemented
                     }
                     else
                     {
-                        m_dir.Z = 0f;
+                        m_newVelocity.Z = 0f;
                     }
                 }
 
-                DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
+            }
 
-//                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
-//                m_VhoverTimescale = 0f;        // time to acheive height
-//                pTimestep  is time since last frame,in secs
+            Vector3 posChange = pos - m_lastPositionVector;
+            if (m_BlockingEndPoint != Vector3.Zero)
+            {
+                bool changed = false;
+                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
+                {
+                    pos.X -= posChange.X + 1;
+                    changed = true;
+                }
+                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
+                {
+                    pos.Y -= posChange.Y + 1;
+                    changed = true;
+                }
+                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
+                {
+                    pos.Z -= posChange.Z + 1;
+                    changed = true;
+                }
+                if (pos.X <= 0)
+                {
+                    pos.X += posChange.X + 1;
+                    changed = true;
+                }
+                if (pos.Y <= 0)
+                {
+                    pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
+                    Prim.Position = pos;
+                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                Prim.LocalID, m_BlockingEndPoint, posChange, pos);
+                }
             }
 
+            float Zchange = Math.Abs(posChange.Z);
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                //Start Experimental Values
                 if (Zchange > .3)
-                {
                     grav.Z = (float)(grav.Z * 3);
-                }
                 if (Zchange > .15)
-                {
                     grav.Z = (float)(grav.Z * 2);
-                }
                 if (Zchange > .75)
-                {
                     grav.Z = (float)(grav.Z * 1.5);
-                }
                 if (Zchange > .05)
-                {
                     grav.Z = (float)(grav.Z * 1.25);
-                }
                 if (Zchange > .025)
-                {
                     grav.Z = (float)(grav.Z * 1.125);
-                }
-                float terraintemp = m_prim.Scene.GetTerrainHeightAtXYZ(pos);
-                float postemp = (pos.Z - terraintemp);
+                float postemp = (pos.Z - terrainHeight);
                 if (postemp > 2.5f)
-                {
                     grav.Z = (float)(grav.Z * 1.037125);
-                }
-                DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
-                //End Experimental Values
+                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
             }
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
-            {
-                m_dir.X = 0;
-            }
+                m_newVelocity.X = 0;
             if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-            {
-                m_dir.Y = 0;
-            }
+                m_newVelocity.Y = 0;
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-            {
-                m_dir.Z = 0;
-            }
-
-            m_lastPositionVector = m_prim.Position;
+                m_newVelocity.Z = 0;
 
             // Apply velocity
-            m_prim.Velocity = m_dir;
+            Prim.Velocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
             // m_prim.AddForce(grav, false);
-            // m_prim.Force = grav;
 
             // Apply friction
-            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
-            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
+            m_lastLinearVelocityVector *= keepFraction;
 
-            DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
-                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4},1Mdecay={5}",
+                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav, keepFraction);
 
         } // end MoveLinear()
 
+        // Apply the effect of the angular motor.
         private void MoveAngular(float pTimestep)
         {
             // m_angularMotorDirection         // angular velocity requested by LSL motor
@@ -860,7 +712,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastAngularVelocity           // what was last applied to body
 
             // Get what the body is doing, this includes 'external' influences
-            Vector3 angularVelocity = m_prim.RotationalVelocity;
+            Vector3 angularVelocity = Prim.RotationalVelocity;
 
             if (m_angularMotorApply > 0)
             {
@@ -870,32 +722,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // There are m_angularMotorApply steps.
                 Vector3 origAngularVelocity = m_angularMotorVelocity;
                 // ramp up to new value
-                //   current velocity  +=                         error                       /    (time to get there / step interval)
-                //                               requested speed            -  last motor speed
+                //   current velocity    +=                         error                          /    (  time to get there   / step interval)
+                //                               requested speed       -       last motor speed
                 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
 
-                DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}", 
-                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},dir={5},vel={6}",
+                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
 
-                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
-                                            // velocity may still be acheived.
+                // This is done so that if script request rate is less than phys frame rate the expected
+                //    velocity may still be acheived.
+                m_angularMotorApply--;
             }
             else
             {
                 // No motor recently applied, keep the body velocity
                 // and decay the velocity
                 m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
+                if (m_angularMotorVelocity.LengthSquared() < 0.00001)
+                    m_angularMotorVelocity = Vector3.Zero;
             } // end motor section
 
             // Vertical attractor section
             Vector3 vertattr = Vector3.Zero;
             if (m_verticalAttractionTimescale < 300)
             {
-                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
+                float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
                 // get present body rotation
-                Quaternion rotq = m_prim.Orientation;
+                Quaternion rotq = Prim.Orientation;
                 // make a vector pointing up
                 Vector3 verterr = Vector3.Zero;
                 verterr.Z = 1.0f;
@@ -924,8 +779,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
-                DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
-                            m_prim.LocalID, verterr, bounce, vertattr);
+                VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
+                            Prim.LocalID, verterr, bounce, vertattr);
 
             } // else vertical attractor is off
 
@@ -937,18 +792,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             // Sum velocities
             m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
-            
+         
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 m_lastAngularVelocity.X = 0;
                 m_lastAngularVelocity.Y = 0;
-                DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
             }
 
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
             }
 
              // apply friction
@@ -956,14 +811,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
 
             // Apply to the body
-            m_prim.RotationalVelocity = m_lastAngularVelocity;
+            Prim.RotationalVelocity = m_lastAngularVelocity;
 
-            DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
+            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = m_prim.Orientation;
+            Quaternion rotq = Prim.Orientation;
             Quaternion m_rot = rotq;
             bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
@@ -996,23 +851,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_rot.Y = 0;
                 changed = true;
             }
-            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
+            if (changed)
             {
-                m_rot.X = 0;
-                m_rot.Y = 0;
-                changed = true;
+                Prim.Orientation = m_rot;
+                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }
-            if (changed)
-                m_prim.Orientation = m_rot;
 
-            DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
         }
 
         // Invoke the detailed logger and output something if it's enabled.
-        private void DetailLog(string msg, params Object[] args)
+        private void VDetailLog(string msg, params Object[] args)
         {
-            if (m_prim.Scene.VehicleLoggingEnabled)
-                m_prim.Scene.PhysicsLogging.Write(msg, args);
+            if (Prim.PhysicsScene.VehicleLoggingEnabled)
+                Prim.PhysicsScene.PhysicsLogging.Write(msg, args);
         }
     }
 }