1 files changed, 297 insertions, 207 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 3a73fba..be8a502 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -45,9 +45,7 @@ using System;
 using System.Collections.Generic;
 using System.Reflection;
 using System.Runtime.InteropServices;
-using log4net;
 using OpenMetaverse;
-using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -100,7 +98,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_angularMotorTimescale = 0;                      // motor angular velocity ramp up rate
         private float m_angularMotorDecayTimescale = 0;                 // motor angular velocity decay rate
         private Vector3 m_angularFrictionTimescale = Vector3.Zero;      // body angular velocity  decay rate
-        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
+        private Vector3 m_lastAngularCorrection = Vector3.Zero;
         private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
@@ -113,6 +111,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_bankingEfficiency = 0;
         private float m_bankingMix = 0;
         private float m_bankingTimescale = 0;
+        private Vector3 m_lastBanking = Vector3.Zero;
 
         //Hover and Buoyancy properties
         private float m_VhoverHeight = 0f;
@@ -127,7 +126,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //Attractor properties
         private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
-        private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
+        private float m_verticalAttractionTimescale = 600f;         // Timescale > 500  means no vert attractor.
 
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
@@ -154,7 +153,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
+                    m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
                     m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
                     break;
                 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
@@ -162,7 +161,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularMotor.TimeScale = m_angularMotorTimescale;
                     break;
                 case Vehicle.BANKING_EFFICIENCY:
-                    m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f));
+                    m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
                     break;
                 case Vehicle.BANKING_MIX:
                     m_bankingMix = Math.Max(pValue, 0.01f);
@@ -171,10 +170,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BUOYANCY:
-                    m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f));
+                    m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
                     break;
                 case Vehicle.HOVER_EFFICIENCY:
-                    m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f));
+                    m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
                     break;
                 case Vehicle.HOVER_HEIGHT:
                     m_VhoverHeight = pValue;
@@ -189,7 +188,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
+                    m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
                     m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_TIMESCALE:
@@ -197,7 +196,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearMotor.TimeScale = m_linearMotorTimescale;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
-                    m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
+                    m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
                     m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
@@ -242,9 +241,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     // Limit requested angular speed to 2 rps= 4 pi rads/sec
-                    pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f));
-                    pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
-                    pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
+                    pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
+                    pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
+                    pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
                     m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     m_angularMotor.SetTarget(m_angularMotorDirection);
                     break;
@@ -330,6 +329,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = 0;
                     m_bankingTimescale = 1000;
                     m_bankingMix = 1;
+                    m_lastBanking = Vector3.Zero;
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags = (VehicleFlag)0;
@@ -364,6 +364,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = 0;
                     m_bankingTimescale = 10;
                     m_bankingMix = 1;
+                    m_lastBanking = Vector3.Zero;
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -402,6 +403,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = -0.2f;
                     m_bankingMix = 1;
                     m_bankingTimescale = 1;
+                    m_lastBanking = Vector3.Zero;
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -440,14 +442,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = -0.3f;
                     m_bankingMix = 0.8f;
                     m_bankingTimescale = 1;
+                    m_lastBanking = Vector3.Zero;
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT
                                     | VehicleFlag.LIMIT_ROLL_ONLY
-                                    | VehicleFlag.LIMIT_MOTOR_UP
                                     | VehicleFlag.HOVER_UP_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP
                                     | VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
@@ -478,6 +481,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = 1;
                     m_bankingMix = 0.7f;
                     m_bankingTimescale = 2;
+                    m_lastBanking = Vector3.Zero;
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -516,6 +520,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingEfficiency = 0;
                     m_bankingMix = 0.7f;
                     m_bankingTimescale = 5;
+                    m_lastBanking = Vector3.Zero;
+
                     m_referenceFrame = Quaternion.Identity;
 
                     m_referenceFrame = Quaternion.Identity;
@@ -558,9 +564,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (IsActive)
             {
+                // Remember the mass so we don't have to fetch it every step
                 m_vehicleMass = Prim.Linkset.LinksetMass;
 
-                // Friction effects are handled by this vehicle code
+                // Friction affects are handled by this vehicle code
                 float friction = 0f;
                 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
 
@@ -574,6 +581,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Vector3 localInertia = new Vector3(1f, 1f, 1f);
                 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
                 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
+                BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
 
                 VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
                                 Prim.LocalID, friction, localInertia, angularDamping);
@@ -598,31 +606,167 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Refresh();
         }
 
+        #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.
+        //      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 Quaternion? m_knownOrientation;
+        private Vector3? m_knownRotationalVelocity;
+
+        private const int m_knownChangedPosition           = 1 << 0;
+        private const int m_knownChangedVelocity           = 1 << 1;
+        private const int m_knownChangedOrientation        = 1 << 2;
+        private const int m_knownChangedRotationalVelocity = 1 << 3;
+
+        private void ForgetKnownVehicleProperties()
+        {
+            m_knownTerrainHeight = null;
+            m_knownWaterLevel = null;
+            m_knownPosition = null;
+            m_knownVelocity = null;
+            m_knownOrientation = null;
+            m_knownRotationalVelocity = null;
+            m_knownChanged = 0;
+        }
+        private void PushKnownChanged()
+        {
+            if (m_knownChanged != 0)
+            {
+                if ((m_knownChanged & m_knownChangedPosition) != 0)
+                    Prim.ForcePosition = VehiclePosition;
+                if ((m_knownChanged & m_knownChangedOrientation) != 0)
+                    Prim.ForceOrientation = VehicleOrientation;
+                if ((m_knownChanged & m_knownChangedVelocity) != 0)
+                    Prim.ForceVelocity = VehicleVelocity;
+                if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
+                {
+                    Prim.ForceRotationalVelocity = VehicleRotationalVelocity;
+                    BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, VehicleRotationalVelocity);
+                }
+                // If we set one of the values (ie, the physics engine didn't do it) we must force
+                //      an UpdateProperties event to send the changes up to the simulator.
+                BulletSimAPI.PushUpdate2(Prim.PhysBody.ptr);
+            }
+        }
+
+        // Since the computation of terrain height can be a little involved, this routine
+        //    is used ot fetch the height only once for each vehicle simulation step.
+        private float GetTerrainHeight(Vector3 pos)
+        {
+            if (m_knownTerrainHeight == null)
+                m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+            return (float)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)
+                m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+            return (float)m_knownWaterLevel;
+        }
+
+        private Vector3 VehiclePosition
+        {
+            get
+            {
+                if (m_knownPosition == null)
+                    m_knownPosition = Prim.ForcePosition;
+                return (Vector3)m_knownPosition;
+            }
+            set
+            {
+                m_knownPosition = value;
+                m_knownChanged |= m_knownChangedPosition;
+            }
+        }
+
+        private Quaternion VehicleOrientation
+        {
+            get
+            {
+                if (m_knownOrientation == null)
+                    m_knownOrientation = Prim.ForceOrientation;
+                return (Quaternion)m_knownOrientation;
+            }
+            set
+            {
+                m_knownOrientation = value;
+                m_knownChanged |= m_knownChangedOrientation;
+            }
+        }
+
+        private Vector3 VehicleVelocity
+        {
+            get
+            {
+                if (m_knownVelocity == null)
+                    m_knownVelocity = Prim.ForceVelocity;
+                return (Vector3)m_knownVelocity;
+            }
+            set
+            {
+                m_knownVelocity = value;
+                m_knownChanged |= m_knownChangedVelocity;
+            }
+        }
+
+        private Vector3 VehicleRotationalVelocity
+        {
+            get
+            {
+                if (m_knownRotationalVelocity == null)
+                    m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                return (Vector3)m_knownRotationalVelocity;
+            }
+            set
+            {
+                m_knownRotationalVelocity = value;
+                m_knownChanged |= m_knownChangedRotationalVelocity;
+            }
+        }
+        #endregion // Known vehicle value functions
+
         // One step of the vehicle properties for the next 'pTimestep' seconds.
         internal void Step(float pTimestep)
         {
             if (!IsActive) return;
 
+            ForgetKnownVehicleProperties();
+
             MoveLinear(pTimestep);
             MoveAngular(pTimestep);
 
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = Prim.ForcePosition;
+            m_lastPositionVector = VehiclePosition;
+
+            // If we forced the changing of some vehicle parameters, update the values and
+            //      for the physics engine to note the changes so an UpdateProperties event will happen.
+            PushKnownChanged();
 
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
+                    Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
         }
 
-        // Apply the effect of the linear motor.
-        // Also does hover and float.
+        // Apply the effect of the linear motor and other linear motions (like hover and float).
         private void MoveLinear(float pTimestep)
         {
             Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
 
-            // Rotate new object velocity from vehicle relative to world coordinates
-            linearMotorContribution *= Prim.ForceOrientation;
+            // The movement computed in the linear motor is relative to the vehicle
+            //     coordinates. Rotate the movement to world coordinates.
+            linearMotorContribution *= VehicleOrientation;
 
             // ==================================================================
             // Gravity and Buoyancy
@@ -630,16 +774,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
             Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
 
-            Vector3 pos = Prim.ForcePosition;
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-
-            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
 
-            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep);
 
-            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
+            ComputeLinearBlockingEndPoint(pTimestep);
 
-            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
 
             // ==================================================================
             Vector3 newVelocity = linearMotorContribution
@@ -667,42 +808,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 newVelocity = Vector3.Zero;
 
             // ==================================================================
-            // Stuff new linear velocity into the vehicle
-            Prim.ForceVelocity = newVelocity;
-            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
+            // Stuff new linear velocity into the vehicle.
+            // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
+            VehicleVelocity = newVelocity;
 
             // Other linear forces are applied as forces.
-            Vector3 totalDownForce = grav * m_vehicleMass;
+            Vector3 totalDownForce = grav * m_vehicleMass * pTimestep;
             if (totalDownForce != Vector3.Zero)
             {
                 Prim.AddForce(totalDownForce, false);
             }
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
-                                newVelocity, Prim.Velocity, totalDownForce);
+            VDetailLog("{0},  MoveLinear,done,newVel={1},totDown={2},linContrib={3},terrContrib={4},hoverContrib={5},limitContrib={6}",
+                                Prim.LocalID, newVelocity, totalDownForce,
+                                linearMotorContribution, terrainHeightContribution, hoverContribution, limitMotorUpContribution
+            );
 
         } // end MoveLinear()
 
-        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
         {
             Vector3 ret = Vector3.Zero;
             // If below the terrain, move us above the ground a little.
-            // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
-            // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
-            // if (rotatedSize.Z < terrainHeight)
-            if (pos.Z < terrainHeight)
+            // TODO: Consider taking the rotated size of the object or possibly casting a ray.
+            if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
             {
                 // TODO: correct position by applying force rather than forcing position.
-                pos.Z = terrainHeight + 2;
-                Prim.ForcePosition = pos;
-                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
+                VehiclePosition += new Vector3(0f, 0f, GetTerrainHeight(VehiclePosition) + 2f);
+                VDetailLog("{0},  MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
             }
             return ret;
         }
 
-        public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearHover(float pTimestep)
         {
             Vector3 ret = Vector3.Zero;
 
@@ -713,11 +851,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
@@ -727,46 +865,44 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
                     // If body is already heigher, use its height as target height
-                    if (pos.Z > m_VhoverTargetHeight)
-                        m_VhoverTargetHeight = pos.Z;
+                    if (VehiclePosition.Z > m_VhoverTargetHeight)
+                        m_VhoverTargetHeight = VehiclePosition.Z;
                 }
                 
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
-                    if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f)
+                    if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
                     {
+                        Vector3 pos = VehiclePosition;
                         pos.Z = m_VhoverTargetHeight;
-                        Prim.ForcePosition = pos;
+                        VehiclePosition = pos;
                     }
                 }
                 else
                 {
-                    float verticalError = pos.Z - m_VhoverTargetHeight;
-                    // RA: where does the 50 come from?
-                    float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
-                    // Replace Vertical speed with correction figure if significant
-                    if (verticalError > 0.01f)
+                    // Error is positive if below the target and negative if above.
+                    float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
+                    float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
+
+                    // TODO: implement m_VhoverEfficiency correctly
+                    if (Math.Abs(verticalError) > m_VhoverEfficiency)
                     {
                         ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
-                        //KF: m_VhoverEfficiency is not yet implemented
-                    }
-                    else if (verticalError < -0.01)
-                    {
-                        ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
                     }
                 }
 
-                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}",
-                                Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},  MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
+                                Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
             }
 
             return ret;
         }
 
-        public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos)
+        public bool ComputeLinearBlockingEndPoint(float pTimestep)
         {
             bool changed = false;
 
+            Vector3 pos = VehiclePosition;
             Vector3 posChange = pos - m_lastPositionVector;
             if (m_BlockingEndPoint != Vector3.Zero)
             {
@@ -797,8 +933,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 if (changed)
                 {
-                    Prim.ForcePosition = pos;
-                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                    VehiclePosition = pos;
+                    VDetailLog("{0},  MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
@@ -812,13 +948,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
         //    when they are in mid jump. 
         // TODO: this code is wrong. Also, what should it do for boats?
-        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearMotorUp(float pTimestep)
         {
             Vector3 ret = Vector3.Zero;
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
                 // If the vehicle is motoring into the sky, get it going back down.
-                float distanceAboveGround = pos.Z - terrainHeight;
+                // float distanceAboveGround = pos.Z - Math.Max(GetTerrainHeight(pos), GetWaterLevel(pos));
+                float distanceAboveGround = VehiclePosition.Z - GetTerrainHeight(VehiclePosition);
                 if (distanceAboveGround > 1f)
                 {
                     // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
@@ -830,7 +967,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //     has a decay factor. This says this force should
                 //     be computed with a motor.
                 // TODO: add interaction with banking.
-                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
+                VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
                                     Prim.LocalID, distanceAboveGround, ret);
             }
             return ret;
@@ -839,36 +976,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // =======================================================================
         // =======================================================================
         // Apply the effect of the angular motor.
+        // The 'contribution' is how much angular correction velocity each function wants.
+        //     All the contributions are added together and the orientation of the vehicle
+        //     is changed by all the contributed corrections.
         private void MoveAngular(float pTimestep)
         {
-            // m_angularMotorDirection         // angular velocity requested by LSL motor
-            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-            // m_angularMotorTimescale         // motor angular velocity ramp up time
-            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
-            // m_angularFrictionTimescale      // body angular velocity  decay rate
-            // m_lastAngularVelocity           // what was last applied to body
-
-            /*
-            if (m_angularMotorDirection.LengthSquared() > 0.0001)
-            {
-                Vector3 origVel = m_angularMotorVelocity;
-                Vector3 origDir = m_angularMotorDirection;
-
-                //       new velocity    +=                      error                       /  (  time to get there   / step interval)
-                //                           requested direction   - current vehicle direction
-                m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) /  (m_angularMotorTimescale / pTimestep);
-                // decay requested direction
-                m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
-
-                VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
-                        Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
-            }
-            else
-            {
-                m_angularMotorVelocity = Vector3.Zero;
-            }
-            */
-
+            // The user wants how many radians per second angular change?
             Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
 
             // ==================================================================
@@ -881,25 +994,47 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 angularMotorContribution.X = 0f;
                 angularMotorContribution.Y = 0f;
-                VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
+                VDetailLog("{0},  MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
             }
 
-            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
+            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
 
-            Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
+            Vector3 deflectionContribution = ComputeAngularDeflection();
 
-            Vector3 bankingContribution = ComputeAngularBanking(pTimestep);
+            Vector3 bankingContribution = ComputeAngularBanking(angularMotorContribution.Z);
 
             // ==================================================================
             m_lastVertAttractor = verticalAttractionContribution;
 
-            // Sum velocities
-            m_lastAngularVelocity = angularMotorContribution
+            // Sum corrections
+            m_lastAngularCorrection = angularMotorContribution
                                     + verticalAttractionContribution
                                     + deflectionContribution
                                     + bankingContribution;
 
             // ==================================================================
+            // The correction is applied to the current orientation.
+            if (!m_lastAngularCorrection.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                Vector3 scaledCorrection = m_lastAngularCorrection * pTimestep;
+
+                VehicleRotationalVelocity = scaledCorrection;
+
+                VDetailLog("{0},  MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5},scaledCorr={6}",
+                                    Prim.LocalID,
+                                    angularMotorContribution, verticalAttractionContribution,
+                                    bankingContribution, deflectionContribution,
+                                    m_lastAngularCorrection, scaledCorrection
+                                    );
+            }
+            else
+            {
+                // The vehicle is not adding anything velocity wise.
+                VehicleRotationalVelocity = Vector3.Zero;
+                VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
+            }
+
+            // ==================================================================
             //Offset section
             if (m_linearMotorOffset != Vector3.Zero)
             {
@@ -927,53 +1062,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     torqueFromOffset.Z = 0;
                 torqueFromOffset *= m_vehicleMass;
                 Prim.ApplyTorqueImpulse(torqueFromOffset, true);
-                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
             }
 
-            // ==================================================================
-            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
-            {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                // TODO: zeroing is good but it also sets values in unmanaged code. Remove the stores when idle.
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
-                Prim.ZeroAngularMotion(true);
-            }
-            else
-            {
-                // Apply to the body.
-                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
-                // Since we are stuffing the angular velocity directly into the object, the computed
-                //     velocity needs to be scaled by the timestep.
-                // Also remove any motion that is on the object so added motion is only from vehicle.
-                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
-                                                - Prim.ForceRotationalVelocity);
-                // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
-                Prim.ForceRotationalVelocity = applyAngularForce;
-
-                VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
-                                    Prim.LocalID,
-                                    angularMotorContribution, verticalAttractionContribution,
-                                    bankingContribution, deflectionContribution,
-                                    applyAngularForce, m_lastAngularVelocity
-                                    );
-            }
         }
 
-        public Vector3 ComputeAngularVerticalAttraction(float pTimestep)
+        public Vector3 ComputeAngularVerticalAttraction()
         {
             Vector3 ret = Vector3.Zero;
 
             // If vertical attaction timescale is reasonable and we applied an angular force last time...
             if (m_verticalAttractionTimescale < 500)
             {
-                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
-                verticalError.Normalize();
-                m_verticalAttractionMotor.SetCurrent(verticalError);
-                m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
-                ret = m_verticalAttractionMotor.Step(pTimestep);
-                /*
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
-                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
+                Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
                 verticalError.Normalize();
 
                 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
@@ -991,63 +1093,70 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
 
                 // Y error means needed rotation around X axis and visa versa.
-                verticalAttractionContribution.X =    verticalError.Y;
-                verticalAttractionContribution.Y =  - verticalError.X;
-                verticalAttractionContribution.Z = 0f;
+                ret.X =    verticalError.Y;
+                ret.Y =  - verticalError.X;
+                ret.Z = 0f;
 
                 // scale by the time scale and timestep
-                Vector3 unscaledContrib = verticalAttractionContribution;
-                verticalAttractionContribution /= m_verticalAttractionTimescale;
-                verticalAttractionContribution *= pTimestep;
+                Vector3 unscaledContrib = ret;
+                ret /= m_verticalAttractionTimescale;
+                // This returns the angular correction desired. Timestep is added later.
+                // ret *= pTimestep;
 
                 // apply efficiency
-                Vector3 preEfficiencyContrib = verticalAttractionContribution;
+                Vector3 preEfficiencyContrib = ret;
+                // TODO: implement efficiency.
+                // Effenciency squared seems to give a more realistic effect
                 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
-                verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+                // ret *= efficencySquared;
 
-                VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
                                             Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
                                             m_verticalAttractionEfficiency, efficencySquared,
-                                            verticalAttractionContribution);
-                 */
-
+                                            ret);
             }
             return ret;
         }
 
-        public Vector3 ComputeAngularDeflection(float pTimestep)
+        // Return the angular correction to correct the direction the vehicle is pointing to be
+        //      the direction is should want to be pointing.
+        public Vector3 ComputeAngularDeflection()
         {
             Vector3 ret = Vector3.Zero;
 
             if (m_angularDeflectionEfficiency != 0)
             {
-                // Compute a scaled vector that points in the preferred axis (X direction)
-                Vector3 scaledDefaultDirection =
-                    new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
-                // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
-                // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
-                Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
+                // Where the vehicle should want to point relative to the vehicle
+                Vector3 preferredDirection = Vector3.UnitX * m_referenceFrame;
 
-                // Scale by efficiency and timescale
-                ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+                // Where the vehicle is pointing relative to the vehicle.
+                Vector3 currentDirection = Vector3.UnitX * Quaternion.Add(VehicleOrientation, m_referenceFrame);
 
-                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret);
+                // Difference between where vehicle is pointing and where it should wish to point
+                Vector3 directionCorrection = preferredDirection - currentDirection;
 
-                // This deflection computation is not correct.
-                ret = Vector3.Zero;
+                // Scale the correction by recovery timescale and efficiency
+                ret = directionCorrection * m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                VDetailLog("{0},  MoveAngular,Deflection,perfDir={1},currentDir={2},dirCorrection={3},ret={4}",
+                    Prim.LocalID, preferredDirection, currentDirection, directionCorrection, ret);
             }
             return ret;
         }
 
-        public Vector3 ComputeAngularBanking(float pTimestep)
+        // Return an angular change to tip the vehicle (around X axis) when turning (turned around Z).
+        // Remembers the last banking value calculated and returns the difference needed this tick.
+        // TurningFactor is rate going left or right (pos=left, neg=right, scale=0..1).
+        public Vector3 ComputeAngularBanking(float turningFactor)
         {
             Vector3 ret = Vector3.Zero;
+            Vector3 computedBanking = Vector3.Zero;
 
             if (m_bankingEfficiency != 0)
             {
-                Vector3 dir = Vector3.One * Prim.ForceOrientation;
+                Vector3 currentDirection = Vector3.UnitX * VehicleOrientation;
+
                 float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
-                //Changes which way it banks in and out of turns
 
                 //Use the square of the efficiency, as it looks much more how SL banking works
                 float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
@@ -1055,58 +1164,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     effSquared *= -1; //Keep the negative!
 
                 float mix = Math.Abs(m_bankingMix);
-                if (m_angularMotorVelocity.X == 0)
-                {
-                    // The vehicle is stopped
-                    /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
-                    {
-                        Vector3 axisAngle;
-                        float angle;
-                        parent.Orientation.GetAxisAngle(out axisAngle, out angle);
-                        Vector3 rotatedVel = parent.Velocity * parent.Orientation;
-                        if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
-                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
-                        else
-                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
-                    }*/
-                }
-                else
-                {
-                    ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
-                }
+                // TODO: Must include reference frame.
+                float forwardSpeed = VehicleVelocity.X;
 
-                //If they are colliding, we probably shouldn't shove the prim around... probably
-                if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
+                if (!Prim.IsColliding && forwardSpeed > mix)
                 {
-                    float angVelZ = m_angularMotorVelocity.X * -1;
-                    /*if(angVelZ > mix)
-                        angVelZ = mix;
-                    else if(angVelZ < -mix)
-                        angVelZ = -mix;*/
-                    //This controls how fast and how far the banking occurs
-                    Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
-                    if (bankingRot.X > 3)
-                        bankingRot.X = 3;
-                    else if (bankingRot.X < -3)
-                        bankingRot.X = -3;
-                    bankingRot *= Prim.ForceOrientation;
-                    ret += bankingRot;
+                    computedBanking.X = ClampInRange(-3f, turningFactor * (effSquared * mult), 3f);
                 }
-                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
-                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}",
-                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, ret);
+
+                // 'computedBanking' is now how much banking that should be happening.
+                ret = computedBanking - m_lastBanking;
+
+                // Scale the correction by timescale and efficiency
+                ret /= m_bankingTimescale * m_bankingEfficiency;
+
+                VDetailLog("{0},  MoveAngular,Banking,computedB={1},lastB={2},bEff={3},effSq={4},mult={5},mix={6},banking={7}",
+                                Prim.LocalID, computedBanking, m_lastBanking, m_bankingEfficiency, effSquared, mult, mix, ret);
             }
+            m_lastBanking = computedBanking;
             return ret;
         }
 
-
         // This is from previous instantiations of XXXDynamics.cs.
         // Applies roll reference frame.
         // TODO: is this the right way to separate the code to do this operation?
         //    Should this be in MoveAngular()?
         internal void LimitRotation(float timestep)
         {
-            Quaternion rotq = Prim.ForceOrientation;
+            Quaternion rotq = VehicleOrientation;
             Quaternion m_rot = rotq;
             if (m_RollreferenceFrame != Quaternion.Identity)
             {
@@ -1134,12 +1219,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
             if (rotq != m_rot)
             {
-                Prim.ForceOrientation = m_rot;
-                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
+                VehicleOrientation = m_rot;
+                VDetailLog("{0},  LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
             }
 
         }
 
+        private float ClampInRange(float low, float val, float high)
+        {
+            return Math.Max(low, Math.Min(val, high));
+        }
+
         // Invoke the detailed logger and output something if it's enabled.
         private void VDetailLog(string msg, params Object[] args)
         {