Merge branch 'master' of ssh://opensimulator.org/var/git/opensim

14 files changed, 647 insertions, 303 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 4c195e1..21aa9be 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -165,8 +165,8 @@ public sealed class BSCharacter : BSPhysObject
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Do this after the object has been added to the world
-        BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr,
-                        (uint)CollisionFilterGroups.AvatarFilter,
+        BulletSimAPI.SetCollisionGroupMask2(PhysBody.ptr,
+                        (uint)CollisionFilterGroups.AvatarGroup,
                         (uint)CollisionFilterGroups.AvatarMask);
     }
 
@@ -307,7 +307,7 @@ public sealed class BSCharacter : BSPhysObject
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             if (Position.Z < waterHeight)
             {
                 _position.Z = waterHeight;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 74eb9ab..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,6 +442,7 @@ 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
@@ -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);
 
-            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep);
 
-            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
+            ComputeLinearBlockingEndPoint(pTimestep);
 
-            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
-
-            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.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,45 +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)
             {
@@ -796,32 +933,41 @@ 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);
                 }
             }
             return changed;
         }
 
-        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
+        // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
+        //    Prevent ground vehicles from motoring into the sky.This flag has a subtle effect when
+        //    used with conjunction with banking: the strength of the banking will decay when the
+        //    vehicle no longer experiences collisions. The decay timescale is the same as
+        //    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 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);
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
                     ret = new Vector3(0, 0, -distanceAboveGround);
                 }
-                // TODO: this calculation is all wrong. From the description at
+                // TODO: this calculation is wrong. From the description at
                 //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
                 //     has a decay factor. This says this force should
                 //     be computed with a motor.
-                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
+                // TODO: add interaction with banking.
+                VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
                                     Prim.LocalID, distanceAboveGround, ret);
             }
             return ret;
@@ -830,61 +976,63 @@ 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);
 
             // ==================================================================
-            // NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
+            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
+            //    This flag prevents linear deflection parallel to world z-axis. This is useful
+            //    for preventing ground vehicles with large linear deflection, like bumper cars,
+            //    from climbing their linear deflection into the sky. 
+            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 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
-                                    + bankingContribution
-                                    + deflectionContribution;
+                                    + 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
@@ -914,52 +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.
-                Prim.ZeroAngularMotion(true);
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
-            }
-            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)
@@ -977,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);
@@ -1041,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)
             {
@@ -1120,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)
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index e91bfa8..851d508 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -1,3 +1,30 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
 using System;
 using System.Collections.Generic;
 using System.Text;
@@ -8,7 +35,7 @@ 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 = 10000f;
+    public const float Infinite = 12345f;
     public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
 
     public BSMotor(string useName)
@@ -19,7 +46,9 @@ public abstract class BSMotor
     public virtual void Reset() { }
     public virtual void Zero() { }
 
+    // A name passed at motor creation for easily identifyable debugging messages.
     public string UseName { get; private set; }
+
     // Used only for outputting debug information. Might not be set so check for null.
     public BSScene PhysicsScene { get; set; }
     protected void MDetailLog(string msg, params Object[] parms)
@@ -34,10 +63,23 @@ 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 is decays in TargetValueDecayTimeScale and
+//     the CurrentValue will be held back by FrictionTimeScale.
+// TimeScale and TargetDelayTimeScale may be 'infinite' which means go 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
+//    values of CurrentValue are returned that approach the TargetValue.
+// The feature of decaying TargetValue is so vehicles will eventually
+//    come to a stop rather than run forever. This can be disabled by
+//    setting TargetValueDecayTimescale to 'infinite'.
+// The change from CurrentValue to TargetValue is linear over TimeScale seconds.
 public class BSVMotor : BSMotor
 {
-    public Vector3 FrameOfReference { get; set; }
-    public Vector3 Offset { get; set; }
+    // public Vector3 FrameOfReference { get; set; }
+    // public Vector3 Offset { get; set; }
 
     public float TimeScale { get; set; }
     public float TargetValueDecayTimeScale { get; set; }
@@ -72,6 +114,14 @@ 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()
+    {
+        return Step(1f);
+    }
+
     public Vector3 Step(float timeStep)
     {
         Vector3 returnCurrent = Vector3.Zero;
@@ -99,18 +149,19 @@ public class BSVMotor : BSMotor
             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;
                 CurrentValue *= (Vector3.One - frictionFactor);
             }
 
-            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},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
                                 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}",
+            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);
         }
@@ -120,7 +171,7 @@ public class BSVMotor : BSMotor
             CurrentValue = Vector3.Zero;
             TargetValue = Vector3.Zero;
 
-            MDetailLog("{0},BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
                                     BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
 
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index c62c79a..ea1f71a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -253,8 +253,9 @@ public sealed class BSPrim : BSPhysObject
         // Zero some other properties in the physics engine
         PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
         {
-            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity);
+            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
         });
     }
 
@@ -329,7 +330,7 @@ public sealed class BSPrim : BSPhysObject
 
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(_position);
             // TODO: a floating motor so object will bob in the water
             if (Math.Abs(Position.Z - waterHeight) > 0.1f)
             {
@@ -347,7 +348,9 @@ public sealed class BSPrim : BSPhysObject
         if (ret)
         {
             // Apply upforce and overcome gravity.
-            AddForce(upForce - PhysicsScene.DefaultGravity, false, inTaintTime);
+            OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
+            DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+            AddForce(correctionForce, false, inTaintTime);
         }
         return ret;
     }
@@ -643,9 +646,13 @@ public sealed class BSPrim : BSPhysObject
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Collision filter can be set only when the object is in the world
-        if (PhysBody.collisionFilter != 0 || PhysBody.collisionMask != 0)
+        if (PhysBody.collisionGroup != 0 || PhysBody.collisionMask != 0)
         {
-            BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr, (uint)PhysBody.collisionFilter, (uint)PhysBody.collisionMask);
+            if (!BulletSimAPI.SetCollisionGroupMask2(PhysBody.ptr, (uint)PhysBody.collisionGroup, (uint)PhysBody.collisionMask))
+            {
+                PhysicsScene.Logger.ErrorFormat("{0} Failure setting prim collision mask. localID={1}, grp={2:X}, mask={3:X}",
+                                LogHeader, LocalID, PhysBody.collisionGroup, PhysBody.collisionMask);
+            }
         }
 
         // Recompute any linkset parameters.
@@ -684,11 +691,11 @@ public sealed class BSPrim : BSPhysObject
             // There can be special things needed for implementing linksets
             Linkset.MakeStatic(this);
             // The activation state is 'disabled' so Bullet will not try to act on it.
-            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            // BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
             // Start it out sleeping and physical actions could wake it up.
-            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
+            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.ISLAND_SLEEPING);
 
-            PhysBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
+            PhysBody.collisionGroup = CollisionFilterGroups.StaticObjectGroup;
             PhysBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
         }
         else
@@ -734,7 +741,7 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.ACTIVE_TAG);
             // BulletSimAPI.Activate2(BSBody.ptr, true);
 
-            PhysBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
+            PhysBody.collisionGroup = CollisionFilterGroups.ObjectGroup;
             PhysBody.collisionMask = CollisionFilterGroups.ObjectMask;
         }
     }
@@ -762,7 +769,7 @@ public sealed class BSPrim : BSPhysObject
                 m_log.ErrorFormat("{0} MakeSolid: physical body of wrong type for non-solidness. id={1}, type={2}", LogHeader, LocalID, bodyType);
             }
             CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            PhysBody.collisionFilter = CollisionFilterGroups.VolumeDetectFilter;
+            PhysBody.collisionGroup = CollisionFilterGroups.VolumeDetectGroup;
             PhysBody.collisionMask = CollisionFilterGroups.VolumeDetectMask;
         }
     }
@@ -838,15 +845,6 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 RotationalVelocity {
         get {
-            /*
-            OMV.Vector3 pv = OMV.Vector3.Zero;
-            // if close to zero, report zero
-            // This is copied from ODE but I'm not sure why it returns zero but doesn't
-            //    zero the property in the physics engine.
-            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
-                return pv;
-             */
-
             return _rotationalVelocity;
         }
         set {
@@ -1408,7 +1406,7 @@ public sealed class BSPrim : BSPhysObject
             LastEntityProperties = CurrentEntityProperties;
             CurrentEntityProperties = entprop;
 
-            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
+            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
             DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
                     LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 09b1423..f72bd74 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -96,6 +96,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     public long SimulationStep { get { return m_simulationStep; } }
     private int m_taintsToProcessPerStep;
 
+    public delegate void PreStepAction(float timeStep);
+    public event PreStepAction BeforeStep;
+
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
     public int SimulationNowTime { get; private set; }
@@ -127,7 +130,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     public const uint GROUNDPLANE_ID = 1;
     public const uint CHILDTERRAIN_ID = 2;  // Terrain allocated based on our mega-prim childre start here
 
-    private float m_waterLevel;
+    public float SimpleWaterLevel { get; set; }
     public BSTerrainManager TerrainManager { get; private set; }
 
     public ConfigurationParameters Params
@@ -182,6 +185,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     private string m_physicsLoggingDir;
     private string m_physicsLoggingPrefix;
     private int m_physicsLoggingFileMinutes;
+    private bool m_physicsLoggingDoFlush;
     // 'true' of the vehicle code is to log lots of details
     public bool VehicleLoggingEnabled { get; private set; }
 
@@ -290,6 +294,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
                 m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-%REGIONNAME%-");
                 m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
+                m_physicsLoggingDoFlush = pConfig.GetBoolean("PhysicsLoggingDoFlush", false);
                 // Very detailed logging for vehicle debugging
                 VehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
 
@@ -485,8 +490,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         ProcessTaints();
 
         // Some of the prims operate with special vehicle properties
-        ProcessVehicles(timeStep);
-        ProcessTaints();    // the vehicles might have added taints
+        DoPreStepActions(timeStep);
+
+        // the prestep actions might have added taints
+        ProcessTaints();
 
         // step the physical world one interval
         m_simulationStep++;
@@ -634,12 +641,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public override void SetWaterLevel(float baseheight)
     {
-        m_waterLevel = baseheight;
-    }
-    // Someday....
-    public float GetWaterLevelAtXYZ(Vector3 loc)
-    {
-        return m_waterLevel;
+        SimpleWaterLevel = baseheight;
     }
 
     public override void DeleteTerrain()
@@ -910,6 +912,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         }
     }
 
+    private void DoPreStepActions(float timeStep)
+    {
+        ProcessVehicles(timeStep);
+
+        PreStepAction actions = BeforeStep;
+        if (actions != null)
+            actions(timeStep);
+
+    }
+
     // Some prims have extra vehicle actions
     // Called at taint time!
     private void ProcessVehicles(float timeStep)
@@ -974,6 +986,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //          Should handle fetching the right type from the ini file and converting it.
     //    -- a delegate for getting the value as a float
     //    -- a delegate for setting the value from a float
+    //    -- an optional delegate to update the value in the world. Most often used to
+    //          push the new value to an in-world object.
     //
     // The single letter parameters for the delegates are:
     //    s = BSScene
@@ -1493,7 +1507,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     {
         PhysicsLogging.Write(msg, args);
         // Add the Flush() if debugging crashes. Gets all the messages written out.
-        // PhysicsLogging.Flush();
+        if (m_physicsLoggingDoFlush) PhysicsLogging.Flush();
     }
     // Used to fill in the LocalID when there isn't one. It's the correct number of characters.
     public const string DetailLogZero = "0000000000";
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 892c34b..b94dcf6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -620,8 +620,7 @@ public sealed class BSShapeCollection : IDisposable
         }
         else
         {
-            // Pass false for physicalness as this creates some sort of bounding box which we don't need
-            meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, false);
+            meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false);
 
             if (meshData != null)
             {
@@ -694,8 +693,8 @@ public sealed class BSShapeCollection : IDisposable
         else
         {
             // Build a new hull in the physical world
-            // Pass false for physicalness as this creates some sort of bounding box which we don't need
-            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, false);
+            // Pass true for physicalness as this creates some sort of bounding box which we don't need
+            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false);
             if (meshData != null)
             {
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
index 1450f66..83b9c37 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -121,8 +121,8 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         // redo its bounding box now that it is in the world
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
 
-        BulletSimAPI.SetCollisionFilterMask2(m_mapInfo.terrainBody.ptr,
-                            (uint)CollisionFilterGroups.TerrainFilter,
+        BulletSimAPI.SetCollisionGroupMask2(m_mapInfo.terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainGroup,
                             (uint)CollisionFilterGroups.TerrainMask);
 
         // Make it so the terrain will not move or be considered for movement.
@@ -148,7 +148,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     }
 
     // The passed position is relative to the base of the region.
-    public override float GetHeightAtXYZ(Vector3 pos)
+    public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
 
@@ -166,5 +166,11 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         }
         return ret;
     }
+
+    // The passed position is relative to the base of the region.
+    public override float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        return PhysicsScene.SimpleWaterLevel;
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index cd623f1..83df360 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -62,7 +62,8 @@ public abstract class BSTerrainPhys : IDisposable
         ID = id;
     }
     public abstract void Dispose();
-    public abstract float GetHeightAtXYZ(Vector3 pos);
+    public abstract float GetTerrainHeightAtXYZ(Vector3 pos);
+    public abstract float GetWaterLevelAtXYZ(Vector3 pos);
 }
 
 // ==========================================================================================
@@ -75,6 +76,7 @@ public sealed class BSTerrainManager
     public const float HEIGHT_INITIALIZATION = 24.987f;
     public const float HEIGHT_INITIAL_LASTHEIGHT = 24.876f;
     public const float HEIGHT_GETHEIGHT_RET = 24.765f;
+    public const float WATER_HEIGHT_GETHEIGHT_RET = 19.998f;
 
     // If the min and max height are equal, we reduce the min by this
     //    amount to make sure that a bounding box is built for the terrain.
@@ -138,8 +140,8 @@ public sealed class BSTerrainManager
         // Ground plane does not move
         BulletSimAPI.ForceActivationState2(m_groundPlane.ptr, ActivationState.DISABLE_SIMULATION);
         // Everything collides with the ground plane.
-        BulletSimAPI.SetCollisionFilterMask2(m_groundPlane.ptr,
-                        (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
+        BulletSimAPI.SetCollisionGroupMask2(m_groundPlane.ptr,
+                        (uint)CollisionFilterGroups.GroundPlaneGroup, (uint)CollisionFilterGroups.GroundPlaneMask);
 
         // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain.
         BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize);
@@ -358,7 +360,7 @@ public sealed class BSTerrainManager
             BSTerrainPhys physTerrain;
             if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
             {
-                ret = physTerrain.GetHeightAtXYZ(loc - terrainBaseXYZ);
+                ret = physTerrain.GetTerrainHeightAtXYZ(loc - terrainBaseXYZ);
             }
             else
             {
@@ -370,6 +372,33 @@ public sealed class BSTerrainManager
         return ret;
     }
 
+    public float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        float ret = WATER_HEIGHT_GETHEIGHT_RET;
+
+        float tX = pos.X;
+        float tY = pos.Y;
+
+        Vector3 terrainBaseXYZ = Vector3.Zero;
+        terrainBaseXYZ.X = ((int)(tX / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
+        terrainBaseXYZ.Y = ((int)(tY / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
+
+        lock (m_terrains)
+        {
+            BSTerrainPhys physTerrain;
+            if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
+            {
+                ret = physTerrain.GetWaterLevelAtXYZ(pos);
+            }
+            else
+            {
+                PhysicsScene.Logger.ErrorFormat("{0} GetWaterHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
+                        LogHeader, PhysicsScene.RegionName, tX, tY);
+            }
+        }
+        return ret;
+    }
+
     // Although no one seems to check this, I do support combining.
     public bool SupportsCombining()
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
index 5f6675d..6ce767d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -130,8 +130,8 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         // Redo its bounding box now that it is in the world
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr);
 
-        BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr,
-                            (uint)CollisionFilterGroups.TerrainFilter,
+        BulletSimAPI.SetCollisionGroupMask2(m_terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainGroup,
                             (uint)CollisionFilterGroups.TerrainMask);
 
         // Make it so the terrain will not move or be considered for movement.
@@ -148,7 +148,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         }
     }
 
-    public override float GetHeightAtXYZ(Vector3 pos)
+    public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         // For the moment use the saved heightmap to get the terrain height.
         // TODO: raycast downward to find the true terrain below the position.
@@ -169,6 +169,12 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         return ret;
     }
 
+    // The passed position is relative to the base of the region.
+    public override float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        return PhysicsScene.SimpleWaterLevel;
+    }
+
     // Convert the passed heightmap to mesh information suitable for CreateMeshShape2().
     // Return 'true' if successfully created.
     public static bool ConvertHeightmapToMesh(
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 1e003e6..a5acfd1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -57,12 +57,12 @@ public struct BulletBody
     {
         ID = id;
         ptr = xx;
-        collisionFilter = 0;
+        collisionGroup = 0;
         collisionMask = 0;
     }
     public IntPtr ptr;
     public uint ID;
-    public CollisionFilterGroups collisionFilter;
+    public CollisionFilterGroups collisionGroup;
     public CollisionFilterGroups collisionMask;
     public override string ToString()
     {
@@ -71,10 +71,10 @@ public struct BulletBody
         buff.Append(ID.ToString());
         buff.Append(",p=");
         buff.Append(ptr.ToString("X"));
-        if (collisionFilter != 0 || collisionMask != 0)
+        if (collisionGroup != 0 || collisionMask != 0)
         {
-            buff.Append(",f=");
-            buff.Append(collisionFilter.ToString("X"));
+            buff.Append(",g=");
+            buff.Append(collisionGroup.ToString("X"));
             buff.Append(",m=");
             buff.Append(collisionMask.ToString("X"));
         }
@@ -376,36 +376,36 @@ public enum CollisionFilterGroups : uint
     // Don't use the bit definitions!!  Define the use in a
     //   filter/mask definition below. This way collision interactions
     //   are more easily debugged.
-    BNoneFilter              = 0,
-    BDefaultFilter           = 1 << 0,
-    BStaticFilter            = 1 << 1,
-    BKinematicFilter         = 1 << 2,
-    BDebrisFilter            = 1 << 3,
-    BSensorTrigger           = 1 << 4,
-    BCharacterFilter         = 1 << 5,
-    BAllFilter               = 0xFFFFFFFF,
+    BNoneGroup              = 0,
+    BDefaultGroup           = 1 << 0,
+    BStaticGroup            = 1 << 1,
+    BKinematicGroup         = 1 << 2,
+    BDebrisGroup            = 1 << 3,
+    BSensorTrigger          = 1 << 4,
+    BCharacterGroup         = 1 << 5,
+    BAllGroup               = 0xFFFFFFFF,
     // Filter groups defined by BulletSim
-    BGroundPlaneFilter       = 1 << 10,
-    BTerrainFilter           = 1 << 11,
-    BRaycastFilter           = 1 << 12,
-    BSolidFilter             = 1 << 13,
-    BLinksetFilter           = 1 << 14,
+    BGroundPlaneGroup       = 1 << 10,
+    BTerrainGroup           = 1 << 11,
+    BRaycastGroup           = 1 << 12,
+    BSolidGroup             = 1 << 13,
+    BLinksetGroup           = 1 << 14,
 
     // The collsion filters and masked are defined in one place -- don't want them scattered
-    AvatarFilter            = BCharacterFilter,
-    AvatarMask              = BAllFilter,
-    ObjectFilter            = BSolidFilter,
-    ObjectMask              = BAllFilter,
-    StaticObjectFilter      = BStaticFilter,
-    StaticObjectMask        = BAllFilter & ~BStaticFilter,   // static objects don't collide with each other
-    LinksetFilter           = BLinksetFilter,
-    LinksetMask             = BAllFilter & ~BLinksetFilter, // linkset objects don't collide with each other
-    VolumeDetectFilter      = BSensorTrigger,
+    AvatarGroup             = BCharacterGroup,
+    AvatarMask              = BAllGroup,
+    ObjectGroup             = BSolidGroup,
+    ObjectMask              = BAllGroup,
+    StaticObjectGroup       = BStaticGroup,
+    StaticObjectMask        = AvatarGroup | ObjectGroup,    // static things don't interact with much
+    LinksetGroup            = BLinksetGroup,
+    LinksetMask             = BAllGroup & ~BLinksetGroup, // linkset objects don't collide with each other
+    VolumeDetectGroup       = BSensorTrigger,
     VolumeDetectMask        = ~BSensorTrigger,
-    TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter & ~BStaticFilter,  // static objects on the ground don't collide
-    GroundPlaneFilter       = BGroundPlaneFilter,
-    GroundPlaneMask         = BAllFilter
+    TerrainGroup            = BTerrainGroup,
+    TerrainMask             = BAllGroup & ~BStaticGroup,  // static objects on the ground don't collide
+    GroundPlaneGroup        = BGroundPlaneGroup,
+    GroundPlaneMask         = BAllGroup
 
 };
 
@@ -945,7 +945,7 @@ public static extern IntPtr GetConstraintRef2(IntPtr obj, int index);
 public static extern int GetNumConstraintRefs2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCollisionFilterMask2(IntPtr body, uint filter, uint mask);
+public static extern bool SetCollisionGroupMask2(IntPtr body, uint filter, uint mask);
 
 // =====================================================================================
 // btCollisionShape entries
@@ -1007,13 +1007,16 @@ public static extern void DumpRigidBody2(IntPtr sim, IntPtr collisionObject);
 public static extern void DumpCollisionShape2(IntPtr sim, IntPtr collisionShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpConstraint2(IntPtr sim, IntPtr constrain);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpAllInfo2(IntPtr sim);
+public static extern void DumpActivationInfo2(IntPtr sim);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
+public static extern void DumpAllInfo2(IntPtr sim);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpPhysicsStatistics2(IntPtr sim);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
new file mode 100755
index 0000000..68f25fc
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -0,0 +1,117 @@
+CRASHES
+=================================================
+20121129.1411: editting/moving phys object across region boundries causes crash
+    getPos-> btRigidBody::upcast -> getBodyType -> BOOM
+20121128.1600: mesh object not rezzing (no physics mesh).
+    Causes many errors. Doesn't stop after first error with box shape.
+    Eventually crashes when deleting the object.
+
+BULLETSIM TODO LIST:
+=================================================
+Neb car jiggling left and right
+Vehicles (Move smoothly)
+Light cycle falling over when driving
+Light cycle not banking
+Do single prim vehicles don't seem to properly vehiclize.
+Gun sending shooter flying
+Collision margin (gap between physical objects lying on each other)
+Boundry checking (crashes related to crossing boundry)
+    Add check for border edge position for avatars and objects.
+    Verify the events are created for border crossings.
+Avatar rotation (check out changes to ScenePresence for physical rotation)
+Avatar running (what does phys engine need to do?)
+Small physical objects do not interact correctly
+    Create chain of .5x.5x.1 torui and make all but top physical so to hang.
+        The chain will fall apart and pairs will dance around on ground
+    Chains of 1x1x.2 will stay connected but will dance.
+    Chains above 2x2x.4 are move stable and get stablier as torui get larger.
+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.
+Add PID motor for avatar movement (slow to stop, ...)
+Implement function efficiency for lineaar and angular motion.
+
+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
+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.
+Linkset explosion after three "rides" on Nebadon lite vehicle (LinksetConstraint)
+
+Speed up creation of large physical linksets
+        For instance, sitting in Neb's car (130 prims) takes several seconds to become physical
+Performance test with lots of avatars. Can BulletSim support a thousand?
+Optimize collisions in C++: only send up to the object subscribed to collisions.
+    Use collision subscription and remove the collsion(A,B) and collision(B,A)
+Check wheter SimMotionState needs large if statement (see TODO).
+
+Implement 'top colliders' info.
+Avatar jump
+Implement meshes or just verify that they work.
+Do prim hash codes work for sculpties and meshes?
+Performance measurement and changes to make quicker.
+Implement detailed physics stats (GetStats()).
+
+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?
+
+Measure performance improvement from hulls
+Test not using ghost objects for volume detect implementation.
+Performance of closures and delegates for taint processing
+        Are there faster ways?
+        Is any slowdown introduced by the existing implementation significant?
+Is there are more efficient method of implementing pre and post step actions?
+        See http://www.codeproject.com/Articles/29922/Weak-Events-in-C
+
+Package Bullet source mods for Bullet internal stats output
+
+Physics Arena central pyramid: why is one side permiable?
+
+INTERNAL IMPROVEMENT/CLEANUP
+=================================================
+Remove unused fields from ShapeData (not used in API2)
+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.
+Generalize Dynamics and PID with standardized motors.
+Generalize Linkset and vehicles into PropertyManagers
+    Methods for Refresh, RemoveBodyDependencies, RestoreBodyDependencies
+        Possibly generalized a 'pre step action' registration.
+Complete implemention of preStepActions
+        Replace vehicle step call with prestep event.
+        Is there a need for postStepActions? postStepTaints?
+Implement linkset by setting position of children when root updated. (LinksetManual)
+LinkablePrim class? Would that simplify/centralize the linkset logic?
+Linkset implementation using manual prim movement.
+Linkset implementation using compound shapes.
+    Compound shapes will need the LocalID in the shapes and collision
+    processing to get it from there.
+BSScene.UpdateParameterSet() is broken. How to set params on objects?
+Remove HeightmapInfo from terrain specification.
+    Since C++ code does not need terrain height, this structure et al are not needed.
+Add floating motor for BS_FLOATS_ON_WATER so prim and avatar will
+    bob at the water level. BSPrim.PositionSanityCheck().
+
+DONE DONE DONE DONE
+=================================================
+Cleanup code in BSDynamics by using motors.
+Consider implementing terrain with a mesh rather than heightmap.
+    Would have better and adjustable resolution.
+NOTDONE: Build terrain mesh so heighmap is height of the center of the square meter.
+    SL and ODE define meter square as being at one corner with one diagional.
+Terrain as mesh.
+How are static linksets seen by the physics engine?
+    A: they are not linked in physics. When moved, all the children are repositioned.
+Remember to remove BSScene.DetailLog Refresh call.
+Convert BSCharacter to use all API2
+Avatar pushing difficult (too heavy?)
+Use asset service passed to BulletSim to get sculptie bodies, etc.
+Vehicles (fix bouncing on terrain)
+Remove old code in DLL (all non-API2 stuff).
+Measurements of mega-physical prim performance (with graph)
+Debug Bullet internal stats output (why is timing all wrong?)
+        Bullet stats logging only works with a single instance of Bullet (one region).
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs
index 3a9ca1b..10c4bd3 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -36,6 +36,7 @@ namespace OpenSim.Region.Physics.Manager
     {
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod);
         IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical);
+        IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache);
     }
 
     // Values for level of detail to be passed to the mesher.
diff --git a/OpenSim/Region/Physics/Manager/ZeroMesher.cs b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
index ba19db6..270d2ec 100644
--- a/OpenSim/Region/Physics/Manager/ZeroMesher.cs
+++ b/OpenSim/Region/Physics/Manager/ZeroMesher.cs
@@ -64,11 +64,16 @@ namespace OpenSim.Region.Physics.Manager
     {
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false);
+            return CreateMesh(primName, primShape, size, lod, false, false);
         }
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
+            return CreateMesh(primName, primShape, size, lod, false, false);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
+        {
             // Remove the reference to the encoded JPEG2000 data so it can be GCed
             primShape.SculptData = OpenMetaverse.Utils.EmptyBytes;
 
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index 6fa91ab..8145d61 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -702,11 +702,16 @@ namespace OpenSim.Region.Physics.Meshing
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod)
         {
-            return CreateMesh(primName, primShape, size, lod, false);
+            return CreateMesh(primName, primShape, size, lod, false, true);
         }
 
         public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical)
         {
+            return CreateMesh(primName, primShape, size, lod, isPhysical, true);
+        }
+
+        public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache)
+        {
 #if SPAM
             m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName);
 #endif
@@ -716,9 +721,12 @@ namespace OpenSim.Region.Physics.Meshing
 
             // If this mesh has been created already, return it instead of creating another copy
             // For large regions with 100k+ prims and hundreds of copies of each, this can save a GB or more of memory
-            key = primShape.GetMeshKey(size, lod);
-            if (m_uniqueMeshes.TryGetValue(key, out mesh))
-                return mesh;
+            if (shouldCache)
+            {
+                key = primShape.GetMeshKey(size, lod);
+                if (m_uniqueMeshes.TryGetValue(key, out mesh))
+                    return mesh;
+            }
 
             if (size.X < 0.01f) size.X = 0.01f;
             if (size.Y < 0.01f) size.Y = 0.01f;
@@ -741,7 +749,10 @@ namespace OpenSim.Region.Physics.Meshing
                 // trim the vertex and triangle lists to free up memory
                 mesh.TrimExcess();
 
-                m_uniqueMeshes.Add(key, mesh);
+                if (shouldCache)
+                {
+                    m_uniqueMeshes.Add(key, mesh);
+                }
             }
 
             return mesh;