19 files changed, 2281 insertions, 1099 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 4c195e1..4dd6264 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -105,12 +105,12 @@ public sealed class BSCharacter : BSPhysObject
        DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5}",
                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
-        // do actual create at taint time
+        // do actual creation in taint time
        PhysicsScene.TaintedObject("BSCharacter.create", delegate()
        {
            DetailLog("{0},BSCharacter.create,taint", LocalID);
            // New body and shape into PhysBody and PhysShape
-            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, null, null);
+            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this);
            SetPhysicalProperties();
        });
@@ -124,7 +124,9 @@ public sealed class BSCharacter : BSPhysObject
        PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
        {
            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysBody.Clear();
            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
+            PhysShape.Clear();
        });
    }
@@ -165,9 +167,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,
+        PhysBody.collisionType = CollisionType.Avatar;
-                        (uint)CollisionFilterGroups.AvatarFilter,
+        PhysBody.ApplyCollisionMask();
-                        (uint)CollisionFilterGroups.AvatarMask);
    }
    public override void RequestPhysicsterseUpdate()
@@ -187,6 +188,11 @@ public sealed class BSCharacter : BSPhysObject
        set {
            // When an avatar's size is set, only the height is changed.
            _size = value;
+            // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
+            //     replace with the default values.
+            if (_size.X == 0f) _size.X = PhysicsScene.Params.avatarCapsuleDepth;
+            if (_size.Y == 0f) _size.Y = PhysicsScene.Params.avatarCapsuleWidth;
            ComputeAvatarScale(_size);
            ComputeAvatarVolumeAndMass();
            DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
@@ -194,15 +200,18 @@ public sealed class BSCharacter : BSPhysObject
            PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
            {
-                BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
+                if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
-                UpdatePhysicalMassProperties(RawMass);
+                {
+                    BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
+                    UpdatePhysicalMassProperties(RawMass);
+                    // Make sure this change appears as a property update event
+                    BulletSimAPI.PushUpdate2(PhysBody.ptr);
+                }
            });
        }
    }
-    public override OMV.Vector3 Scale { get; set; }
    public override PrimitiveBaseShape Shape
    {
        set { BaseShape = value; }
@@ -236,7 +245,8 @@ public sealed class BSCharacter : BSPhysObject
        // Zero some other properties directly into the physics engine
        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
        {
-            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.ClearAllForces2(PhysBody.ptr);
        });
    }
    public override void ZeroAngularMotion(bool inTaintTime)
@@ -245,10 +255,13 @@ public sealed class BSCharacter : BSPhysObject
        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
        {
-            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            if (PhysBody.HasPhysicalBody)
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            {
-            // The next also get rid of applied linear force but the linear velocity is untouched.
+                BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-            BulletSimAPI.ClearForces2(PhysBody.ptr);
+                BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+                // The next also get rid of applied linear force but the linear velocity is untouched.
+                BulletSimAPI.ClearForces2(PhysBody.ptr);
+            }
        });
    }
@@ -273,7 +286,8 @@ public sealed class BSCharacter : BSPhysObject
            PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
            {
                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
            });
        }
    }
@@ -297,6 +311,15 @@ public sealed class BSCharacter : BSPhysObject
    {
        bool ret = false;
+        // TODO: check for out of bounds
+        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        {
+            // The character is out of the known/simulated area.
+            // Upper levels of code will handle the transition to other areas so, for
+            //     the time, we just ignore the position.
+            return ret;
+        }
        // If below the ground, move the avatar up
        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
        if (Position.Z < terrainHeight)
@@ -307,7 +330,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;
@@ -315,7 +338,6 @@ public sealed class BSCharacter : BSPhysObject
            }
        }
-        // TODO: check for out of bounds
        return ret;
    }
@@ -332,7 +354,8 @@ public sealed class BSCharacter : BSPhysObject
            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
            {
                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
            });
            ret = true;
        }
@@ -359,7 +382,8 @@ public sealed class BSCharacter : BSPhysObject
            PhysicsScene.TaintedObject("BSCharacter.SetForce", delegate()
            {
                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
            });
        }
    }
@@ -398,7 +422,8 @@ public sealed class BSCharacter : BSPhysObject
                if (_currentFriction != PhysicsScene.Params.avatarStandingFriction)
                {
                    _currentFriction = PhysicsScene.Params.avatarStandingFriction;
-                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
+                    if (PhysBody.HasPhysicalBody)
+                        BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
                }
            }
            else
@@ -406,7 +431,8 @@ public sealed class BSCharacter : BSPhysObject
                if (_currentFriction != PhysicsScene.Params.avatarFriction)
                {
                    _currentFriction = PhysicsScene.Params.avatarFriction;
-                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
+                    if (PhysBody.HasPhysicalBody)
+                        BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
                }
            }
            _velocity = value;
@@ -443,8 +469,11 @@ public sealed class BSCharacter : BSPhysObject
            // m_log.DebugFormat("{0}: set orientation to {1}", LogHeader, _orientation);
            PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
            {
-                // _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+                if (PhysBody.HasPhysicalBody)
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                {
+                    // _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+                    BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                }
            });
        }
    }
@@ -517,10 +546,13 @@ public sealed class BSCharacter : BSPhysObject
            _floatOnWater = value;
            PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
            {
-                if (_floatOnWater)
+                if (PhysBody.HasPhysicalBody)
-                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                {
-                else
+                    if (_floatOnWater)
-                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                        CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    else
+                        CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                }
            });
        }
    }
@@ -553,7 +585,8 @@ public sealed class BSCharacter : BSPhysObject
            DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
            // Buoyancy is faked by changing the gravity applied to the object
            float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
-            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
        }
    }
@@ -599,7 +632,8 @@ public sealed class BSCharacter : BSPhysObject
            PhysicsScene.TaintedObject("BSCharacter.AddForce", delegate()
            {
                DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
            });
        }
        else
@@ -616,9 +650,6 @@ public sealed class BSCharacter : BSPhysObject
    private void ComputeAvatarScale(OMV.Vector3 size)
    {
-        // The 'size' given by the simulator is the mid-point of the avatar
-        //    and X and Y are unspecified.
        OMV.Vector3 newScale = size;
        // newScale.X = PhysicsScene.Params.avatarCapsuleWidth;
        // newScale.Y = PhysicsScene.Params.avatarCapsuleDepth;
@@ -677,7 +708,7 @@ public sealed class BSCharacter : BSPhysObject
        }
        // Tell the linkset about value changes
-        Linkset.UpdateProperties(this);
+        Linkset.UpdateProperties(this, true);
        // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
        // base.RequestPhysicsterseUpdate();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 65fac00..6b1e304 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -57,7 +57,7 @@ public abstract class BSConstraint : IDisposable
        if (m_enabled)
        {
            m_enabled = false;
-            if (m_constraint.ptr != IntPtr.Zero)
+            if (m_constraint.HasPhysicalConstraint)
            {
                bool success = BulletSimAPI.DestroyConstraint2(m_world.ptr, m_constraint.ptr);
                m_world.physicsScene.DetailLog("{0},BSConstraint.Dispose,taint,id1={1},body1={2},id2={3},body2={4},success={5}",
@@ -65,7 +65,7 @@ public abstract class BSConstraint : IDisposable
                                    m_body1.ID, m_body1.ptr.ToString("X"),
                                    m_body2.ID, m_body2.ptr.ToString("X"),
                                    success);
-                m_constraint.ptr = System.IntPtr.Zero;
+                m_constraint.Clear();
            }
        }
    }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index 23ef052..b073555 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -65,7 +65,7 @@ public sealed class BSConstraint6Dof : BSConstraint
        m_world = world;
        m_body1 = obj1;
        m_body2 = obj2;
-        if (obj1.ptr == IntPtr.Zero || obj2.ptr == IntPtr.Zero)
+        if (!obj1.HasPhysicalBody || !obj2.HasPhysicalBody)
        {
            world.physicsScene.DetailLog("{0},BS6DOFConstraint,badBodyPtr,wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
                            BSScene.DetailLogZero, world.worldID,
@@ -83,7 +83,7 @@ public sealed class BSConstraint6Dof : BSConstraint
            world.physicsScene.DetailLog("{0},BS6DofConstraint,createMidPoint,wID={1}, csrt={2}, rID={3}, rBody={4}, cID={5}, cBody={6}",
                                BSScene.DetailLogZero, world.worldID, m_constraint.ptr.ToString("X"),
                                obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
-            if (m_constraint.ptr == IntPtr.Zero)
+            if (!m_constraint.HasPhysicalConstraint)
            {
                world.physicsScene.Logger.ErrorFormat("{0} Failed creation of 6Dof constraint. rootID={1}, childID={2}",
                                LogHeader, obj1.ID, obj2.ID);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..5887249 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -24,30 +24,17 @@
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
+ * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
-/* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to
+ * are Copyright (c) 2009 Linden Research, Inc and are used under their license
- * call the BulletSim system.
+ * of Creative Commons Attribution-Share Alike 3.0
- */
+ * (http://creativecommons.org/licenses/by-sa/3.0/).
-/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
- * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
- * ODEPrim.cs contains methods dealing with Prim editing, Prim
- * characteristics and Kinetic motion.
- * ODEDynamics.cs contains methods dealing with Prim Physical motion
- * (dynamics) and the associated settings. Old Linear and angular
- * motors for dynamic motion have been replace with  MoveLinear()
- * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
- * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
- * switch between 'VEHICLE' parameter use and general dynamics
- * settings use.
 */
 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
@@ -80,10 +67,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        private Quaternion m_referenceFrame = Quaternion.Identity;
        // Linear properties
+        private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
        private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
        private Vector3 m_linearMotorOffset = Vector3.Zero;             // the point of force can be offset from the center
        private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
-        private Vector3 m_newVelocity = Vector3.Zero;                   // velocity computed to be applied to body
        private Vector3 m_linearFrictionTimescale = Vector3.Zero;
        private float m_linearMotorDecayTimescale = 0;
        private float m_linearMotorTimescale = 0;
@@ -93,13 +80,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        // private Vector3 m_linearMotorOffset = Vector3.Zero;
        //Angular properties
+        private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
        private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
        // private int m_angularMotorApply = 0;                            // application frame counter
        private Vector3 m_angularMotorVelocity = Vector3.Zero;          // current angular motor velocity
        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_lastAngularVelocity = Vector3.Zero;
        private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
        //Deflection properties
@@ -124,8 +112,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
        //Attractor properties
-        private float m_verticalAttractionEfficiency = 1.0f;        // damped
+        private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
-        private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
+        private float m_verticalAttractionEfficiency = 1.0f; // damped
+        private float m_verticalAttractionCutoff = 500f;     // per the documentation
+        // Timescale > cutoff  means no vert attractor.
+        private float m_verticalAttractionTimescale = 510f;
        public BSDynamics(BSScene myScene, BSPrim myPrim)
        {
@@ -137,7 +128,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        // Return 'true' if this vehicle is doing vehicle things
        public bool IsActive
        {
-            get { return Type != Vehicle.TYPE_NONE; }
+            get { return Type != Vehicle.TYPE_NONE && Prim.IsPhysical; }
        }
        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
@@ -152,13 +143,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                    break;
                case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
+                    m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
                    break;
                case Vehicle.ANGULAR_MOTOR_TIMESCALE:
                    m_angularMotorTimescale = Math.Max(pValue, 0.01f);
+                    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);
@@ -167,10 +160,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;
@@ -185,33 +178,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                    break;
                case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
+                    m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
                    break;
                case Vehicle.LINEAR_MOTOR_TIMESCALE:
                    m_linearMotorTimescale = Math.Max(pValue, 0.01f);
+                    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:
                    m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
+                    m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
                    break;
                // These are vector properties but the engine lets you use a single float value to
                // set all of the components to the same value
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                    m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
                    break;
                case Vehicle.ANGULAR_MOTOR_DIRECTION:
                    m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
-                    // m_angularMotorApply = 100;
+                    m_angularMotor.SetTarget(m_angularMotorDirection);
                    break;
                case Vehicle.LINEAR_FRICTION_TIMESCALE:
                    m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                    break;
                case Vehicle.LINEAR_MOTOR_DIRECTION:
                    m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
                    m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                    break;
                case Vehicle.LINEAR_MOTOR_OFFSET:
                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -227,21 +227,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            {
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
                    m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
                    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.X = ClampInRange(-12.56f, pValue.X, 12.56f);
-                    pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
+                    pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
-                    pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
+                    pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
-                    // m_angularMotorApply = 100;
+                    m_angularMotor.SetTarget(m_angularMotorDirection);
                    break;
                case Vehicle.LINEAR_FRICTION_TIMESCALE:
                    m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                    break;
                case Vehicle.LINEAR_MOTOR_DIRECTION:
                    m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                    m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                    break;
                case Vehicle.LINEAR_MOTOR_OFFSET:
                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -303,7 +306,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_VhoverEfficiency = 0;
                    m_VhoverTimescale = 0;
                    m_VehicleBuoyancy = 0;
-                    
                    m_linearDeflectionEfficiency = 1;
                    m_linearDeflectionTimescale = 1;
@@ -319,6 +322,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_referenceFrame = Quaternion.Identity;
                    m_flags = (VehicleFlag)0;
                    break;
                case Vehicle.TYPE_SLED:
@@ -351,10 +355,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_bankingMix = 1;
                    m_referenceFrame = Quaternion.Identity;
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
-                    m_flags &=
+                                | VehicleFlag.HOVER_TERRAIN_ONLY
-                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                                | VehicleFlag.HOVER_GLOBAL_HEIGHT
-                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                                | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                            | VehicleFlag.LIMIT_ROLL_ONLY
+                            | VehicleFlag.LIMIT_MOTOR_UP);
                    break;
                case Vehicle.TYPE_CAR:
                    m_linearMotorDirection = Vector3.Zero;
@@ -498,6 +506,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_bankingEfficiency = 0;
                    m_bankingMix = 0.7f;
                    m_bankingTimescale = 5;
                    m_referenceFrame = Quaternion.Identity;
                    m_referenceFrame = Quaternion.Identity;
@@ -510,6 +519,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    break;
            }
+            // Update any physical parameters based on this type.
+            Refresh();
+            m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
+                                m_linearMotorDecayTimescale, m_linearFrictionTimescale,
+                                1f);
+            m_linearMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
+            m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
+                                m_angularMotorDecayTimescale, m_angularFrictionTimescale,
+                                1f);
+            m_angularMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
+            m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
+                                BSMotor.Infinite, BSMotor.InfiniteVector,
+                                m_verticalAttractionEfficiency);
+            // Z goes away and we keep X and Y
+            m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
+            m_verticalAttractionMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
        }
        // Some of the properties of this prim may have changed.
@@ -518,13 +547,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        {
            if (IsActive)
            {
-                // Friction effects are handled by this vehicle code
+                // Remember the mass so we don't have to fetch it every step
-                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
+                m_vehicleMass = Prim.Linkset.LinksetMass;
-                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
+                // Friction affects are handled by this vehicle code
-                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
+                float friction = 0f;
+                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
-                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+                // Moderate angular movement introduced by Bullet.
+                // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
+                //     Maybe compute linear and angular factor and damping from params.
+                float angularDamping = PhysicsScene.Params.vehicleAngularDamping;
+                BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
+                // Vehicles report collision events so we know when it's on the ground
+                BulletSimAPI.AddToCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
+                // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet
+                // Vector3 localInertia = new Vector3(1f, 1f, 1f);
+                // Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
+                Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(Prim.PhysShape.ptr, m_vehicleMass);
+                BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
+                BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
+                VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4}",
+                                Prim.LocalID, m_vehicleMass, friction, localInertia, angularDamping);
+            }
+            else
+            {
+                BulletSimAPI.RemoveFromCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
            }
        }
@@ -546,116 +597,294 @@ 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 Vector3 m_knownForce;
+        private Quaternion? m_knownOrientation;
+        private Vector3? m_knownRotationalVelocity;
+        private Vector3 m_knownRotationalForce;
+        private float? m_knownForwardSpeed;
+        private const int m_knownChangedPosition           = 1 << 0;
+        private const int m_knownChangedVelocity           = 1 << 1;
+        private const int m_knownChangedForce              = 1 << 2;
+        private const int m_knownChangedOrientation        = 1 << 3;
+        private const int m_knownChangedRotationalVelocity = 1 << 4;
+        private const int m_knownChangedRotationalForce    = 1 << 5;
+        private void ForgetKnownVehicleProperties()
+        {
+            m_knownTerrainHeight = null;
+            m_knownWaterLevel = null;
+            m_knownPosition = null;
+            m_knownVelocity = null;
+            m_knownForce = Vector3.Zero;
+            m_knownOrientation = null;
+            m_knownRotationalVelocity = null;
+            m_knownRotationalForce = Vector3.Zero;
+            m_knownForwardSpeed = null;
+            m_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;
+                    BulletSimAPI.SetInterpolationLinearVelocity2(Prim.PhysBody.ptr, VehicleVelocity);
+                }
+                if ((m_knownChanged & m_knownChangedForce) != 0)
+                    Prim.AddForce((Vector3)m_knownForce, false, true);
+                if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
+                {
+                    Prim.ForceRotationalVelocity = VehicleRotationalVelocity;
+                    // Fake out Bullet by making it think the velocity is the same as last time.
+                    BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, VehicleRotationalVelocity);
+                }
+                if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
+                    Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
+                // 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);
+            }
+            m_knownChanged = 0;
+        }
+        // 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 void VehicleAddForce(Vector3 aForce)
+        {
+            m_knownForce += aForce;
+            m_knownChanged |= m_knownChangedForce;
+        }
+        private Vector3 VehicleRotationalVelocity
+        {
+            get
+            {
+                if (m_knownRotationalVelocity == null)
+                    m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                return (Vector3)m_knownRotationalVelocity;
+            }
+            set
+            {
+                m_knownRotationalVelocity = value;
+                m_knownChanged |= m_knownChangedRotationalVelocity;
+            }
+        }
+        private void VehicleAddAngularForce(Vector3 aForce)
+        {
+            m_knownRotationalForce += aForce;
+            m_knownChanged |= m_knownChangedRotationalForce;
+        }
+        private float VehicleForwardSpeed
+        {
+            get
+            {
+                if (m_knownForwardSpeed == null)
+                    m_knownForwardSpeed = (VehicleVelocity * Quaternion.Inverse(VehicleOrientation)).X;
+                return (float)m_knownForwardSpeed;
+            }
+        }
+        #endregion // Known vehicle value functions
        // One step of the vehicle properties for the next 'pTimestep' seconds.
        internal void Step(float pTimestep)
        {
            if (!IsActive) return;
-            // DEBUG
+            ForgetKnownVehicleProperties();
-            // Because Bullet does apply forces to the vehicle, our last computed
-            //     linear and angular velocities are not what is happening now.
-            // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
-            // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
-            // m_lastAngularVelocity = Prim.ForceRotationalVelocity;   // DEBUG: account for what Bullet did last time
-            // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);        // DEBUG:
-            // END DEBUG
-            m_vehicleMass = Prim.Linkset.LinksetMass;
            MoveLinear(pTimestep);
-            // Commented out for debug
            MoveAngular(pTimestep);
-            // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false);    // DEBUG DEBUG
-            // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity;    // DEBUG DEBUG
            LimitRotation(pTimestep);
            // remember the position so next step we can limit absolute movement effects
-            m_lastPositionVector = Prim.ForcePosition;
+            m_lastPositionVector = VehiclePosition;
-            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
+            // If we forced the changing of some vehicle parameters, update the values and
-                                    Prim.LocalID, 
+            //      for the physics engine to note the changes so an UpdateProperties event will happen.
-                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
+            PushKnownChanged();
-                                    BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
-                                    Prim.Inertia,
-                                    m_vehicleMass
-                                    );
            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);
-        }// end Step
+        }
-        // Apply the effect of the linear motor.
+        // Apply the effect of the linear motor and other linear motions (like hover and float).
-        // Also does hover and float.
        private void MoveLinear(float pTimestep)
        {
-            // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates
+            Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
-            // m_lastLinearVelocityVector is the current speed we are moving in that direction
-            if (m_linearMotorDirection.LengthSquared() > 0.001f)
-            {
-                Vector3 origDir = m_linearMotorDirection;       // DEBUG
-                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
-                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
-                Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
-                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
+            // The movement computed in the linear motor is relative to the vehicle
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
+            //     coordinates. Rotate the movement to world coordinates.
-                m_lastLinearVelocityVector += addAmount;
+            linearMotorContribution *= VehicleOrientation;
-                float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
+            // ==================================================================
-                m_linearMotorDirection *= (1f - decayFactor);
+            // Buoyancy: force to overcome gravity.
+            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
+            // So, if zero, don't change anything (let gravity happen). If one, negate the effect of gravity.
+            Vector3 buoyancyContribution =  Prim.PhysicsScene.DefaultGravity * m_VehicleBuoyancy;
-                // Rotate new object velocity from vehicle relative to world coordinates
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
-                m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
-                // Apply friction for next time
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep);
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
-                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
+            ComputeLinearBlockingEndPoint(pTimestep);
-                                Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
-                                m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
-            }
-            else
-            {
-                // if what remains of direction is very small, zero it.
-                m_linearMotorDirection = Vector3.Zero;
-                m_lastLinearVelocityVector = Vector3.Zero;
-                m_newVelocity = Vector3.Zero;
-                VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
-            }
-            // m_newVelocity is velocity computed from linear motor in world coordinates
+            // ==================================================================
+            Vector3 newVelocity = linearMotorContribution
+                            + terrainHeightContribution
+                            + hoverContribution
+                            + limitMotorUpContribution;
-            // Gravity and Buoyancy
+            Vector3 newForce = buoyancyContribution;
-            // There is some gravity, make a gravity force vector that is applied after object velocity.
-            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
+            // If not changing some axis, reduce out velocity
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
+            if ((m_flags & (VehicleFlag.NO_X)) != 0)
+                newVelocity.X = 0;
+            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
+                newVelocity.Y = 0;
+            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
+                newVelocity.Z = 0;
-            /*
+            // ==================================================================
-             * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
+            // Clamp high or low velocities
-            // Preserve the current Z velocity
+            float newVelocityLengthSq = newVelocity.LengthSquared();
-            Vector3 vel_now = m_prim.Velocity;
+            // if (newVelocityLengthSq > 1e6f)
-            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
+            if (newVelocityLengthSq > 1000f)
-             */
+            {
+                newVelocity /= newVelocity.Length();
+                newVelocity *= 1000f;
+            }
+            // else if (newVelocityLengthSq < 1e-6f)
+            else if (newVelocityLengthSq < 0.001f)
+                newVelocity = Vector3.Zero;
+            // ==================================================================
+            // 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 = newForce * m_vehicleMass;
+            if (!totalDownForce.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                VehicleAddForce(totalDownForce);
+            }
+            VDetailLog("{0},  MoveLinear,done,newVel={1},totDown={2},IsColliding={3}",
+                                Prim.LocalID, newVelocity, totalDownForce, Prim.IsColliding);
+            VDetailLog("{0},  MoveLinear,done,linContrib={1},terrContrib={2},hoverContrib={3},limitContrib={4},buoyContrib={5}",
+                                Prim.LocalID,
+                                linearMotorContribution, terrainHeightContribution, hoverContribution, 
+                                limitMotorUpContribution, buoyancyContribution
+            );
-            Vector3 pos = Prim.ForcePosition;
+        } // end MoveLinear()
-//            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
+        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
            // If below the terrain, move us above the ground a little.
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+            // TODO: Consider taking the rotated size of the object or possibly casting a ray.
-            // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
+            if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
-            //     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)
            {
-                pos.Z = terrainHeight + 2;
+                // TODO: correct position by applying force rather than forcing position.
-                Prim.ForcePosition = pos;
+                Vector3 newPosition = VehiclePosition;
-                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
+                newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
+                VehiclePosition = newPosition;
+                VDetailLog("{0},  MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
+                        Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
            }
+            return ret;
+        }
+        public Vector3 ComputeLinearHover(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
-            // Check if hovering
            // m_VhoverEfficiency: 0=bouncy, 1=totally damped
            // m_VhoverTimescale: time to achieve height
            if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
@@ -663,11 +892,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)
                {
@@ -677,45 +906,47 @@ 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)
+                    if (VehiclePosition.Z > m_VhoverTargetHeight)
-                        m_VhoverTargetHeight = pos.Z;
+                        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;
+                    // Error is positive if below the target and negative if above.
-                    // RA: where does the 50 come from?
+                    float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
-                    float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
+                    float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
-                    // Replace Vertical speed with correction figure if significant
-                    if (Math.Abs(verticalError) > 0.01f)
+                    // TODO: implement m_VhoverEfficiency correctly
-                    {
+                    if (Math.Abs(verticalError) > m_VhoverEfficiency)
-                        m_newVelocity.Z += verticalCorrectionVelocity;
-                        //KF: m_VhoverEfficiency is not yet implemented
-                    }
-                    else if (verticalError < -0.01)
-                    {
-                        m_newVelocity.Z -= verticalCorrectionVelocity;
-                    }
-                    else
                    {
-                        m_newVelocity.Z = 0f;
+                        ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
                    }
                }
-                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, 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)
+        {
+            bool changed = false;
+            Vector3 pos = VehiclePosition;
            Vector3 posChange = pos - m_lastPositionVector;
            if (m_BlockingEndPoint != Vector3.Zero)
            {
-                bool changed = false;
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
@@ -743,233 +974,110 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                }
                if (changed)
                {
-                    Prim.ForcePosition = pos;
+                    VehiclePosition = pos;
-                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                    VDetailLog("{0},  MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
                                Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                }
            }
+            return changed;
+        }
-            #region downForce
+        // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
-            Vector3 downForce = Vector3.Zero;
+        //    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 (height from water)?
+        //    This is just using the ground and a general collision check. Should really be using
+        //    a downward raycast to find what is below.
+        public Vector3 ComputeLinearMotorUp(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
+            float distanceAboveGround = 0f;
            if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
            {
-                // If the vehicle is motoring into the sky, get it going back down.
+                float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
-                // Is this an angular force or both linear and angular??
+                distanceAboveGround = VehiclePosition.Z - targetHeight;
-                float distanceAboveGround = pos.Z - terrainHeight;
+                // Not colliding if the vehicle is off the ground
-                if (distanceAboveGround > 2f)
+                if (!Prim.IsColliding)
                {
                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
                    // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
-                    downForce = new Vector3(0, 0, -distanceAboveGround);
+                    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.
-                                    Prim.LocalID, distanceAboveGround, downForce);
-            }
-            #endregion // downForce
-            // If not changing some axis, reduce out velocity
-            if ((m_flags & (VehicleFlag.NO_X)) != 0)
-                m_newVelocity.X = 0;
-            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-                m_newVelocity.Y = 0;
-            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-                m_newVelocity.Z = 0;
-            // Clamp REALLY high or low velocities
-            if (m_newVelocity.LengthSquared() > 1e6f)
-            {
-                m_newVelocity /= m_newVelocity.Length();
-                m_newVelocity *= 1000f;
            }
-            else if (m_newVelocity.LengthSquared() < 1e-6f)
+            VDetailLog("{0},  MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
-                m_newVelocity = Vector3.Zero;
+                                Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret);
+            return ret;
-            // Stuff new linear velocity into the vehicle
+        }
-            Prim.ForceVelocity = m_newVelocity;
-            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
-            Vector3 totalDownForce = downForce + grav;
-            if (totalDownForce != Vector3.Zero)
-            {
-                Prim.AddForce(totalDownForce * m_vehicleMass, false);
-                // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
-            }
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
-        } // end MoveLinear()
        // =======================================================================
+        // =======================================================================
        // 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 resulting velocity is
+        //     set directly on the vehicle.
        private void MoveAngular(float pTimestep)
        {
-            // m_angularMotorDirection         // angular velocity requested by LSL motor
+            // The user wants how many radians per second angular change?
-            // m_angularMotorApply             // application frame counter
+            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
-            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-            // m_angularMotorTimescale         // motor angular velocity ramp up rate
+            // ==================================================================
-            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
+            // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
-            // m_angularFrictionTimescale      // body angular velocity  decay rate
+            //    This flag prevents linear deflection parallel to world z-axis. This is useful
-            // m_lastAngularVelocity           // what was last applied to body
+            //    for preventing ground vehicles with large linear deflection, like bumper cars,
+            //    from climbing their linear deflection into the sky. 
-            if (m_angularMotorDirection.LengthSquared() > 0.0001)
+            // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
-            {
+            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
-                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;
+                angularMotorContribution.X = 0f;
+                angularMotorContribution.Y = 0f;
+                VDetailLog("{0},  MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
            }
-            #region Vertical attactor
+            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
-            Vector3 vertattr = Vector3.Zero;
-            Vector3 deflection = Vector3.Zero;
-            Vector3 banking = Vector3.Zero;
-            // If vertical attaction timescale is reasonable and we applied an angular force last time...
-            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
-            {
-                float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
-                if (Prim.IsColliding)
-                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
-                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-                // Create a vector of the vehicle "up" in world coordinates
-                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
-                // verticalError.X and .Y are the World error amounts. They are 0 when there is no
-                //      error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its
-                //      side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall
-                //      and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
-                //      modulated to prevent a stable inverted body.
-                // Error is 0 (no error) to +/- 2 (max error)
-                if (verticalError.Z < 0.0f)
-                {
-                    verticalError.X = 2.0f - verticalError.X;
-                    verticalError.Y = 2.0f - verticalError.Y;
-                }
-                // scale it by VAservo (timestep and timescale)
-                verticalError = verticalError * VAservo;
-                // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
-                //     then .X increases, so change  Body angular velocity  X based on Y, and Y based on X.
-                //     Z is not changed.
-                vertattr.X =    verticalError.Y;
-                vertattr.Y =  - verticalError.X;
-                vertattr.Z = 0f;
-                // scaling appears better usingsquare-law
+            Vector3 deflectionContribution = ComputeAngularDeflection();
-                Vector3 angularVelocity = Prim.ForceRotationalVelocity;
-                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-                vertattr.X += bounce * angularVelocity.X;
-                vertattr.Y += bounce * angularVelocity.Y;
-                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
+            Vector3 bankingContribution = ComputeAngularBanking();
-                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
-            }
+            // ==================================================================
-            #endregion // Vertical attactor
+            m_lastVertAttractor = verticalAttractionContribution;
-            #region Deflection
+            m_lastAngularVelocity = angularMotorContribution
+                                    + verticalAttractionContribution
+                                    + deflectionContribution
+                                    + bankingContribution;
-            if (m_angularDeflectionEfficiency != 0)
+            // ==================================================================
+            // Apply the correction velocity.
+            // TODO: Should this be applied as an angular force (torque)?
+            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
-                // Compute a scaled vector that points in the preferred axis (X direction)
+                VehicleRotationalVelocity = m_lastAngularVelocity;
-                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);
-                // Scale by efficiency and timescale
-                deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
-                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
-                                        Prim.LocalID, preferredAxisOfMotion, deflection);
-                // This deflection computation is not correct.
-                deflection = Vector3.Zero;
-            }
-            #endregion
-            #region Banking
-            if (m_bankingEfficiency != 0)
+                VDetailLog("{0},  MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
+                                    Prim.LocalID,
+                                    angularMotorContribution, verticalAttractionContribution,
+                                    bankingContribution, deflectionContribution,
+                                    m_lastAngularVelocity
+                                    );
+            }
+            else
            {
-                Vector3 dir = Vector3.One * Prim.ForceOrientation;
+                // The vehicle is not adding anything angular wise.
-                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
+                VehicleRotationalVelocity = Vector3.Zero;
-                    //Changes which way it banks in and out of turns
+                VDetailLog("{0},  MoveAngular,done,zero", Prim.LocalID);
-                //Use the square of the efficiency, as it looks much more how SL banking works
-                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
-                if (m_bankingEfficiency < 0)
-                    effSquared *= -1; //Keep the negative!
-                float mix = Math.Abs(m_bankingMix);
-                if (m_angularMotorVelocity.X == 0)
-                {
-                    /*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
-                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
-                if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
-                    //If they are colliding, we probably shouldn't shove the prim around... probably
-                {
-                    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;
-                    banking += bankingRot;
-                }
-                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
-                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 
-                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
            }
-            #endregion
+            // ==================================================================
-            m_lastVertAttractor = vertattr;
-            // Sum velocities
-            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
-            #region Linear Motor Offset
            //Offset section
            if (m_linearMotorOffset != Vector3.Zero)
            {
@@ -985,8 +1093,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                //
                // The torque created is the linear velocity crossed with the offset
-                // NOTE: this computation does should be in the linear section
+                // TODO: this computation should be in the linear section
-                //    because there we know the impulse being applied.
+                //    because that is where we know the impulse being applied.
                Vector3 torqueFromOffset = Vector3.Zero;
                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
                if (float.IsNaN(torqueFromOffset.X))
@@ -995,47 +1103,175 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    torqueFromOffset.Y = 0;
                if (float.IsNaN(torqueFromOffset.Z))
                    torqueFromOffset.Z = 0;
-                torqueFromOffset *= m_vehicleMass;
-                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
+                VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
-                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+                VDetailLog("{0},  BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
            }
-            #endregion
+        }
+        // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
+        //      Some vehicles, like boats, should always keep their up-side up. This can be done by
+        //      enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
+        //      the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
+        //      VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
+        //      and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
+        //      efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
+        //      efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
+        public Vector3 ComputeAngularVerticalAttraction()
+        {
+            Vector3 ret = Vector3.Zero;
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+            // If vertical attaction timescale is reasonable
+            if (m_verticalAttractionTimescale < m_verticalAttractionCutoff)
            {
-                m_lastAngularVelocity.X = 0;
+                // Take a vector pointing up and convert it from world to vehicle relative coords.
-                m_lastAngularVelocity.Y = 0;
+                Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
-                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
+                //    is now:
+                //    leaning to one side: rotated around the X axis with the Y value going
+                //        from zero (nearly straight up) to one (completely to the side)) or
+                //    leaning front-to-back: rotated around the Y axis with the value of X being between
+                //         zero and one.
+                // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
+                // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
+                if (verticalError.Z < 0f)
+                {
+                    verticalError.X = 2f - verticalError.X;
+                    verticalError.Y = 2f - verticalError.Y;
+                }
+                // Y error means needed rotation around X axis and visa versa.
+                ret.X =    verticalError.Y;
+                ret.Y =  - verticalError.X;
+                ret.Z = 0f;
+                // Scale the correction force by how far we're off from vertical.
+                // Z error of one says little error. As Z gets smaller, the vehicle is leaning farther over.
+                float clampedSqrZError = ClampInRange(0.01f, verticalError.Z * verticalError.Z, 1f);
+                float vertForce = 1f / clampedSqrZError;
+                ret *= vertForce;
+                // Correction happens over a number of seconds.
+                Vector3 unscaledContrib = ret;
+                ret /= m_verticalAttractionTimescale;
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},vertForce={3},eff={4},vertAttr={5}",
+                                Prim.LocalID, verticalError, unscaledContrib, vertForce, m_verticalAttractionEfficiency, ret);
            }
+            return ret;
+        }
+        // Return the angular correction to correct the direction the vehicle is pointing to be
+        //      the direction is should want to be pointing.
+        // The vehicle is moving in some direction and correct its orientation to it is pointing
+        //     in that direction.
+        // TODO: implement reference frame.
+        public Vector3 ComputeAngularDeflection()
+        {
+            Vector3 ret = Vector3.Zero;
+            return ret;     // DEBUG DEBUG DEBUG  debug one force at a time
-            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            if (m_angularDeflectionEfficiency != 0)
            {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                // The direction the vehicle is moving
-                Prim.ZeroAngularMotion(true);
+                Vector3 movingDirection = VehicleVelocity;
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                movingDirection.Normalize();
+                // The direction the vehicle is pointing
+                Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
+                pointingDirection.Normalize();
+                // The difference between what is and what should be
+                Vector3 deflectionError = movingDirection - pointingDirection;
+                // Scale the correction by recovery timescale and efficiency
+                ret = (-deflectionError * VehicleForwardSpeed) * m_angularDeflectionEfficiency;
+                ret /= m_angularDeflectionTimescale;
+                VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
+                    Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
            }
-            else
+            return ret;
+        }
+        // Return an angular change to rotate the vehicle around the Z axis when the vehicle
+        //     is tipped around the X axis.
+        // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
+        //      The vertical attractor feature must be enabled in order for the banking behavior to
+        //      function. The way banking works is this: a rotation around the vehicle's roll-axis will
+        //      produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
+        //      of the yaw effect will be proportional to the
+        //          VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
+        //                 velocity along its preferred axis of motion. 
+        //          The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
+        //                  positive rotation (by the right-hand rule) about the roll-axis will effect a
+        //                  (negative) torque around the yaw-axis, making it turn to the right--that is the
+        //                  vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
+        //                  Negating the banking coefficient will make it so that the vehicle leans to the
+        //                  outside of the turn (not very "physical" but might allow interesting vehicles so why not?). 
+        //           The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
+        //                  banking vehicles do what you want rather than what the laws of physics allow.
+        //                  For example, consider a real motorcycle...it must be moving forward in order for
+        //                  it to turn while banking, however video-game motorcycles are often configured
+        //                  to turn in place when at a dead stop--because they are often easier to control
+        //                  that way using the limited interface of the keyboard or game controller. The
+        //                  VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
+        //                  banking by functioning as a slider between a banking that is correspondingly
+        //                  totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
+        //                  banking effect depends only on the vehicle's rotation about its roll-axis compared
+        //                  to "dynamic" where the banking is also proportional to its velocity along its
+        //                  roll-axis. Finding the best value of the "mixture" will probably require trial and error. 
+        //      The time it takes for the banking behavior to defeat a preexisting angular velocity about the
+        //      world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
+        //      bank quickly then give it a banking timescale of about a second or less, otherwise you can
+        //      make a sluggish vehicle by giving it a timescale of several seconds. 
+        public Vector3 ComputeAngularBanking()
+        {
+            Vector3 ret = Vector3.Zero;
+            if (m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
            {
-                // Apply to the body.
+                // This works by rotating a unit vector to the orientation of the vehicle. The
-                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
+                //    roll (tilt) will be Y component of a tilting Z vector (zero for no tilt
-                // Since we are stuffing the angular velocity directly into the object, the computed
+                //    up to one for full over).
-                //     velocity needs to be scaled by the timestep.
+                Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
-                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
-                Prim.ForceRotationalVelocity = applyAngularForce;
+                // Figure out the yaw value for this much roll.
+                float turnComponent = rollComponents.Y * rollComponents.Y * m_bankingEfficiency;
-                // Decay the angular movement for next time
+                // Keep the sign
-                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
+                if (rollComponents.Y < 0f)
-                m_lastAngularVelocity *= Vector3.One - decayamount;
+                    turnComponent = -turnComponent;
-                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
+                // TODO: there must be a better computation of the banking force.
-                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
+                float bankingTurnForce = turnComponent;
+                //        actual error  =       static turn error            +           dynamic turn error
+                float mixedBankingError = bankingTurnForce * (1f - m_bankingMix) + bankingTurnForce * m_bankingMix * VehicleForwardSpeed;
+                // TODO: the banking effect should not go to infinity but what to limit it to?
+                mixedBankingError = ClampInRange(-20f, mixedBankingError, 20f);
+                // Build the force vector to change rotation from what it is to what it should be
+                ret.Z = -mixedBankingError;
+                // Don't do it all at once.
+                ret /= m_bankingTimescale;
+                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},turnComp={3},bankErr={4},mixedBankErr={5},ret={6}",
+                            Prim.LocalID, rollComponents, VehicleForwardSpeed, turnComponent, bankingTurnForce, mixedBankingError, ret);
            }
-        } //end MoveAngular
+            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)
            {
@@ -1063,12 +1299,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            }
            if (rotq != m_rot)
            {
-                Prim.ForceOrientation = m_rot;
+                VehicleOrientation = m_rot;
-                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, 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/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 0df4310..2017fa5 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,6 +32,15 @@ using OMV = OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
+// A BSPrim can get individual information about its linkedness attached
+//    to it through an instance of a subclass of LinksetInfo.
+// Each type of linkset will define the information needed for its type.
+public abstract class BSLinksetInfo
+{
+    public virtual void Clear() { }
+}
 public abstract class BSLinkset
 {
    // private static string LogHeader = "[BULLETSIM LINKSET]";
@@ -87,13 +96,6 @@ public abstract class BSLinkset
        return BSPhysicsShapeType.SHAPE_UNKNOWN;
    }
-    // Linksets move around the children so the linkset might need to compute the child position
-    public virtual OMV.Vector3 Position(BSPhysObject member)
-        { return member.RawPosition; }
-    public virtual OMV.Quaternion Orientation(BSPhysObject member)
-        { return member.RawOrientation; }
-    // TODO: does this need to be done for Velocity and RotationalVelocityy?
-    
    // We keep the prim's mass in the linkset structure since it could be dependent on other prims
    protected float m_mass;
    public float LinksetMass
@@ -116,7 +118,7 @@ public abstract class BSLinkset
        get { return ComputeLinksetGeometricCenter(); }
    }
-    protected void Initialize(BSScene scene, BSPhysObject parent)
+    protected BSLinkset(BSScene scene, BSPhysObject parent)
    {
        // A simple linkset of one (no children)
        LinksetID = m_nextLinksetID++;
@@ -127,6 +129,7 @@ public abstract class BSLinkset
        LinksetRoot = parent;
        m_children = new HashSet<BSPhysObject>();
        m_mass = parent.RawMass;
+        Rebuilding = false;
    }
    // Link to a linkset where the child knows the parent.
@@ -219,7 +222,7 @@ public abstract class BSLinkset
    // I am the root of a linkset and a new child is being added
    // Called while LinkActivity is locked.
    protected abstract void AddChildToLinkset(BSPhysObject child);
+    
    // I am the root of a linkset and one of my children is being removed.
    // Safe to call even if the child is not really in my linkset.
    protected abstract void RemoveChildFromLinkset(BSPhysObject child);
@@ -229,6 +232,10 @@ public abstract class BSLinkset
    // May be called at runtime or taint-time.
    public abstract void Refresh(BSPhysObject requestor);
+    // Flag denoting the linkset is in the process of being rebuilt.
+    // Used to know not the schedule a rebuild in the middle of a rebuild.
+    protected bool Rebuilding { get; set; }
    // The object is going dynamic (physical). Do any setup necessary
    //     for a dynamic linkset.
    // Only the state of the passed object can be modified. The rest of the linkset
@@ -245,8 +252,9 @@ public abstract class BSLinkset
    // Called when a parameter update comes from the physics engine for any object
    //      of the linkset is received.
+    // Passed flag is update came from physics engine (true) or the user (false).
    // Called at taint-time!!
-    public abstract void UpdateProperties(BSPhysObject physObject);
+    public abstract void UpdateProperties(BSPhysObject physObject, bool physicalUpdate);
    // Routine used when rebuilding the body of the root of the linkset
    // Destroy all the constraints have have been made to root.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index b9c2cf9..2a7b72c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -28,22 +28,54 @@ using System;
 using System.Collections.Generic;
 using System.Text;
+using OpenSim.Framework;
 using OMV = OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
+// When a child is linked, the relationship position of the child to the parent
+//    is remembered so the child's world position can be recomputed when it is
+//    removed from the linkset.
+sealed class BSLinksetCompoundInfo : BSLinksetInfo
+{
+    public OMV.Vector3 OffsetPos;
+    public OMV.Quaternion OffsetRot;
+    public BSLinksetCompoundInfo(OMV.Vector3 p, OMV.Quaternion r)
+    {
+        OffsetPos = p;
+        OffsetRot = r;
+    }
+    public override void Clear()
+    {
+        OffsetPos = OMV.Vector3.Zero;
+        OffsetRot = OMV.Quaternion.Identity;
+    }
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<p=");
+        buff.Append(OffsetPos.ToString());
+        buff.Append(",r=");
+        buff.Append(OffsetRot.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
+};
 public sealed class BSLinksetCompound : BSLinkset
 {
    private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
-    public BSLinksetCompound(BSScene scene, BSPhysObject parent)
+    public BSLinksetCompound(BSScene scene, BSPhysObject parent) : base(scene, parent)
    {
-        base.Initialize(scene, parent);
    }
    // For compound implimented linksets, if there are children, use compound shape for the root.
    public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
    { 
+        // Returning 'unknown' means we don't have a preference.
        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
        if (IsRoot(requestor) && HasAnyChildren)
        {
@@ -55,23 +87,27 @@ public sealed class BSLinksetCompound : BSLinkset
    // When physical properties are changed the linkset needs to recalculate
    //   its internal properties.
-    // This is queued in the 'post taint' queue so the
-    //   refresh will happen once after all the other taints are applied.
    public override void Refresh(BSPhysObject requestor)
    {
-        // External request for Refresh (from BSPrim) is not necessary
+        // Something changed so do the rebuilding thing
-        // InternalRefresh(requestor);
+        // ScheduleRebuild();
    }
-    private void InternalRefresh(BSPhysObject requestor)
+    // Schedule a refresh to happen after all the other taint processing.
+    private void ScheduleRebuild(BSPhysObject requestor)
    {
-        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID);
+        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,rebuilding={1}", 
-        // Queue to happen after all the other taint processing
+                            requestor.LocalID, Rebuilding);
-        PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate()
+        // When rebuilding, it is possible to set properties that would normally require a rebuild.
+        //    If already rebuilding, don't request another rebuild.
+        if (!Rebuilding)
        {
-            if (IsRoot(requestor) && HasAnyChildren)
+            PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", LinksetRoot.LocalID, delegate()
-                RecomputeLinksetCompound();
+            {
-        });
+                if (HasAnyChildren)
+                    RecomputeLinksetCompound();
+            });
+        }
    }
    // The object is going dynamic (physical). Do any setup necessary
@@ -84,12 +120,23 @@ public sealed class BSLinksetCompound : BSLinkset
    {
        bool ret = false;
        DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
-        if (!IsRoot(child))
+        if (IsRoot(child))
+        {
+            // The root is going dynamic. Make sure mass is properly set.
+            m_mass = ComputeLinksetMass();
+            ScheduleRebuild(LinksetRoot);
+        }
+        else
        {
-            // Physical children are removed from the world as the shape ofthe root compound
+            // The origional prims are removed from the world as the shape of the root compound
            //     shape takes over.
            BulletSimAPI.AddToCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
            BulletSimAPI.ForceActivationState2(child.PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            // We don't want collisions from the old linkset children.
+            BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            child.PhysBody.collisionType = CollisionType.LinksetChild;
            ret = true;
        }
        return ret;
@@ -104,33 +151,39 @@ public sealed class BSLinksetCompound : BSLinkset
    {
        bool ret = false;
        DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
-        if (!IsRoot(child))
+        if (IsRoot(child))
+        {
+            ScheduleRebuild(LinksetRoot);
+        }
+        else
        {
            // The non-physical children can come back to life.
            BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            // Don't force activation so setting of DISABLE_SIMULATION can stay.
+            child.PhysBody.collisionType = CollisionType.LinksetChild;
+            // Don't force activation so setting of DISABLE_SIMULATION can stay if used.
            BulletSimAPI.Activate2(child.PhysBody.ptr, false);
            ret = true;
        }
        return ret;
    }
-    // Called at taint-time!!
+    public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate)
-    public override void UpdateProperties(BSPhysObject updated)
    {
-        // Nothing to do for constraints on property updates
+        // The user moving a child around requires the rebuilding of the linkset compound shape
-    }
+        // One problem is this happens when a border is crossed -- the simulator implementation
+        //    is to store the position into the group which causes the move of the object
-    // The children move around in relationship to the root.
+        //    but it also means all the child positions get updated.
-    // Just grab the current values of wherever it is right now.
+        //    What would cause an unnecessary rebuild so we make sure the linkset is in a
-    public override OMV.Vector3 Position(BSPhysObject member)
+        //    region before bothering to do a rebuild.
-    {
+        if (!IsRoot(updated) 
-        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
+                && !physicalUpdate 
-    }
+                && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
+        {
-    public override OMV.Quaternion Orientation(BSPhysObject member)
+            updated.LinksetInfo = null;
-    {
+            ScheduleRebuild(updated);
-        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+        }
    }
    // Routine called when rebuilding the body of some member of the linkset.
@@ -146,20 +199,58 @@ public sealed class BSLinksetCompound : BSLinkset
        if (!IsRoot(child))
        {
-            // Cause the current shape to be freed and the new one to be built.
+            // Because it is a convenient time, recompute child world position and rotation based on
-            InternalRefresh(LinksetRoot);
+            //    its position in the linkset.
-            ret = true;
+            RecomputeChildWorldPosition(child, true);
        }
+        // Cannot schedule a refresh/rebuild here because this routine is called when
+        //     the linkset is being rebuilt.
+        // InternalRefresh(LinksetRoot);
        return ret;
    }
    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    // this routine will restore the removed constraints.
+    //     this routine will restore the removed constraints.
    // Called at taint-time!!
    public override void RestoreBodyDependencies(BSPrim child)
    {
-        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
+    }
+    // When the linkset is built, the child shape is added to the compound shape relative to the
+    //    root shape. The linkset then moves around but this does not move the actual child
+    //    prim. The child prim's location must be recomputed based on the location of the root shape.
+    private void RecomputeChildWorldPosition(BSPhysObject child, bool inTaintTime)
+    {
+        BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
+        if (lci != null)
+        {
+            if (inTaintTime)
+            {
+                OMV.Vector3 oldPos = child.RawPosition;
+                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
+                DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
+                                            child.LocalID, oldPos, lci, child.RawPosition);
+            }
+            else
+            {
+                // TaintedObject is not used here so the raw position is set now and not at taint-time.
+                child.Position = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
+            }
+        }
+        else
+        {
+            // This happens when children have been added to the linkset but the linkset
+            //     has not been constructed yet. So like, at taint time, adding children to a linkset
+            //     and then changing properties of the children (makePhysical, for instance)
+            //     but the post-print action of actually rebuilding the linkset has not yet happened.
+            // PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}",
+            //                                 LogHeader, child.LocalID);
+            DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID);
+        }
    }
    // ================================================================
@@ -174,14 +265,14 @@ public sealed class BSLinksetCompound : BSLinkset
            DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
-            // Cause constraints and assorted properties to be recomputed before the next simulation step.
+            // Rebuild the compound shape with the new child shape included
-            InternalRefresh(LinksetRoot);
+            ScheduleRebuild(child);
        }
        return;
    }
    // Remove the specified child from the linkset.
-    // Safe to call even if the child is not really in my linkset.
+    // Safe to call even if the child is not really in the linkset.
    protected override void RemoveChildFromLinkset(BSPhysObject child)
    {
        if (m_children.Remove(child))
@@ -192,6 +283,7 @@ public sealed class BSLinksetCompound : BSLinkset
                            child.LocalID, child.PhysBody.ptr.ToString("X"));
            // Cause the child's body to be rebuilt and thus restored to normal operation
+            RecomputeChildWorldPosition(child, false);
            child.ForceBodyShapeRebuild(false);
            if (!HasAnyChildren)
@@ -201,8 +293,8 @@ public sealed class BSLinksetCompound : BSLinkset
            }
            else
            {
-                // Schedule a rebuild of the linkset  before the next simulation tick.
+                // Rebuild the compound shape with the child removed
-                InternalRefresh(LinksetRoot);
+                ScheduleRebuild(child);
            }
        }
        return;
@@ -215,54 +307,83 @@ public sealed class BSLinksetCompound : BSLinkset
    // Called at taint time!!
    private void RecomputeLinksetCompound()
    {
-        // Cause the root shape to be rebuilt as a compound object with just the root in it
+        try
-        LinksetRoot.ForceBodyShapeRebuild(true);
+        {
+            // Suppress rebuilding while rebuilding
+            Rebuilding = true;
-        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
+            // Cause the root shape to be rebuilt as a compound object with just the root in it
-                        LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+            LinksetRoot.ForceBodyShapeRebuild(true);
-        // Add a shape for each of the other children in the linkset
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
-        ForEachMember(delegate(BSPhysObject cPrim)
+                            LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
-        {
-            if (!IsRoot(cPrim))
+            // Add a shape for each of the other children in the linkset
+            ForEachMember(delegate(BSPhysObject cPrim)
            {
-                // Each child position and rotation is given relative to the root.
+                if (!IsRoot(cPrim))
-                OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+                {
-                OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                    // Compute the displacement of the child from the root of the linkset.
-                OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+                    // This info is saved in the child prim so the relationship does not
+                    //    change over time and the new child position can be computed
+                    //    when the linkset is being disassembled (the linkset may have moved).
+                    BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
+                    if (lci == null)
+                    {
+                        // Each child position and rotation is given relative to the root.
+                        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+                        OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                        OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+                        // Save relative position for recomputing child's world position after moving linkset.
+                        lci = new BSLinksetCompoundInfo(displacementPos, displacementRot);
+                        cPrim.LinksetInfo = lci;
+                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
+                    }
-                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
+                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
-                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot);
+                        LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
-                if (cPrim.PhysShape.isNativeShape)
+                    if (cPrim.PhysShape.isNativeShape)
-                {
+                    {
-                    // Native shapes are not shared so we need to create a new one.
+                        // A native shape is turning into a hull collision shape because native
-                    // A mesh or hull is created because scale is not available on a native shape.
+                        //    shapes are not shared so we have to hullify it so it will be tracked
-                    //     (TODO: Bullet does have a btScaledCollisionShape. Can that be used?)
+                        //    and freed at the correct time. This also solves the scaling problem
-                    BulletShape saveShape = cPrim.PhysShape;
+                        //    (native shapes scaled but hull/meshes are assumed to not be).
-                    cPrim.PhysShape.ptr = IntPtr.Zero;  // Don't let the create free the child's shape
+                        // TODO: decide of the native shape can just be used in the compound shape.
-                    PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
+                        //    Use call to CreateGeomNonSpecial().
-                    BulletShape newShape = cPrim.PhysShape;
+                        BulletShape saveShape = cPrim.PhysShape;
-                    cPrim.PhysShape = saveShape;
+                        cPrim.PhysShape.Clear();        // Don't let the create free the child's shape
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot);
+                        // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null);
-                }
+                        PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
-                else
+                        BulletShape newShape = cPrim.PhysShape;
-                {
+                        cPrim.PhysShape = saveShape;
-                    // For the shared shapes (meshes and hulls), just use the shape in the child.
+                        BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, lci.OffsetPos, lci.OffsetRot);
-                    if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
+                    }
+                    else
                    {
-                        PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
+                        // For the shared shapes (meshes and hulls), just use the shape in the child.
-                                            LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
+                        // The reference count added here will be decremented when the compound shape
+                        //     is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
+                        if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
+                        {
+                            PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
+                                                LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
+                        }
+                        BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, lci.OffsetPos, lci.OffsetRot);
                    }
-                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
                }
-            }
+                return false;   // 'false' says to move onto the next child in the list
-            return false;   // 'false' says to move onto the next child in the list
+            });
-        });
-        // With all of the linkset packed into the root prim, it has the mass of everyone.
+            // With all of the linkset packed into the root prim, it has the mass of everyone.
-        float linksetMass = LinksetMass;
+            float linksetMass = LinksetMass;
-        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+            LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+        }
+        finally
+        {
+            Rebuilding = false;
+        }
        BulletSimAPI.RecalculateCompoundShapeLocalAabb2(LinksetRoot.PhysShape.ptr);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index c855fda..8c36c31 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,9 +36,8 @@ public sealed class BSLinksetConstraints : BSLinkset
 {
    // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
-    public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
+    public BSLinksetConstraints(BSScene scene, BSPhysObject parent) : base(scene, parent)
    {
-        base.Initialize(scene, parent);
    }
    // When physical properties are changed the linkset needs to recalculate
@@ -79,23 +78,11 @@ public sealed class BSLinksetConstraints : BSLinkset
    }
    // Called at taint-time!!
-    public override void UpdateProperties(BSPhysObject updated)
+    public override void UpdateProperties(BSPhysObject updated, bool inTaintTime)
    {
        // Nothing to do for constraints on property updates
    }
-    // The children of the linkset are moved around by the constraints.
-    // Just grab the current values of wherever it is right now.
-    public override OMV.Vector3 Position(BSPhysObject member)
-    {
-        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
-    }
-    public override OMV.Quaternion Orientation(BSPhysObject member)
-    {
-        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
-    }
    // Routine called when rebuilding the body of some member of the linkset.
    // Destroy all the constraints have have been made to root and set
    //     up to rebuild the constraints before the next simulation step.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
new file mode 100755
index 0000000..c113a43
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
@@ -0,0 +1,200 @@
+/*
+ * 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 copyrightD
+ *       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;
+using System.Reflection;
+using Nini.Config;
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public struct MaterialAttributes
+{
+    // Material type values that correspond with definitions for LSL
+    public enum Material : int
+    {
+        Stone = 0,
+        Metal,
+        Glass,
+        Wood,
+        Flesh,
+        Plastic,
+        Rubber,
+        Light,
+        // Hereafter are BulletSim additions
+        Avatar,
+        NumberOfTypes   // the count of types in the enum.
+    }
+    // Names must be in the order of the above enum.
+    // These names must coorespond to the lower case field names in the MaterialAttributes
+    //     structure as reflection is used to select the field to put the value in.
+    public static readonly string[] MaterialAttribs = { "Density", "Friction", "Restitution"};
+    public MaterialAttributes(string t, float d, float f, float r)
+    {
+        type = t;
+        density = d;
+        friction = f;
+        restitution = r;
+    }
+    public string type;
+    public float density;
+    public float friction;
+    public float restitution;
+}
+public static class BSMaterials
+{
+    // Attributes for each material type
+    private static readonly MaterialAttributes[] Attributes;
+    // Map of material name to material type code
+    public static readonly Dictionary<string, MaterialAttributes.Material> MaterialMap;
+    static BSMaterials()
+    {
+        // Attribute sets for both the non-physical and physical instances of materials.
+        Attributes = new MaterialAttributes[(int)MaterialAttributes.Material.NumberOfTypes * 2];
+        // Map of name to type code.
+        MaterialMap = new Dictionary<string, MaterialAttributes.Material>();
+        MaterialMap.Add("Stone", MaterialAttributes.Material.Stone);
+        MaterialMap.Add("Metal", MaterialAttributes.Material.Metal);
+        MaterialMap.Add("Glass", MaterialAttributes.Material.Glass);
+        MaterialMap.Add("Wood", MaterialAttributes.Material.Wood);
+        MaterialMap.Add("Flesh", MaterialAttributes.Material.Flesh);
+        MaterialMap.Add("Plastic", MaterialAttributes.Material.Plastic);
+        MaterialMap.Add("Rubber", MaterialAttributes.Material.Rubber);
+        MaterialMap.Add("Light", MaterialAttributes.Material.Light);
+        MaterialMap.Add("Avatar", MaterialAttributes.Material.Avatar);
+    }
+    // This is where all the default material attributes are defined.
+    public static void InitializeFromDefaults(ConfigurationParameters parms)
+    {
+        // Values from http://wiki.secondlife.com/wiki/PRIM_MATERIAL
+        float dDensity = parms.defaultDensity;
+        float dFriction = parms.defaultFriction;
+        float dRestitution = parms.defaultRestitution;
+        Attributes[(int)MaterialAttributes.Material.Stone] =
+                    new MaterialAttributes("stone",dDensity, 0.8f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Metal] =
+                    new MaterialAttributes("metal",dDensity, 0.3f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Glass] =
+                    new MaterialAttributes("glass",dDensity, 0.2f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Wood] =
+                    new MaterialAttributes("wood",dDensity, 0.6f, 0.5f);
+        Attributes[(int)MaterialAttributes.Material.Flesh] =
+                    new MaterialAttributes("flesh",dDensity, 0.9f, 0.3f);
+        Attributes[(int)MaterialAttributes.Material.Plastic] =
+                    new MaterialAttributes("plastic",dDensity, 0.4f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Rubber] =
+                    new MaterialAttributes("rubber",dDensity, 0.9f, 0.9f);
+        Attributes[(int)MaterialAttributes.Material.Light] =
+                    new MaterialAttributes("light",dDensity, dFriction, dRestitution);
+        Attributes[(int)MaterialAttributes.Material.Avatar] =
+                    new MaterialAttributes("avatar",60f, 0.2f, 0f);
+        Attributes[(int)MaterialAttributes.Material.Stone + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("stonePhysical",dDensity, 0.8f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Metal + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("metalPhysical",dDensity, 0.8f, 0.4f);
+        Attributes[(int)MaterialAttributes.Material.Glass + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("glassPhysical",dDensity, 0.8f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Wood + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("woodPhysical",dDensity, 0.8f, 0.5f);
+        Attributes[(int)MaterialAttributes.Material.Flesh + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("fleshPhysical",dDensity, 0.8f, 0.3f);
+        Attributes[(int)MaterialAttributes.Material.Plastic + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("plasticPhysical",dDensity, 0.8f, 0.7f);
+        Attributes[(int)MaterialAttributes.Material.Rubber + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("rubberPhysical",dDensity, 0.8f, 0.9f);
+        Attributes[(int)MaterialAttributes.Material.Light + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("lightPhysical",dDensity, dFriction, dRestitution);
+        Attributes[(int)MaterialAttributes.Material.Avatar + (int)MaterialAttributes.Material.NumberOfTypes] =
+                    new MaterialAttributes("avatarPhysical",60f, 0.2f, 0f);
+    }
+    // Under the [BulletSim] section, one can change the individual material
+    //    attribute values. The format of the configuration parameter is:
+    //        <materialName><Attribute>["Physical"] = floatValue
+    //    For instance:
+    //        [BulletSim]
+    //             StoneFriction = 0.2
+    //             FleshRestitutionPhysical = 0.8
+    // Materials can have different parameters for their static and
+    //    physical instantiations. When setting the non-physical value,
+    //    both values are changed. Setting the physical value only changes
+    //    the physical value.
+    public static void InitializefromParameters(IConfig pConfig)
+    {
+        foreach (KeyValuePair<string, MaterialAttributes.Material> kvp in MaterialMap)
+        {
+            string matName = kvp.Key;
+            foreach (string attribName in MaterialAttributes.MaterialAttribs)
+            {
+                string paramName = matName + attribName;
+                if (pConfig.Contains(paramName))
+                {
+                    float paramValue = pConfig.GetFloat(paramName);
+                    SetAttributeValue((int)kvp.Value, attribName, paramValue);
+                    // set the physical value also
+                    SetAttributeValue((int)kvp.Value + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);
+                }
+                paramName += "Physical";
+                if (pConfig.Contains(paramName))
+                {
+                    float paramValue = pConfig.GetFloat(paramName);
+                    SetAttributeValue((int)kvp.Value + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);
+                }
+            }
+        }
+    }
+    // Use reflection to set the value in the attribute structure.
+    private static void SetAttributeValue(int matType, string attribName, float val)
+    {
+        MaterialAttributes thisAttrib = Attributes[matType];
+        FieldInfo fieldInfo = thisAttrib.GetType().GetField(attribName.ToLower());
+        if (fieldInfo != null)
+        {
+            fieldInfo.SetValue(thisAttrib, val);
+            Attributes[matType] = thisAttrib;
+        }
+    }
+    // Given a material type, return a structure of attributes.
+    public static MaterialAttributes GetAttributes(MaterialAttributes.Material type, bool isPhysical)
+    {
+        int ind = (int)type;
+        if (isPhysical) ind += (int)MaterialAttributes.Material.NumberOfTypes;
+        return Attributes[ind];
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index bc6e4c4..e0faf4e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -1,104 +1,220 @@
-using System;
+/*
-using System.Collections.Generic;
+ * Copyright (c) Contributors, http://opensimulator.org/
-using System.Text;
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
-using OpenMetaverse;
+ *
+ * Redistribution and use in source and binary forms, with or without
-namespace OpenSim.Region.Physics.BulletSPlugin
+ * modification, are permitted provided that the following conditions are met:
-{
+ *     * Redistributions of source code must retain the above copyright
-public abstract class BSMotor
+ *       notice, this list of conditions and the following disclaimer.
-{
+ *     * Redistributions in binary form must reproduce the above copyright
-    public virtual void Reset() { }
+ *       notice, this list of conditions and the following disclaimer in the
-    public virtual void Zero() { }
+ *       documentation and/or other materials provided with the distribution.
-}
+ *     * Neither the name of the OpenSimulator Project nor the
-// Can all the incremental stepping be replaced with motor classes?
+ *       names of its contributors may be used to endorse or promote products
-public class BSVMotor : BSMotor
+ *       derived from this software without specific prior written permission.
-{
+ *
-    public Vector3 FrameOfReference { get; set; }
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
-    public Vector3 Offset { get; set; }
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-    public float TimeScale { get; set; }
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
-    public float TargetValueDecayTimeScale { get; set; }
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-    public Vector3 CurrentValueReductionTimescale { get; set; }
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-    public float Efficiency { get; set; }
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-    public Vector3 TargetValue { get; private set; }
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-    public Vector3 CurrentValue { get; private set; }
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+using System;
-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
+using System.Collections.Generic;
-    {
+using System.Text;
-        TimeScale = timeScale;
+using OpenMetaverse;
-        TargetValueDecayTimeScale = decayTimeScale;
-        CurrentValueReductionTimescale = frictionTimeScale;
+namespace OpenSim.Region.Physics.BulletSPlugin
-        Efficiency = efficiency;
+{
-    }
+public abstract class BSMotor
-    public void SetCurrent(Vector3 current)
+{
-    {
+    // Timescales and other things can be turned off by setting them to 'infinite'.
-        CurrentValue = current;
+    public const float Infinite = 12345f;
-    }
+    public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
-    public void SetTarget(Vector3 target)
-    {
+    public BSMotor(string useName)
-        TargetValue = target;
+    {
-    }
+        UseName = useName;
-    public Vector3 Step(float timeStep)
+        PhysicsScene = null;
-    {
+    }
-        if (CurrentValue.LengthSquared() > 0.001f)
+    public virtual void Reset() { }
-        {
+    public virtual void Zero() { }
-            // Vector3 origDir = Target;       // DEBUG
-            // Vector3 origVel = CurrentValue;   // DEBUG
+    // A name passed at motor creation for easily identifyable debugging messages.
+    public string UseName { get; private set; }
-            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete
-            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;
+    // Used only for outputting debug information. Might not be set so check for null.
-            CurrentValue += addAmount;
+    public BSScene PhysicsScene { get; set; }
+    protected void MDetailLog(string msg, params Object[] parms)
-            float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
+    {
-            TargetValue *= (1f - decayFactor);
+        if (PhysicsScene != null)
+        {
-            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;
+            if (PhysicsScene.VehicleLoggingEnabled)
-            CurrentValue *= (Vector3.One - frictionFactor);
+            {
-        }
+                PhysicsScene.DetailLog(msg, parms);
-        else
+            }
-        {
+        }
-            // if what remains of direction is very small, zero it.
+    }
-            TargetValue = Vector3.Zero;
+}
-            CurrentValue = Vector3.Zero;
+// Can all the incremental stepping be replaced with motor classes?
-            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
+// Motor which moves CurrentValue to TargetValue over TimeScale seconds.
-        }
+// The TargetValue decays in TargetValueDecayTimeScale and
-        return CurrentValue;
+//     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,
-public class BSFMotor : BSMotor
+//    CurrentValue is X and TargetValue is Y. As the motor is stepped, new
-{
+//    values of CurrentValue are returned that approach the TargetValue.
-    public float TimeScale { get; set; }
+// The feature of decaying TargetValue is so vehicles will eventually
-    public float DecayTimeScale { get; set; }
+//    come to a stop rather than run forever. This can be disabled by
-    public float Friction { get; set; }
+//    setting TargetValueDecayTimescale to 'infinite'.
-    public float Efficiency { get; set; }
+// The change from CurrentValue to TargetValue is linear over TimeScale seconds.
+public class BSVMotor : BSMotor
-    public float Target { get; private set; }
+{
-    public float CurrentValue { get; private set; }
+    // public Vector3 FrameOfReference { get; set; }
+    // public Vector3 Offset { get; set; }
-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)
-    {
+    public float TimeScale { get; set; }
-    }
+    public float TargetValueDecayTimeScale { get; set; }
-    public void SetCurrent(float target)
+    public Vector3 FrictionTimescale { get; set; }
-    {
+    public float Efficiency { get; set; }
-    }
-    public void SetTarget(float target)
+    public Vector3 TargetValue { get; private set; }
-    {
+    public Vector3 CurrentValue { get; private set; }
-    }
-    public float Step(float timeStep)
+    public BSVMotor(string useName)
-    {
+        : base(useName)
-        return 0f;
+    {
-    }
+        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
-}
+        Efficiency = 1f;
-public class BSPIDMotor : BSMotor
+        FrictionTimescale = BSMotor.InfiniteVector;
-{
+        CurrentValue = TargetValue = Vector3.Zero;
-    // TODO: write and use this one
+    }
-    BSPIDMotor()
+    public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
-    {
+        : this(useName)
-    }
+    {
-}
+        TimeScale = timeScale;
-}
+        TargetValueDecayTimeScale = decayTimeScale;
+        FrictionTimescale = frictionTimeScale;
+        Efficiency = efficiency;
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
+    public void SetCurrent(Vector3 current)
+    {
+        CurrentValue = current;
+    }
+    public void SetTarget(Vector3 target)
+    {
+        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;
+        if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
+        {
+            Vector3 origTarget = TargetValue;       // DEBUG
+            Vector3 origCurrVal = CurrentValue;   // DEBUG
+            // Addition =  (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete
+            Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
+            CurrentValue += addAmount;
+            // The desired value reduces to zero which also reduces the difference with current.
+            // If the decay time is infinite, don't decay at all.
+            float decayFactor = 0f;
+            if (TargetValueDecayTimeScale != BSMotor.Infinite)
+            {
+                decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
+                TargetValue *= (1f - decayFactor);
+            }
+            Vector3 frictionFactor = Vector3.Zero;
+            if (FrictionTimescale != BSMotor.InfiniteVector)
+            {
+                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
+                // Individual friction components can be 'infinite' so compute each separately.
+                frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep;
+                frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep;
+                frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep;
+                CurrentValue *= (Vector3.One - frictionFactor);
+            }
+            returnCurrent = CurrentValue;
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
+                                BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
+                                timeStep, TimeScale, addAmount,
+                                TargetValueDecayTimeScale, decayFactor,
+                                FrictionTimescale, frictionFactor);
+            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},curr={2},target={3},add={4},decay={5},frict={6},ret={7}",
+                                    BSScene.DetailLogZero, UseName, CurrentValue, TargetValue,
+                                    addAmount, decayFactor, frictionFactor, returnCurrent);
+        }
+        else
+        {
+            // Difference between what we have and target is small. Motor is done.
+            CurrentValue = Vector3.Zero;
+            TargetValue = Vector3.Zero;
+            MDetailLog("{0},  BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
+                                    BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
+        }
+        return returnCurrent;
+    }
+    public override string ToString()
+    {
+        return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>",
+            UseName, CurrentValue, TargetValue, TargetValueDecayTimeScale, FrictionTimescale);
+    }
+}
+public class BSFMotor : BSMotor
+{
+    public float TimeScale { get; set; }
+    public float DecayTimeScale { get; set; }
+    public float Friction { get; set; }
+    public float Efficiency { get; set; }
+    public float Target { get; private set; }
+    public float CurrentValue { get; private set; }
+    public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
+        : base(useName)
+    {
+    }
+    public void SetCurrent(float target)
+    {
+    }
+    public void SetTarget(float target)
+    {
+    }
+    public float Step(float timeStep)
+    {
+        return 0f;
+    }
+}
+public class BSPIDMotor : BSMotor
+{
+    // TODO: write and use this one
+    public BSPIDMotor(string useName)
+        : base(useName)
+    {
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index f6a890e..92a5f2f 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -60,6 +60,9 @@ public abstract class BSPhysObject : PhysicsActor
        Linkset = BSLinkset.Factory(PhysicsScene, this);
        LastAssetBuildFailed = false;
+        // Default material type
+        Material = MaterialAttributes.Material.Wood;
        CollisionCollection = new CollisionEventUpdate();
        SubscribedEventsMs = 0;
        CollidingStep = 0;
@@ -72,6 +75,7 @@ public abstract class BSPhysObject : PhysicsActor
    public string TypeName { get; protected set; }
    public BSLinkset Linkset { get; set; }
+    public BSLinksetInfo LinksetInfo { get; set; }
    // Return the object mass without calculating it or having side effects
    public abstract float RawMass { get; }
@@ -105,10 +109,17 @@ public abstract class BSPhysObject : PhysicsActor
    public EntityProperties CurrentEntityProperties { get; set; }
    public EntityProperties LastEntityProperties { get; set; }
-    public abstract OMV.Vector3 Scale { get; set; }
+    public virtual OMV.Vector3 Scale { get; set; }
    public abstract bool IsSolid { get; }
    public abstract bool IsStatic { get; }
+    // Materialness
+    public MaterialAttributes.Material Material { get; private set; }
+    public override void SetMaterial(int material)
+    {
+        Material = (MaterialAttributes.Material)material;
+    }
    // Stop all physical motion.
    public abstract void ZeroMotion(bool inTaintTime);
    public abstract void ZeroAngularMotion(bool inTaintTime);
@@ -128,6 +139,17 @@ public abstract class BSPhysObject : PhysicsActor
    public abstract OMV.Quaternion RawOrientation { get; set; }
    public abstract OMV.Quaternion ForceOrientation { get; set; }
+    // The system is telling us the velocity it wants to move at.
+    protected OMV.Vector3 m_targetVelocity;
+    public override OMV.Vector3 TargetVelocity
+    {
+        get { return m_targetVelocity; }
+        set
+        {
+            m_targetVelocity = value;
+            Velocity = value;
+        }
+    }
    public abstract OMV.Vector3 ForceVelocity { get; set; }
    public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
@@ -192,7 +214,7 @@ public abstract class BSPhysObject : PhysicsActor
    {
        bool ret = true;
        // If the 'no collision' call, force it to happen right now so quick collision_end
-        bool force = CollisionCollection.Count == 0;
+        bool force = (CollisionCollection.Count == 0);
        // throttle the collisions to the number of milliseconds specified in the subscription
        if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
@@ -210,8 +232,10 @@ public abstract class BSPhysObject : PhysicsActor
            // DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
            base.SendCollisionUpdate(CollisionCollection);
-            // The collisionCollection structure is passed around in the simulator.
+            // The CollisionCollection instance is passed around in the simulator.
            // Make sure we don't have a handle to that one and that a new one is used for next time.
+            //    This fixes an interesting 'gotcha'. If we call CollisionCollection.Clear() here, 
+            //    a race condition is created for the other users of this instance.
            CollisionCollection = new CollisionEventUpdate();
        }
        return ret;
@@ -229,7 +253,8 @@ public abstract class BSPhysObject : PhysicsActor
            PhysicsScene.TaintedObject(TypeName+".SubscribeEvents", delegate()
            {
-                CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                if (PhysBody.HasPhysicalBody)
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
            });
        }
        else
@@ -243,7 +268,9 @@ public abstract class BSPhysObject : PhysicsActor
        SubscribedEventsMs = 0;
        PhysicsScene.TaintedObject(TypeName+".UnSubscribeEvents", delegate()
        {
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            // Make sure there is a body there because sometimes destruction happens in an un-ideal order.
+            if (PhysBody.HasPhysicalBody)
+                CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
        });
    }
    // Return 'true' if the simulator wants collision events
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 2b3fa25..758d92b 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -45,7 +45,6 @@ public sealed class BSPrim : BSPhysObject
    private static readonly string LogHeader = "[BULLETS PRIM]";
    // _size is what the user passed. Scale is what we pass to the physics engine with the mesh.
-    // Often Scale is unity because the meshmerizer will apply _size when creating the mesh.
    private OMV.Vector3 _size;  // the multiplier for each mesh dimension as passed by the user
    private bool _grabbed;
@@ -93,7 +92,7 @@ public sealed class BSPrim : BSPhysObject
        _physicsActorType = (int)ActorTypes.Prim;
        _position = pos;
        _size = size;
-        Scale = size;   // the scale will be set by CreateGeom depending on object type
+        Scale = size;   // prims are the size the user wants them to be (different for BSCharactes).
        _orientation = rotation;
        _buoyancy = 1f;
        _velocity = OMV.Vector3.Zero;
@@ -108,8 +107,8 @@ public sealed class BSPrim : BSPhysObject
        _mass = CalculateMass();
        // No body or shape yet
-        PhysBody = new BulletBody(LocalID, IntPtr.Zero);
+        PhysBody = new BulletBody(LocalID);
-        PhysShape = new BulletShape(IntPtr.Zero);
+        PhysShape = new BulletShape();
        DetailLog("{0},BSPrim.constructor,call", LocalID);
        // do the actual object creation at taint time
@@ -143,7 +142,9 @@ public sealed class BSPrim : BSPhysObject
            DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
            // If there are physical body and shape, release my use of same.
            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysBody.Clear();
            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
+            PhysShape.Clear();
        });
    }
@@ -157,12 +158,10 @@ public sealed class BSPrim : BSPhysObject
            // We presume the scale and size are the same. If scale must be changed for
            //     the physical shape, that is done when the geometry is built.
            _size = value;
+            Scale = _size;
            ForceBodyShapeRebuild(false);
        }
    }
-    // Scale is what we set in the physics engine. It is different than 'size' in that
-    //     'size' can be encorporated into the mesh. In that case, the scale is <1,1,1>.
-    public override OMV.Vector3 Scale { get; set; }
    public override PrimitiveBaseShape Shape {
        set {
@@ -189,13 +188,17 @@ public sealed class BSPrim : BSPhysObject
        }
    }
    public override bool Selected {
-        set {
+        set
-            _isSelected = value;
+        {
-            PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
+            if (value != _isSelected)
            {
-                DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
+                _isSelected = value;
-                SetObjectDynamic(false);
+                PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
-            });
+                {
+                    DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
+                    SetObjectDynamic(false);
+                });
+            }
        }
    }
    public override void CrossingFailure() { return; }
@@ -244,7 +247,8 @@ public sealed class BSPrim : BSPhysObject
        // Zero some other properties in the physics engine
        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
        {
-            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.ClearAllForces2(PhysBody.ptr);
        });
    }
    public override void ZeroAngularMotion(bool inTaintTime)
@@ -253,8 +257,12 @@ 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);
+            // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity);
-            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            if (PhysBody.HasPhysicalBody)
+            {
+                BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+                BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+            }
        });
    }
@@ -271,9 +279,12 @@ public sealed class BSPrim : BSPhysObject
    }
    public override OMV.Vector3 Position {
        get {
+            /* NOTE: this refetch is not necessary. The simulator knows about linkset children
+             *    and does not fetch this position info for children. Thus this is commented out.
            // child prims move around based on their parent. Need to get the latest location
            if (!Linkset.IsRoot(this))
-                _position = Linkset.Position(this);
+                _position = Linkset.PositionGet(this);
+            */
            // don't do the GetObjectPosition for root elements because this function is called a zillion times.
            // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
@@ -281,18 +292,22 @@ public sealed class BSPrim : BSPhysObject
        }
        set {
            // If the position must be forced into the physics engine, use ForcePosition.
+            // All positions are given in world positions.
            if (_position == value)
            {
+                DetailLog("{0},BSPrim.setPosition,taint,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
                return;
            }
            _position = value;
-            // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
            PositionSanityCheck(false);
+            // A linkset might need to know if a component information changed.
+            Linkset.UpdateProperties(this, false);
            PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
            {
-                // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                ForcePosition = _position;
-                ActivateIfPhysical(false);
            });
        }
    }
@@ -303,9 +318,11 @@ public sealed class BSPrim : BSPhysObject
        }
        set {
            _position = value;
-            // PositionSanityCheck();   // Don't do this! Causes a loop and caller should know better.
+            if (PhysBody.HasPhysicalBody)
-            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            {
-            ActivateIfPhysical(false);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                ActivateIfPhysical(false);
+            }
        }
    }
@@ -316,39 +333,46 @@ public sealed class BSPrim : BSPhysObject
    {
        bool ret = false;
+        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        {
+            // The physical object is out of the known/simulated area.
+            // Upper levels of code will handle the transition to other areas so, for
+            //     the time, we just ignore the position.
+            return ret;
+        }
        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
        OMV.Vector3 upForce = OMV.Vector3.Zero;
-        if (Position.Z < terrainHeight)
+        if (RawPosition.Z < terrainHeight)
        {
            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
            float targetHeight = terrainHeight + (Size.Z / 2f);
            // Upforce proportional to the distance away from the terrain. Correct the error in 1 sec.
-            upForce.Z = (terrainHeight - Position.Z) * 1f;
+            upForce.Z = (terrainHeight - RawPosition.Z) * 1f;
            ret = true;
        }
        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)
+            if (Math.Abs(RawPosition.Z - waterHeight) > 0.1f)
            {
                // Upforce proportional to the distance away from the water. Correct the error in 1 sec.
-                upForce.Z = (waterHeight - Position.Z) * 1f;
+                upForce.Z = (waterHeight - RawPosition.Z) * 1f;
                ret = true;
            }
        }
-        // TODO: check for out of bounds
        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
+        // TODO: This should be intergrated with a geneal physics action mechanism.
+        // TODO: This should be moderated with PID'ness.
        if (ret)
        {
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck:belowTerrain", delegate()
+            // Apply upforce and overcome gravity.
-            {
+            OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
-                // Apply upforce and overcome gravity.
+            DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
-                ForceVelocity = ForceVelocity + upForce - PhysicsScene.DefaultGravity;
+            AddForce(correctionForce, false, inTaintTime);
-            });
        }
        return ret;
    }
@@ -408,7 +432,8 @@ public sealed class BSPrim : BSPhysObject
            PhysicsScene.TaintedObject("BSPrim.setForce", delegate()
            {
                // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
            });
        }
    }
@@ -502,7 +527,8 @@ public sealed class BSPrim : BSPhysObject
            PhysicsScene.TaintedObject("BSPrim.setVelocity", delegate()
            {
                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
            });
        }
    }
@@ -537,23 +563,32 @@ public sealed class BSPrim : BSPhysObject
    }
    public override OMV.Quaternion Orientation {
        get {
+            /* NOTE: this refetch is not necessary. The simulator knows about linkset children
+             *    and does not fetch this position info for children. Thus this is commented out.
            // Children move around because tied to parent. Get a fresh value.
            if (!Linkset.IsRoot(this))
            {
-                _orientation = Linkset.Orientation(this);
+                _orientation = Linkset.OrientationGet(this);
            }
+             */
            return _orientation;
        }
        set {
            if (_orientation == value)
                return;
            _orientation = value;
-            // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
+            // A linkset might need to know if a component information changed.
+            Linkset.UpdateProperties(this, false);
            PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
            {
-                // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
+                if (PhysBody.HasPhysicalBody)
-                // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                {
-                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                    // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
+                    // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                    BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                }
            });
        }
    }
@@ -644,10 +679,7 @@ 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)
+        PhysBody.ApplyCollisionMask();
-        {
-            BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr, (uint)PhysBody.collisionFilter, (uint)PhysBody.collisionMask);
-        }
        // Recompute any linkset parameters.
        // When going from non-physical to physical, this re-enables the constraints that
@@ -672,8 +704,12 @@ public sealed class BSPrim : BSPhysObject
            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
            // Stop all movement
            ZeroMotion(true);
-            // Center of mass is at the center of the object
-            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
+            // Set various physical properties so other object interact properly
+            MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
+            BulletSimAPI.SetFriction2(PhysBody.ptr, matAttrib.friction);
+            BulletSimAPI.SetRestitution2(PhysBody.ptr, matAttrib.restitution);
            // Mass is zero which disables a bunch of physics stuff in Bullet
            UpdatePhysicalMassProperties(0f);
            // Set collision detection parameters
@@ -682,24 +718,27 @@ public sealed class BSPrim : BSPhysObject
                BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
                BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
            }
-            // 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);
+            // This collides like a static object
+            PhysBody.collisionType = CollisionType.Static;
-            PhysBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
+            // There can be special things needed for implementing linksets
-            PhysBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
+            Linkset.MakeStatic(this);
        }
        else
        {
            // Not a Bullet static object
            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
-            // Set various physical properties so internal dynamic properties will get computed correctly as they are set
+            // Set various physical properties so other object interact properly
-            BulletSimAPI.SetFriction2(PhysBody.ptr, PhysicsScene.Params.defaultFriction);
+            MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, true);
-            BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.defaultRestitution);
+            BulletSimAPI.SetFriction2(PhysBody.ptr, matAttrib.friction);
+            BulletSimAPI.SetRestitution2(PhysBody.ptr, matAttrib.restitution);
            // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
            // Since this can be called multiple times, only zero forces when becoming physical
@@ -727,16 +766,15 @@ public sealed class BSPrim : BSPhysObject
            BulletSimAPI.SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
            BulletSimAPI.SetContactProcessingThreshold2(PhysBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
-            // There might be special things needed for implementing linksets.
+            // This collides like an object.
-            Linkset.MakeDynamic(this);
+            PhysBody.collisionType = CollisionType.Dynamic;
            // Force activation of the object so Bullet will act on it.
            // Must do the ForceActivationState2() to overcome the DISABLE_SIMULATION from static objects.
            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.ACTIVE_TAG);
-            // BulletSimAPI.Activate2(BSBody.ptr, true);
-            PhysBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
+            // There might be special things needed for implementing linksets.
-            PhysBody.collisionMask = CollisionFilterGroups.ObjectMask;
+            Linkset.MakeDynamic(this);
        }
    }
@@ -763,8 +801,9 @@ 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.collisionMask = CollisionFilterGroups.VolumeDetectMask;
+            // Change collision info from a static object to a ghosty collision object
+            PhysBody.collisionType = CollisionType.VolumeDetect;
        }
    }
@@ -839,15 +878,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 {
@@ -856,7 +886,8 @@ public sealed class BSPrim : BSPhysObject
            PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
            {
                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
            });
        }
    }
@@ -891,8 +922,11 @@ public sealed class BSPrim : BSPhysObject
            _buoyancy = value;
            // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
            // Buoyancy is faked by changing the gravity applied to the object
-            float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
+            if (PhysBody.HasPhysicalBody)
-            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            {
+                float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
+                BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            }
        }
    }
@@ -960,7 +994,8 @@ public sealed class BSPrim : BSPhysObject
            }
            DetailLog("{0},BSPrim.AddForce,taint,force={1}", LocalID, fSum);
            if (fSum != OMV.Vector3.Zero)
-                BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
        });
    }
@@ -971,7 +1006,8 @@ public sealed class BSPrim : BSPhysObject
        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyForceImpulse", delegate()
        {
            DetailLog("{0},BSPrim.ApplyForceImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
-            BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
        });
    }
@@ -1007,18 +1043,23 @@ public sealed class BSPrim : BSPhysObject
            DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum);
            if (fSum != OMV.Vector3.Zero)
            {
-                BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
                _torque = fSum;
            }
        });
    }
    // A torque impulse.
+    // ApplyTorqueImpulse adds torque directly to the angularVelocity.
+    // AddAngularForce accumulates the force and applied it to the angular velocity all at once.
+    // Computed as: angularVelocity += impulse * inertia;
    public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
    {
        OMV.Vector3 applyImpulse = impulse;
        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
        {
-            BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
+            if (PhysBody.HasPhysicalBody)
+                BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
        });
    }
@@ -1326,7 +1367,7 @@ public sealed class BSPrim : BSPhysObject
    // Rebuild the geometry and object.
    // This is called when the shape changes so we need to recreate the mesh/hull.
    // Called at taint-time!!!
-    private void CreateGeomAndObject(bool forceRebuild)
+    public void CreateGeomAndObject(bool forceRebuild)
    {
        // If this prim is part of a linkset, we must remove and restore the physical
        //    links if the body is rebuilt.
@@ -1336,12 +1377,11 @@ public sealed class BSPrim : BSPhysObject
        // Create the correct physical representation for this type of object.
        // Updates PhysBody and PhysShape with the new information.
        // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
-        // Returns 'true' if either the body or the shape was changed.
        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, null, delegate(BulletBody dBody)
        {
            // Called if the current prim body is about to be destroyed.
            // Remove all the physical dependencies on the old body.
-            // (Maybe someday make the changing of BSShape an event handled by BSLinkset.)
+            // (Maybe someday make the changing of BSShape an event to be subscribed to by BSLinkset, ...)
            needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
        });
@@ -1381,54 +1421,16 @@ public sealed class BSPrim : BSPhysObject
    public override void UpdateProperties(EntityProperties entprop)
    {
-        /*
+        // Updates only for individual prims and for the root object of a linkset.
-        UpdatedProperties changed = 0;
+        if (Linkset.IsRoot(this))
-        // assign to the local variables so the normal set action does not happen
-        // if (_position != entprop.Position)
-        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-        {
-            _position = entprop.Position;
-            changed |= UpdatedProperties.Position;
-        }
-        // if (_orientation != entprop.Rotation)
-        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-        {
-            _orientation = entprop.Rotation;
-            changed |= UpdatedProperties.Rotation;
-        }
-        // if (_velocity != entprop.Velocity)
-        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-        {
-            _velocity = entprop.Velocity;
-            changed |= UpdatedProperties.Velocity;
-        }
-        // if (_acceleration != entprop.Acceleration)
-        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-        {
-            _acceleration = entprop.Acceleration;
-            changed |= UpdatedProperties.Acceleration;
-        }
-        // if (_rotationalVelocity != entprop.RotationalVelocity)
-        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-        {
-            _rotationalVelocity = entprop.RotationalVelocity;
-            changed |= UpdatedProperties.RotationalVel;
-        }
-        if (changed != 0)
        {
-            // Only update the position of single objects and linkset roots
+            // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
-            if (Linkset.IsRoot(this))
+            // TODO: handle physics introduced by Bullet with computed vehicle physics.
+            if (_vehicle.IsActive)
            {
-                base.RequestPhysicsterseUpdate();
+                entprop.RotationalVelocity = OMV.Vector3.Zero;
            }
-        }
-        */
-        // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
-        // Updates only for individual prims and for the root object of a linkset.
-        if (Linkset.IsRoot(this))
-        {
            // Assign directly to the local variables so the normal set action does not happen
            _position = entprop.Position;
            _orientation = entprop.Rotation;
@@ -1437,21 +1439,19 @@ public sealed class BSPrim : BSPhysObject
            _rotationalVelocity = entprop.RotationalVelocity;
            // The sanity check can change the velocity and/or position.
-            if (PositionSanityCheck(true))
+            if (IsPhysical && PositionSanityCheck(true))
            {
                entprop.Position = _position;
                entprop.Velocity = _velocity;
            }
-            // remember the current and last set values
+            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
-            LastEntityProperties = CurrentEntityProperties;
-            CurrentEntityProperties = entprop;
-            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
                    LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
-            // BulletSimAPI.DumpRigidBody2(PhysicsScene.World.ptr, BSBody.ptr);   // DEBUG DEBUG DEBUG
+            // remember the current and last set values
+            LastEntityProperties = CurrentEntityProperties;
+            CurrentEntityProperties = entprop;
            base.RequestPhysicsterseUpdate();
        }
@@ -1466,7 +1466,7 @@ public sealed class BSPrim : BSPhysObject
             */
        // The linkset implimentation might want to know about this.
-        Linkset.UpdateProperties(this);
+        Linkset.UpdateProperties(this, true);
    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 27a78d1..069cb0d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -39,23 +39,10 @@ using log4net;
 using OpenMetaverse;
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
-// Test sculpties (verified that they don't work)
-// Compute physics FPS reasonably
 // Based on material, set density and friction
-// Don't use constraints in linksets of non-physical objects. Means having to move children manually.
-// Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly?
-//     In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground)
-//     At the moment, physical and phantom causes object to drop through the terrain
-// Physical phantom objects and related typing (collision options )
-// Check out llVolumeDetect. Must do something for that.
-// Use collision masks for collision with terrain and phantom objects
 // More efficient memory usage when passing hull information from BSPrim to BulletSim
-// Should prim.link() and prim.delink() membership checking happen at taint time?
-// Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once.
 // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
 // Implement LockAngularMotion
-// Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
-// Remove mesh and Hull stuff. Use mesh passed to bullet and use convexdecom from bullet.
 // Add PID movement operations. What does ScenePresence.MoveToTarget do?
 // Check terrain size. 128 or 127?
 // Raycast
@@ -109,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; }
@@ -140,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
@@ -195,8 +185,10 @@ 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; }
+    public bool VehiclePhysicalLoggingEnabled { get; private set; }
    #region Construction and Initialization
    public BSScene(string identifier)
@@ -234,6 +226,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        if (m_physicsLoggingEnabled)
        {
            PhysicsLogging = new Logging.LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
+            PhysicsLogging.ErrorLogger = m_log; // for DEBUG. Let's the logger output error messages.
        }
        else
        {
@@ -302,12 +295,22 @@ 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);
+                VehiclePhysicalLoggingEnabled = pConfig.GetBoolean("VehiclePhysicalLoggingEnabled", false);
                // Do any replacements in the parameters
                m_physicsLoggingPrefix = m_physicsLoggingPrefix.Replace("%REGIONNAME%", RegionName);
            }
+            // The material characteristics.
+            BSMaterials.InitializeFromDefaults(Params);
+            if (pConfig != null)
+            {
+                // Let the user add new and interesting material property values.
+                BSMaterials.InitializefromParameters(pConfig);
+            }
        }
    }
@@ -345,8 +348,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        // make sure no stepping happens while we're deleting stuff
        m_initialized = false;
-        TerrainManager.ReleaseGroundPlaneAndTerrain();
        foreach (KeyValuePair<uint, BSPhysObject> kvp in PhysObjects)
        {
            kvp.Value.Destroy();
@@ -366,6 +367,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            Shapes = null;
        }
+        if (TerrainManager != null)
+        {
+            TerrainManager.ReleaseGroundPlaneAndTerrain();
+            TerrainManager.Dispose();
+            TerrainManager = null;
+        }
        // Anything left in the unmanaged code should be cleaned out
        BulletSimAPI.Shutdown2(World.ptr);
@@ -490,8 +498,10 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        ProcessTaints();
        // Some of the prims operate with special vehicle properties
-        ProcessVehicles(timeStep);
+        DoPreStepActions(timeStep);
-        ProcessTaints();    // the vehicles might have added taints
+        // the prestep actions might have added taints
+        ProcessTaints();
        // step the physical world one interval
        m_simulationStep++;
@@ -499,16 +509,17 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        try
        {
-            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
+            if (VehiclePhysicalLoggingEnabled) DumpVehicles();  // DEBUG
            if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
            numSubSteps = BulletSimAPI.PhysicsStep2(World.ptr, timeStep, m_maxSubSteps, m_fixedTimeStep,
                        out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
            if (PhysicsLogging.Enabled) simTime = Util.EnvironmentTickCountSubtract(beforeTime);
-            DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}",
+            DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}, objWColl={7}",
-                        DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
+                                    DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, 
-            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
+                                    updatedEntityCount, collidersCount, ObjectsWithCollisions.Count);
+            if (VehiclePhysicalLoggingEnabled) DumpVehicles();  // DEBUG
        }
        catch (Exception e)
        {
@@ -520,9 +531,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            collidersCount = 0;
        }
-        // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in
+        // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in.
-        // Get a value for 'now' so all the collision and update routines don't have to get their own
+        // Get a value for 'now' so all the collision and update routines don't have to get their own.
        SimulationNowTime = Util.EnvironmentTickCount();
        // If there were collisions, process them by sending the event to the prim.
@@ -562,12 +573,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        // Objects that are done colliding are removed from the ObjectsWithCollisions list.
        // Not done above because it is inside an iteration of ObjectWithCollisions.
+        // This complex collision processing is required to create an empty collision
+        //     event call after all collisions have happened on an object. This enables
+        //     the simulator to generate the 'collision end' event.
        if (ObjectsWithNoMoreCollisions.Count > 0)
        {
            foreach (BSPhysObject po in ObjectsWithNoMoreCollisions)
                ObjectsWithCollisions.Remove(po);
            ObjectsWithNoMoreCollisions.Clear();
        }
+        // Done with collisions.
        // If any of the objects had updated properties, tell the object it has been changed by the physics engine
        if (updatedEntityCount > 0)
@@ -585,15 +600,14 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
        ProcessPostStepTaints();
-        // This causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+        // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
        // Only enable this in a limited test world with few objects.
        // BulletSimAPI.DumpAllInfo2(World.ptr);    // DEBUG DEBUG DEBUG
        // The physics engine returns the number of milliseconds it simulated this call.
        // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
-        // We multiply by 55 to give a recognizable running rate (55 or less).
+        // Multiply by 55 to give a nominal frame rate of 55.
-        return numSubSteps * m_fixedTimeStep * 1000 * 55;
+        return (float)numSubSteps * m_fixedTimeStep * 1000f * 55f;
-        // return timeStep * 1000 * 55;
    }
    // Something has collided
@@ -639,12 +653,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
    public override void SetWaterLevel(float baseheight)
    {
-        m_waterLevel = baseheight;
+        SimpleWaterLevel = baseheight;
-    }
-    // Someday....
-    public float GetWaterLevelAtXYZ(Vector3 loc)
-    {
-        return m_waterLevel;
    }
    public override void DeleteTerrain()
@@ -915,6 +924,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)
@@ -979,6 +998,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
@@ -1069,7 +1090,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            (s,p,l,v) => { s.PID_P = v; } ),
        new ParameterDefn("DefaultFriction", "Friction factor used on new objects",
-            0.5f,
+            0.2f,
            (s,cf,p,v) => { s.m_params[0].defaultFriction = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].defaultFriction; },
            (s,p,l,v) => { s.m_params[0].defaultFriction = v; } ),
@@ -1084,7 +1105,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            (s) => { return s.m_params[0].defaultRestitution; },
            (s,p,l,v) => { s.m_params[0].defaultRestitution = v; } ),
        new ParameterDefn("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!)",
-            0f,
+            0.04f,
            (s,cf,p,v) => { s.m_params[0].collisionMargin = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].collisionMargin; },
            (s,p,l,v) => { s.m_params[0].collisionMargin = v; } ),
@@ -1151,7 +1172,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            (s) => { return s.m_params[0].terrainImplementation; },
            (s,p,l,v) => { s.m_params[0].terrainImplementation = v; } ),
        new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
-            0.5f,
+            0.3f,
            (s,cf,p,v) => { s.m_params[0].terrainFriction = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].terrainFriction; },
            (s,p,l,v) => { s.m_params[0].terrainFriction = v;  /* TODO: set on real terrain */} ),
@@ -1165,13 +1186,19 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            (s,cf,p,v) => { s.m_params[0].terrainRestitution = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].terrainRestitution; },
            (s,p,l,v) => { s.m_params[0].terrainRestitution = v;  /* TODO: set on real terrain */ } ),
+        new ParameterDefn("TerrainCollisionMargin", "Margin where collision checking starts" ,
+            0.04f,
+            (s,cf,p,v) => { s.m_params[0].terrainCollisionMargin = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].terrainCollisionMargin; },
+            (s,p,l,v) => { s.m_params[0].terrainCollisionMargin = v;  /* TODO: set on real terrain */ } ),
        new ParameterDefn("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
            0.2f,
            (s,cf,p,v) => { s.m_params[0].avatarFriction = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].avatarFriction; },
            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarFriction, p, l, v); } ),
        new ParameterDefn("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.",
-            10f,
+            10.0f,
            (s,cf,p,v) => { s.m_params[0].avatarStandingFriction = cf.GetFloat(p, v); },
            (s) => { return s.m_params[0].avatarStandingFriction; },
            (s,p,l,v) => { s.m_params[0].avatarStandingFriction = v; } ),
@@ -1206,6 +1233,11 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
            (s) => { return s.m_params[0].avatarContactProcessingThreshold; },
            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
+        new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
+            0.95f,
+            (s,cf,p,v) => { s.m_params[0].vehicleAngularDamping = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].vehicleAngularDamping; },
+            (s,p,l,v) => { s.m_params[0].vehicleAngularDamping = v; } ),
            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
            0f,
@@ -1487,7 +1519,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..d6e2fe9 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -65,9 +65,16 @@ public sealed class BSShapeCollection : IDisposable
    private Dictionary<System.UInt64, MeshDesc> Meshes = new Dictionary<System.UInt64, MeshDesc>();
    private Dictionary<System.UInt64, HullDesc> Hulls = new Dictionary<System.UInt64, HullDesc>();
+    private bool DDetail = false;
    public BSShapeCollection(BSScene physScene)
    {
        PhysicsScene = physScene;
+        // Set the next to 'true' for very detailed shape update detailed logging (detailed details?)
+        // While detailed debugging is still active, this is better than commenting out all the
+        //     DetailLog statements. When debugging slows down, this and the protected logging
+        //     statements can be commented/removed.
+        DDetail = true;
    }
    public void Dispose()
@@ -119,6 +126,11 @@ public sealed class BSShapeCollection : IDisposable
        return ret;
    }
+    public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim)
+    {
+        return GetBodyAndShape(forceRebuild, sim, prim, null, null);
+    }
    // Track another user of a body.
    // We presume the caller has allocated the body.
    // Bodies only have one user so the body is just put into the world if not already there.
@@ -126,13 +138,13 @@ public sealed class BSShapeCollection : IDisposable
    {
        lock (m_collectionActivityLock)
        {
-            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
+            if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
            {
                if (!BulletSimAPI.IsInWorld2(body.ptr))
                {
                    BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, body.ptr);
-                    DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
                }
            });
        }
@@ -142,14 +154,14 @@ public sealed class BSShapeCollection : IDisposable
    // Called when releasing use of a BSBody. BSShape is handled separately.
    public void DereferenceBody(BulletBody body, bool inTaintTime, BodyDestructionCallback bodyCallback )
    {
-        if (body.ptr == IntPtr.Zero)
+        if (!body.HasPhysicalBody)
            return;
        lock (m_collectionActivityLock)
        {
            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceBody", delegate()
            {
-                DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1},inTaintTime={2}",
+                if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1},inTaintTime={2}",
                                            body.ID, body, inTaintTime);
                // If the caller needs to know the old body is going away, pass the event up.
                if (bodyCallback != null) bodyCallback(body);
@@ -157,7 +169,7 @@ public sealed class BSShapeCollection : IDisposable
                if (BulletSimAPI.IsInWorld2(body.ptr))
                {
                    BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
-                    DetailLog("{0},BSShapeCollection.DereferenceBody,removingFromWorld. Body={1}", body.ID, body);
+                    if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,removingFromWorld. Body={1}", body.ID, body);
                }
                // Zero any reference to the shape so it is not freed when the body is deleted.
@@ -184,7 +196,7 @@ public sealed class BSShapeCollection : IDisposable
                {
                    // There is an existing instance of this mesh.
                    meshDesc.referenceCount++;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingMesh,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,existingMesh,key={1},cnt={2}",
                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
                }
                else
@@ -194,7 +206,7 @@ public sealed class BSShapeCollection : IDisposable
                    meshDesc.shapeKey = shape.shapeKey;
                    // We keep a reference to the underlying IMesh data so a hull can be built
                    meshDesc.referenceCount = 1;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
                    ret = true;
                }
@@ -207,7 +219,7 @@ public sealed class BSShapeCollection : IDisposable
                {
                    // There is an existing instance of this hull.
                    hullDesc.referenceCount++;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingHull,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,existingHull,key={1},cnt={2}",
                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
                }
                else
@@ -216,7 +228,7 @@ public sealed class BSShapeCollection : IDisposable
                    hullDesc.ptr = shape.ptr;
                    hullDesc.shapeKey = shape.shapeKey;
                    hullDesc.referenceCount = 1;
-                    DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
                                BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
                    ret = true;
@@ -236,17 +248,17 @@ public sealed class BSShapeCollection : IDisposable
    // Release the usage of a shape.
    public void DereferenceShape(BulletShape shape, bool inTaintTime, ShapeDestructionCallback shapeCallback)
    {
-        if (shape.ptr == IntPtr.Zero)
+        if (!shape.HasPhysicalShape)
            return;
        PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceShape", delegate()
        {
-            if (shape.ptr != IntPtr.Zero)
+            if (shape.HasPhysicalShape)
            {
                if (shape.isNativeShape)
                {
                    // Native shapes are not tracked and are released immediately
-                    DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1},taintTime={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1},taintTime={2}",
                                    BSScene.DetailLogZero, shape.ptr.ToString("X"), inTaintTime);
                    if (shapeCallback != null) shapeCallback(shape);
                    BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
@@ -286,7 +298,7 @@ public sealed class BSShapeCollection : IDisposable
            if (shapeCallback != null) shapeCallback(shape);
            meshDesc.lastReferenced = System.DateTime.Now;
            Meshes[shape.shapeKey] = meshDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
                                BSScene.DetailLogZero, shape, meshDesc.referenceCount);
        }
@@ -307,7 +319,7 @@ public sealed class BSShapeCollection : IDisposable
            hullDesc.lastReferenced = System.DateTime.Now;
            Hulls[shape.shapeKey] = hullDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
                    BSScene.DetailLogZero, shape, hullDesc.referenceCount);
        }
    }
@@ -325,13 +337,13 @@ public sealed class BSShapeCollection : IDisposable
            // Failed the sanity check!!
            PhysicsScene.Logger.ErrorFormat("{0} Attempt to free a compound shape that is not compound!! type={1}, ptr={2}",
                                        LogHeader, shape.type, shape.ptr.ToString("X"));
-            DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
                                        BSScene.DetailLogZero, shape.type, shape.ptr.ToString("X"));
            return;
        }
        int numChildren = BulletSimAPI.GetNumberOfCompoundChildren2(shape.ptr);
-        DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
+        if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
        for (int ii = numChildren - 1; ii >= 0; ii--)
        {
@@ -379,7 +391,7 @@ public sealed class BSShapeCollection : IDisposable
            }
        }
-        DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
+        if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
        if (shapeInfo.type != BSPhysicsShapeType.SHAPE_UNKNOWN)
        {
@@ -408,19 +420,19 @@ public sealed class BSShapeCollection : IDisposable
        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
        {
            // an avatar capsule is close to a native shape (it is not shared)
-            ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
+            GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
                            FixedShapeKey.KEY_CAPSULE, shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
            ret = true;
            haveShape = true;
        }
        // Compound shapes are handled special as they are rebuilt from scratch.
-        // This isn't too great a hardship since most of the child shapes will already been created.
+        // This isn't too great a hardship since most of the child shapes will have already been created.
        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
        {
            ret = GetReferenceToCompoundShape(prim, shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
            haveShape = true;
        }
@@ -433,7 +445,7 @@ public sealed class BSShapeCollection : IDisposable
    }
    // Create a mesh/hull shape or a native shape if 'nativeShapePossible' is 'true'.
-    private bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    public bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
    {
        bool ret = false;
        bool haveShape = false;
@@ -453,19 +465,27 @@ public sealed class BSShapeCollection : IDisposable
                        && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                        && pbs.PathShearX == 0 && pbs.PathShearY == 0) ) )
        {
+            // Get the scale of any existing shape so we can see if the new shape is same native type and same size.
+            OMV.Vector3 scaleOfExistingShape = OMV.Vector3.Zero;
+            if (prim.PhysShape.HasPhysicalShape)
+                scaleOfExistingShape = BulletSimAPI.GetLocalScaling2(prim.PhysShape.ptr);
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,maybeNative,force={1},primScale={2},primSize={3},primShape={4}",
+                        prim.LocalID, forceRebuild, prim.Scale, prim.Size, prim.PhysShape.type);
            // It doesn't look like Bullet scales spheres so make sure the scales are all equal
            if ((pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
                                && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)
            {
                haveShape = true;
                if (forceRebuild
-                        || prim.Scale != prim.Size
+                        || prim.Scale != scaleOfExistingShape
                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_SPHERE
                        )
                {
                    ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
                                            FixedShapeKey.KEY_SPHERE, shapeCallback);
-                    DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
                                        prim.LocalID, forceRebuild, prim.PhysShape);
                }
            }
@@ -473,13 +493,13 @@ public sealed class BSShapeCollection : IDisposable
            {
                haveShape = true;
                if (forceRebuild
-                        || prim.Scale != prim.Size
+                        || prim.Scale != scaleOfExistingShape
                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_BOX
                        )
                {
                    ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
                                            FixedShapeKey.KEY_BOX, shapeCallback);
-                    DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
+                    if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
                                        prim.LocalID, forceRebuild, prim.PhysShape);
                }
            }
@@ -504,13 +524,13 @@ public sealed class BSShapeCollection : IDisposable
        {
            // Update prim.BSShape to reference a hull of this shape.
            ret = GetReferenceToHull(prim,shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
        }
        else
        {
            ret = GetReferenceToMesh(prim, shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
+            if (DDetail) DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
        }
        return ret;
@@ -528,9 +548,10 @@ public sealed class BSShapeCollection : IDisposable
        BulletShape newShape = BuildPhysicalNativeShape(prim, shapeType, shapeKey);
        // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
-        DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}",
+        if (DDetail) DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}",
                                prim.LocalID, newShape, prim.Scale);
+        // native shapes are scaled by Bullet
        prim.PhysShape = newShape;
        return true;
    }
@@ -554,16 +575,14 @@ public sealed class BSShapeCollection : IDisposable
            newShape = new BulletShape(
                        BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1f, 1f, prim.Scale)
                        , shapeType);
-            DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+            if (DDetail) DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
        }
        else
        {
            // Native shapes are scaled in Bullet so set the scaling to the size
-            prim.Scale = prim.Size;
-            nativeShapeData.Scale = prim.Scale;
            newShape = new BulletShape(BulletSimAPI.BuildNativeShape2(PhysicsScene.World.ptr, nativeShapeData), shapeType);
        }
-        if (newShape.ptr == IntPtr.Zero)
+        if (!newShape.HasPhysicalShape)
        {
            PhysicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
                                    LogHeader, prim.LocalID, shapeType);
@@ -580,7 +599,7 @@ public sealed class BSShapeCollection : IDisposable
    // Called at taint-time!
    private bool GetReferenceToMesh(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
    {
-        BulletShape newShape = new BulletShape(IntPtr.Zero);
+        BulletShape newShape = new BulletShape();
        float lod;
        System.UInt64 newMeshKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
@@ -589,7 +608,7 @@ public sealed class BSShapeCollection : IDisposable
        if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_MESH)
            return false;
-        DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
+        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
        // Since we're recreating new, get rid of the reference to the previous shape
@@ -601,8 +620,6 @@ public sealed class BSShapeCollection : IDisposable
        ReferenceShape(newShape);
-        // meshes are already scaled by the meshmerizer
-        prim.Scale = new OMV.Vector3(1f, 1f, 1f);
        prim.PhysShape = newShape;
        return true;        // 'true' means a new shape has been added to this prim
@@ -620,8 +637,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, true, false, false, false);
-            meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, false);
            if (meshData != null)
            {
@@ -663,7 +679,7 @@ public sealed class BSShapeCollection : IDisposable
        if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_HULL)
            return false;
-        DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
+        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
                        prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
        // Remove usage of the previous shape.
@@ -674,8 +690,6 @@ public sealed class BSShapeCollection : IDisposable
        ReferenceShape(newShape);
-        // hulls are already scaled by the meshmerizer
-        prim.Scale = new OMV.Vector3(1f, 1f, 1f);
        prim.PhysShape = newShape;
        return true;        // 'true' means a new shape has been added to this prim
    }
@@ -694,8 +708,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
+            // 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, false);
+            IMesh meshData = PhysicsScene.mesher.CreateMesh(objName, pbs, size, lod, true, false, false, false);
            if (meshData != null)
            {
@@ -784,7 +798,7 @@ public sealed class BSShapeCollection : IDisposable
        BulletShape newShape = new BulletShape(hullPtr, BSPhysicsShapeType.SHAPE_HULL);
        newShape.shapeKey = newHullKey;
-        return newShape;        // 'true' means a new shape has been added to this prim
+        return newShape;
    }
    // Callback from convex hull creater with a newly created hull.
@@ -809,7 +823,7 @@ public sealed class BSShapeCollection : IDisposable
        // Create the shape for the root prim and add it to the compound shape. Cannot be a native shape.
        CreateGeomMeshOrHull(prim, shapeCallback);
        BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, prim.PhysShape.ptr, OMV.Vector3.Zero, OMV.Quaternion.Identity);
-        DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addRootPrim,compShape={1},rootShape={2}",
+        if (DDetail) DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addRootPrim,compShape={1},rootShape={2}",
                                    prim.LocalID, cShape, prim.PhysShape);
        prim.PhysShape = cShape;
@@ -851,7 +865,7 @@ public sealed class BSShapeCollection : IDisposable
    private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim)
    {
        // If the shape was successfully created, nothing more to do
-        if (newShape.ptr != IntPtr.Zero)
+        if (newShape.HasPhysicalShape)
            return newShape;
        // If this mesh has an underlying asset and we have not failed getting it before, fetch the asset
@@ -910,7 +924,7 @@ public sealed class BSShapeCollection : IDisposable
        bool ret = false;
        // the mesh, hull or native shape must have already been created in Bullet
-        bool mustRebuild = (prim.PhysBody.ptr == IntPtr.Zero);
+        bool mustRebuild = !prim.PhysBody.HasPhysicalBody;
        // If there is an existing body, verify it's of an acceptable type.
        // If not a solid object, body is a GhostObject. Otherwise a RigidBody.
@@ -936,13 +950,13 @@ public sealed class BSShapeCollection : IDisposable
            {
                bodyPtr = BulletSimAPI.CreateBodyFromShape2(sim.ptr, shape.ptr,
                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
-                DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
            }
            else
            {
                bodyPtr = BulletSimAPI.CreateGhostFromShape2(sim.ptr, shape.ptr,
                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
-                DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                if (DDetail) DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
            }
            aBody = new BulletBody(prim.LocalID, bodyPtr);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
index 3ca756c..2b120d6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -93,7 +93,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
    {
        m_mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, m_mapInfo.ID,
                                m_mapInfo.minCoords, m_mapInfo.maxCoords, 
-                                m_mapInfo.heightMap, BSTerrainManager.TERRAIN_COLLISION_MARGIN);
+                                m_mapInfo.heightMap, PhysicsScene.Params.terrainCollisionMargin);
        // Create the terrain shape from the mapInfo
        m_mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(m_mapInfo.Ptr),
@@ -121,9 +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,
+        m_mapInfo.terrainBody.collisionType = CollisionType.Terrain;
-                            (uint)CollisionFilterGroups.TerrainFilter,
+        m_mapInfo.terrainBody.ApplyCollisionMask();
-                            (uint)CollisionFilterGroups.TerrainMask);
        // Make it so the terrain will not move or be considered for movement.
        BulletSimAPI.ForceActivationState2(m_mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
@@ -136,7 +135,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
    {
        if (m_mapInfo != null)
        {
-            if (m_mapInfo.terrainBody.ptr != IntPtr.Zero)
+            if (m_mapInfo.terrainBody.HasPhysicalBody)
            {
                BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
                // Frees both the body and the shape.
@@ -148,7 +147,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 +165,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 23fcfd3..3428b9c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -62,11 +62,12 @@ 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);
 }
 // ==========================================================================================
-public sealed class BSTerrainManager
+public sealed class BSTerrainManager : IDisposable
 {
    static string LogHeader = "[BULLETSIM TERRAIN MANAGER]";
@@ -75,13 +76,12 @@ 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.
    public const float HEIGHT_EQUAL_FUDGE = 0.2f;
-    public const float TERRAIN_COLLISION_MARGIN = 0.0f;
    // Until the whole simulator is changed to pass us the region size, we rely on constants.
    public Vector3 DefaultRegionSize = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
@@ -122,6 +122,11 @@ public sealed class BSTerrainManager
        MegaRegionParentPhysicsScene = null;
    }
+    public void Dispose()
+    {
+        ReleaseGroundPlaneAndTerrain();
+    }
    // Create the initial instance of terrain and the underlying ground plane.
    // This is called from the initialization routine so we presume it is
    //    safe to call Bullet in real time. We hope no one is moving prims around yet.
@@ -129,7 +134,8 @@ public sealed class BSTerrainManager
    {
        // The ground plane is here to catch things that are trying to drop to negative infinity
        BulletShape groundPlaneShape = new BulletShape(
-                    BulletSimAPI.CreateGroundPlaneShape2(BSScene.GROUNDPLANE_ID, 1f, TERRAIN_COLLISION_MARGIN),
+                    BulletSimAPI.CreateGroundPlaneShape2(BSScene.GROUNDPLANE_ID, 1f, 
+                                    PhysicsScene.Params.terrainCollisionMargin),
                    BSPhysicsShapeType.SHAPE_GROUNDPLANE);
        m_groundPlane = new BulletBody(BSScene.GROUNDPLANE_ID,
                        BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
@@ -139,8 +145,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,
+        m_groundPlane.collisionType = CollisionType.Groundplane;
-                        (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
+        m_groundPlane.ApplyCollisionMask();
        // 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);
@@ -150,13 +156,13 @@ public sealed class BSTerrainManager
    // Release all the terrain structures we might have allocated
    public void ReleaseGroundPlaneAndTerrain()
    {
-        if (m_groundPlane.ptr != IntPtr.Zero)
+        if (m_groundPlane.HasPhysicalBody)
        {
            if (BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr))
            {
                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_groundPlane.ptr);
            }
-            m_groundPlane.ptr = IntPtr.Zero;
+            m_groundPlane.Clear();
        }
        ReleaseTerrain();
@@ -165,17 +171,22 @@ public sealed class BSTerrainManager
    // Release all the terrain we have allocated
    public void ReleaseTerrain()
    {
-        foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
+        lock (m_terrains)
        {
-            kvp.Value.Dispose();
+            foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
+            {
+                kvp.Value.Dispose();
+            }
+            m_terrains.Clear();
        }
-        m_terrains.Clear();
    }
    // The simulator wants to set a new heightmap for the terrain.
    public void SetTerrain(float[] heightMap) {
        float[] localHeightMap = heightMap;
-        PhysicsScene.TaintedObject("TerrainManager.SetTerrain", delegate()
+        // If there are multiple requests for changes to the same terrain between ticks,
+        //      only do that last one.
+        PhysicsScene.PostTaintObject("TerrainManager.SetTerrain-"+ m_worldOffset.ToString(), 0, delegate()
        {
            if (m_worldOffset != Vector3.Zero && MegaRegionParentPhysicsScene != null)
            {
@@ -211,6 +222,7 @@ public sealed class BSTerrainManager
    //     terrain shape is created and added to the body.
    //     This call is most often used to update the heightMap and parameters of the terrain.
    // (The above does suggest that some simplification/refactoring is in order.)
+    // Called during taint-time.
    private void UpdateTerrain(uint id, float[] heightMap, 
                            Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
    {
@@ -220,7 +232,7 @@ public sealed class BSTerrainManager
        // Find high and low points of passed heightmap.
        // The min and max passed in is usually the area objects can be in (maximum
        //     object height, for instance). The terrain wants the bounding box for the
-        //     terrain so we replace passed min and max Z with the actual terrain min/max Z.
+        //     terrain so replace passed min and max Z with the actual terrain min/max Z.
        float minZ = float.MaxValue;
        float maxZ = float.MinValue;
        foreach (float height in heightMap)
@@ -238,15 +250,15 @@ public sealed class BSTerrainManager
        Vector3 terrainRegionBase = new Vector3(minCoords.X, minCoords.Y, 0f);
-        BSTerrainPhys terrainPhys;
+        lock (m_terrains)
-        if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
        {
-            // There is already a terrain in this spot. Free the old and build the new.
+            BSTerrainPhys terrainPhys;
-            DetailLog("{0},UpdateTerrain:UpdateExisting,call,id={1},base={2},minC={3},maxC={4}",
+            if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
-                            BSScene.DetailLogZero, id, terrainRegionBase, minCoords, minCoords);
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:UpdateExisting", delegate()
            {
+                // There is already a terrain in this spot. Free the old and build the new.
+                DetailLog("{0},UpdateTerrain:UpdateExisting,call,id={1},base={2},minC={3},maxC={4}",
+                                BSScene.DetailLogZero, id, terrainRegionBase, minCoords, minCoords);
                // Remove old terrain from the collection
                m_terrains.Remove(terrainRegionBase);
                // Release any physical memory it may be using.
@@ -271,35 +283,24 @@ public sealed class BSTerrainManager
                    // I hate doing this, but just bail
                    return;
                }
-            });
+            }
-        }
+            else
-        else
+            {
-        {
+                // We don't know about this terrain so either we are creating a new terrain or
-            // We don't know about this terrain so either we are creating a new terrain or
+                //    our mega-prim child is giving us a new terrain to add to the phys world
-            //    our mega-prim child is giving us a new terrain to add to the phys world
-            // if this is a child terrain, calculate a unique terrain id
-            uint newTerrainID = id;
-            if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
-                newTerrainID = ++m_terrainCount;
-            float[] heightMapX = heightMap;
-            Vector3 minCoordsX = minCoords;
-            Vector3 maxCoordsX = maxCoords;
-            DetailLog("{0},UpdateTerrain:NewTerrain,call,id={1}, minC={2}, maxC={3}",
+                // if this is a child terrain, calculate a unique terrain id
-                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
+                uint newTerrainID = id;
+                if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
+                    newTerrainID = ++m_terrainCount;
-            // Code that must happen at taint-time
+                DetailLog("{0},UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:NewTerrain", delegate()
+                                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
-            {
-                DetailLog("{0},UpdateTerrain:NewTerrain,taint,baseX={1},baseY={2}", 
-                                            BSScene.DetailLogZero, minCoordsX.X, minCoordsX.Y);
                BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
                m_terrains.Add(terrainRegionBase, newTerrainPhys);
                m_terrainModified = true;
-            });
+            }
        }
    }
@@ -331,6 +332,13 @@ public sealed class BSTerrainManager
        return newTerrainPhys;
    }
+    // Return 'true' of this position is somewhere in known physical terrain space
+    public bool IsWithinKnownTerrain(Vector3 pos)
+    {
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        return GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ);
+    }
    // Given an X and Y, find the height of the terrain.
    // Since we could be handling multiple terrains for a mega-region,
@@ -341,40 +349,74 @@ public sealed class BSTerrainManager
    private float lastHeightTX = 999999f;
    private float lastHeightTY = 999999f;
    private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    public float GetTerrainHeightAtXYZ(Vector3 pos)
    {
-        float tX = loc.X;
+        float tX = pos.X;
-        float tY = loc.Y;
+        float tY = pos.Y;
        // You'd be surprized at the number of times this routine is called
        //    with the same parameters as last time.
-        if (!m_terrainModified && lastHeightTX == tX && lastHeightTY == tY)
+        if (!m_terrainModified && (lastHeightTX == tX) && (lastHeightTY == tY))
            return lastHeight;
+        m_terrainModified = false;
        lastHeightTX = tX;
        lastHeightTY = tY;
        float ret = HEIGHT_GETHEIGHT_RET;
-        int offsetX = ((int)(tX / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
+        Vector3 terrainBaseXYZ;
-        int offsetY = ((int)(tY / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
        BSTerrainPhys physTerrain;
-        if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
+        if (GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ))
        {
-            ret = physTerrain.GetHeightAtXYZ(loc - terrainBaseXYZ);
+            ret = physTerrain.GetTerrainHeightAtXYZ(pos - terrainBaseXYZ);
-            DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,loc={1},base={2},height={3}",
-                                             BSScene.DetailLogZero, loc, terrainBaseXYZ, ret);
        }
        else
        {
            PhysicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
                    LogHeader, PhysicsScene.RegionName, tX, tY);
+            DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,terrainNotFound,pos={1},base={2}",
+                                BSScene.DetailLogZero, pos, terrainBaseXYZ);
        }
-        m_terrainModified = false;
        lastHeight = ret;
        return ret;
    }
+    public float GetWaterLevelAtXYZ(Vector3 pos)
+    {
+        float ret = WATER_HEIGHT_GETHEIGHT_RET;
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        if (GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ))
+        {
+            ret = physTerrain.GetWaterLevelAtXYZ(pos);
+        }
+        else
+        {
+            PhysicsScene.Logger.ErrorFormat("{0} GetWaterHeightAtXY: terrain not found: pos={1}, terrainBase={2}, height={3}",
+                    LogHeader, PhysicsScene.RegionName, pos, terrainBaseXYZ, ret);
+        }
+        return ret;
+    }
+    // Given an address, return 'true' of there is a description of that terrain and output
+    //    the descriptor class and the 'base' fo the addresses therein.
+    private bool GetTerrainPhysicalAtXYZ(Vector3 pos, out BSTerrainPhys outPhysTerrain, out Vector3 outTerrainBase)
+    {
+        int offsetX = ((int)(pos.X / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
+        int offsetY = ((int)(pos.Y / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
+        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
+        BSTerrainPhys physTerrain = null;
+        lock (m_terrains)
+        {
+            m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
+        }
+        outTerrainBase = terrainBaseXYZ;
+        outPhysTerrain = physTerrain;
+        return (physTerrain != null);
+    }
    // 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 dca7150..6dc0d92 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -88,11 +88,13 @@ public sealed class BSTerrainMesh : BSTerrainPhys
            // Something is very messed up and a crash is in our future.
            return;
        }
+        PhysicsScene.DetailLog("{0},BSTerrainMesh.create,meshed,indices={1},indSz={2},vertices={3},vertSz={4}", 
+                                ID, indicesCount, indices.Length, verticesCount, vertices.Length);
        m_terrainShape = new BulletShape(BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
-                                        indicesCount, indices, verticesCount, vertices),
+                                                    indicesCount, indices, verticesCount, vertices),
                                        BSPhysicsShapeType.SHAPE_MESH);
-        if (m_terrainShape.ptr == IntPtr.Zero)
+        if (!m_terrainShape.HasPhysicalShape)
        {
            // DISASTER!!
            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape", ID);
@@ -105,7 +107,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
        Quaternion rot = Quaternion.Identity;
        m_terrainBody = new BulletBody(id, BulletSimAPI.CreateBodyWithDefaultMotionState2( m_terrainShape.ptr, ID, pos, rot));
-        if (m_terrainBody.ptr == IntPtr.Zero)
+        if (!m_terrainBody.HasPhysicalBody)
        {
            // DISASTER!!
            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase);
@@ -122,15 +124,14 @@ public sealed class BSTerrainMesh : BSTerrainPhys
        // Static objects are not very massive.
        BulletSimAPI.SetMassProps2(m_terrainBody.ptr, 0f, Vector3.Zero);
-        // Return the new terrain to the world of physical objects
+        // Put the new terrain to the world of physical objects
        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
-        // redo its bounding box now that it is in the world
+        // Redo its bounding box now that it is in the world
        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr);
-        BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr,
+        m_terrainBody.collisionType = CollisionType.Terrain;
-                            (uint)CollisionFilterGroups.TerrainFilter,
+        m_terrainBody.ApplyCollisionMask();
-                            (uint)CollisionFilterGroups.TerrainMask);
        // Make it so the terrain will not move or be considered for movement.
        BulletSimAPI.ForceActivationState2(m_terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
@@ -138,7 +139,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
    public override void Dispose()
    {
-        if (m_terrainBody.ptr != IntPtr.Zero)
+        if (m_terrainBody.HasPhysicalBody)
        {
            BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
            // Frees both the body and the shape.
@@ -146,7 +147,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.
@@ -167,6 +168,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(
@@ -188,6 +195,11 @@ public sealed class BSTerrainMesh : BSTerrainPhys
        // Simple mesh creation which assumes magnification == 1.
        // TODO: do a more general solution that scales, adds new vertices and smoothes the result.
+        // Create an array of vertices that is sizeX+1 by sizeY+1 (note the loop
+        //    from zero to <= sizeX). The triangle indices are then generated as two triangles
+        //    per heightmap point. There are sizeX by sizeY of these squares. The extra row and
+        //    column of vertices are used to complete the triangles of the last row and column
+        //    of the heightmap.
        try
        {
            // One vertice per heightmap value plus the vertices off the top and bottom edge.
@@ -200,16 +212,18 @@ public sealed class BSTerrainMesh : BSTerrainPhys
            float magY = (float)sizeY / extentY;
            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3},magX={4},magY={5}",
                                    BSScene.DetailLogZero, totalVertices, totalIndices, extentBase, magX, magY);
+            float minHeight = float.MaxValue;
            // Note that sizeX+1 vertices are created since there is land between this and the next region.
            for (int yy = 0; yy <= sizeY; yy++)
            {
-                for (int xx = 0; xx <= sizeX; xx++)     // Hint: the "<=" means we got through sizeX + 1 times
+                for (int xx = 0; xx <= sizeX; xx++)     // Hint: the "<=" means we go around sizeX + 1 times
                {
                    int offset = yy * sizeX + xx;
-                    // Extend the height from the height from the last row or column
+                    // Extend the height with the height from the last row or column
                    if (yy == sizeY) offset -= sizeX;
                    if (xx == sizeX) offset -= 1;
                    float height = heightMap[offset];
+                    minHeight = Math.Min(minHeight, height);
                    vertices[verticesCount + 0] = (float)xx * magX + extentBase.X;
                    vertices[verticesCount + 1] = (float)yy * magY + extentBase.Y;
                    vertices[verticesCount + 2] = height + extentBase.Z;
@@ -217,14 +231,12 @@ public sealed class BSTerrainMesh : BSTerrainPhys
                }
            }
            verticesCount = verticesCount / 3;
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeVerts,verCount={1}", 
-                                            BSScene.DetailLogZero, verticesCount);
            for (int yy = 0; yy < sizeY; yy++)
            {
                for (int xx = 0; xx < sizeX; xx++)
                {
-                    int offset = yy * sizeX + xx;
+                    int offset = yy * (sizeX + 1) + xx;
                    // Each vertices is presumed to be the upper left corner of a box of two triangles
                    indices[indicesCount + 0] = offset;
                    indices[indicesCount + 1] = offset + 1;
@@ -235,8 +247,7 @@ public sealed class BSTerrainMesh : BSTerrainPhys
                    indicesCount += 6;
                }
            }
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeIndices,indCount={1}",   // DEEBUG DEBUG DEBUG
-                                                        LogHeader, indicesCount);                        // DEBUG
            ret = true;
        }
        catch (Exception e)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index e60a760..962b540 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -25,6 +25,7 @@
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 using System;
+using System.Collections.Generic;
 using System.Runtime.InteropServices;
 using System.Security;
 using System.Text;
@@ -32,93 +33,6 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin {
-// Classes to allow some type checking for the API
-// These hold pointers to allocated objects in the unmanaged space.
-// The physics engine controller class created at initialization
-public struct BulletSim
-{
-    public BulletSim(uint worldId, BSScene bss, IntPtr xx)
-    {
-        ptr = xx;
-        worldID = worldId;
-        physicsScene = bss;
-    }
-    public IntPtr ptr;
-    public uint worldID;
-    // The scene is only in here so very low level routines have a handle to print debug/error messages
-    public BSScene physicsScene;
-}
-// An allocated Bullet btRigidBody
-public struct BulletBody
-{
-    public BulletBody(uint id, IntPtr xx)
-    {
-        ID = id;
-        ptr = xx;
-        collisionFilter = 0;
-        collisionMask = 0;
-    }
-    public IntPtr ptr;
-    public uint ID;
-    public CollisionFilterGroups collisionFilter;
-    public CollisionFilterGroups collisionMask;
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<id=");
-        buff.Append(ID.ToString());
-        buff.Append(",p=");
-        buff.Append(ptr.ToString("X"));
-        if (collisionFilter != 0 || collisionMask != 0)
-        {
-            buff.Append(",f=");
-            buff.Append(collisionFilter.ToString("X"));
-            buff.Append(",m=");
-            buff.Append(collisionMask.ToString("X"));
-        }
-        buff.Append(">");
-        return buff.ToString();
-    }
-}
-public struct BulletShape
-{
-    public BulletShape(IntPtr xx)
-    {
-        ptr = xx;
-        type=BSPhysicsShapeType.SHAPE_UNKNOWN;
-        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
-        isNativeShape = false;
-    }
-    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
-    {
-        ptr = xx;
-        type = typ;
-        shapeKey = 0;
-        isNativeShape = false;
-    }
-    public IntPtr ptr;
-    public BSPhysicsShapeType type;
-    public System.UInt64 shapeKey;
-    public bool isNativeShape;
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
-        buff.Append(ptr.ToString("X"));
-        buff.Append(",s=");
-        buff.Append(type.ToString());
-        buff.Append(",k=");
-        buff.Append(shapeKey.ToString("X"));
-        buff.Append(",n=");
-        buff.Append(isNativeShape.ToString());
-        buff.Append(">");
-        return buff.ToString();
-    }
-}
    // Constraint type values as defined by Bullet
 public enum ConstraintType : int
 {
@@ -132,44 +46,6 @@ public enum ConstraintType : int
        MAX_CONSTRAINT_TYPE
 }
-// An allocated Bullet btConstraint
-public struct BulletConstraint
-{
-    public BulletConstraint(IntPtr xx)
-    {
-        ptr = xx;
-    }
-    public IntPtr ptr;
-}
-// An allocated HeightMapThing which holds various heightmap info.
-// Made a class rather than a struct so there would be only one
-//      instance of this and C# will pass around pointers rather
-//      than making copies.
-public class BulletHeightMapInfo
-{
-    public BulletHeightMapInfo(uint id, float[] hm, IntPtr xx) {
-        ID = id;
-        Ptr = xx;
-        heightMap = hm;
-        terrainRegionBase = Vector3.Zero;
-        minCoords = new Vector3(100f, 100f, 25f);
-        maxCoords = new Vector3(101f, 101f, 26f);
-        minZ = maxZ = 0f;
-        sizeX = sizeY = 256f;
-    }
-    public uint ID;
-    public IntPtr Ptr;
-    public float[] heightMap;
-    public Vector3 terrainRegionBase;
-    public Vector3 minCoords;
-    public Vector3 maxCoords;
-    public float sizeX, sizeY;
-    public float minZ, maxZ;
-    public BulletShape terrainShape;
-    public BulletBody terrainBody;
-}
 // ===============================================================================
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull
@@ -287,6 +163,8 @@ public struct ConfigurationParameters
    public float terrainFriction;
    public float terrainHitFraction;
    public float terrainRestitution;
+    public float terrainCollisionMargin;
    public float avatarFriction;
    public float avatarStandingFriction;
    public float avatarDensity;
@@ -296,6 +174,8 @@ public struct ConfigurationParameters
    public float avatarCapsuleHeight;
        public float avatarContactProcessingThreshold;
+    public float vehicleAngularDamping;
        public float maxPersistantManifoldPoolSize;
        public float maxCollisionAlgorithmPoolSize;
        public float shouldDisableContactPoolDynamicAllocation;
@@ -353,56 +233,35 @@ public enum CollisionFlags : uint
    CF_CHARACTER_OBJECT              = 1 << 4,
    CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
    CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
-    // Following used by BulletSim to control collisions
+    // Following used by BulletSim to control collisions and updates
    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
    BS_FLOATS_ON_WATER               = 1 << 11,
+    BS_VEHICLE_COLLISIONS            = 1 << 12,
    BS_NONE                          = 0,
-    BS_ALL                           = 0xFFFFFFFF,
+    BS_ALL                           = 0xFFFFFFFF
-    // These are the collision flags switched depending on physical state.
-    // The other flags are used for other things and should not be fooled with.
-    BS_ACTIVE = CF_STATIC_OBJECT
-                | CF_KINEMATIC_OBJECT
-                | CF_NO_CONTACT_RESPONSE
 };
-// Values for collisions groups and masks
+// Values f collisions groups and masks
 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.
+    //   are more easily found and debugged.
-    BNoneFilter              = 0,
+    BNoneGroup              = 0,
-    BDefaultFilter           = 1 << 0,
+    BDefaultGroup           = 1 << 0,
-    BStaticFilter            = 1 << 1,
+    BStaticGroup            = 1 << 1,
-    BKinematicFilter         = 1 << 2,
+    BKinematicGroup         = 1 << 2,
-    BDebrisFilter            = 1 << 3,
+    BDebrisGroup            = 1 << 3,
-    BSensorTrigger           = 1 << 4,
+    BSensorTrigger          = 1 << 4,
-    BCharacterFilter         = 1 << 5,
+    BCharacterGroup         = 1 << 5,
-    BAllFilter               = 0xFFFFFFFF,
+    BAllGroup               = 0xFFFFFFFF,
    // Filter groups defined by BulletSim
-    BGroundPlaneFilter       = 1 << 10,
+    BGroundPlaneGroup       = 1 << 10,
-    BTerrainFilter           = 1 << 11,
+    BTerrainGroup           = 1 << 11,
-    BRaycastFilter           = 1 << 12,
+    BRaycastGroup           = 1 << 12,
-    BSolidFilter             = 1 << 13,
+    BSolidGroup             = 1 << 13,
-    BLinksetFilter           = 1 << 14,
+    // BLinksetGroup        = xx  // a linkset proper is either static or dynamic
+    BLinksetChildGroup      = 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,
-    VolumeDetectMask        = ~BSensorTrigger,
-    TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter & ~BStaticFilter,  // static objects on the ground don't collide
-    GroundPlaneFilter       = BGroundPlaneFilter,
-    GroundPlaneMask         = BAllFilter
 };
 // CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
@@ -429,7 +288,7 @@ public enum ConstraintParamAxis : int
 // ===============================================================================
 static class BulletSimAPI {
+// ===============================================================================
 // Link back to the managed code for outputting log messages
 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
@@ -482,6 +341,9 @@ public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData)
 public static extern bool IsNativeShape2(IntPtr shape);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetShapeCollisionMargin(IntPtr shape, float margin);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float height, Vector3 scale);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -938,7 +800,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
@@ -1000,13 +862,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/BulletSimData.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
new file mode 100755
index 0000000..662177f
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimData.cs
@@ -0,0 +1,278 @@
+/*
+ * 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 copyrightD
+ *       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;
+using OMV = OpenMetaverse;
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+// Classes to allow some type checking for the API
+// These hold pointers to allocated objects in the unmanaged space.
+// The physics engine controller class created at initialization
+public struct BulletSim
+{
+    public BulletSim(uint worldId, BSScene bss, IntPtr xx)
+    {
+        ptr = xx;
+        worldID = worldId;
+        physicsScene = bss;
+    }
+    public IntPtr ptr;
+    public uint worldID;
+    // The scene is only in here so very low level routines have a handle to print debug/error messages
+    public BSScene physicsScene;
+}
+// An allocated Bullet btRigidBody
+public struct BulletBody
+{
+    public BulletBody(uint id) : this(id, IntPtr.Zero)
+    {
+    }
+    public BulletBody(uint id, IntPtr xx)
+    {
+        ID = id;
+        ptr = xx;
+        collisionType = CollisionType.Static;
+    }
+    public IntPtr ptr;
+    public uint ID;
+    public CollisionType collisionType;
+    public void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public bool HasPhysicalBody { get { return ptr != IntPtr.Zero; } }
+    // Apply the specificed collision mask into the physical world
+    public void ApplyCollisionMask()
+    {
+        // Should assert the body has been added to the physical world.
+        // (The collision masks are stored in the collision proxy cache which only exists for
+        //    a collision body that is in the world.)
+        BulletSimAPI.SetCollisionGroupMask2(ptr,
+                                BulletSimData.CollisionTypeMasks[collisionType].group,
+                                BulletSimData.CollisionTypeMasks[collisionType].mask);
+    }
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<id=");
+        buff.Append(ID.ToString());
+        buff.Append(",p=");
+        buff.Append(ptr.ToString("X"));
+        buff.Append(",c=");
+        buff.Append(collisionType);
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+public struct BulletShape
+{
+    public BulletShape(IntPtr xx) : this(xx, BSPhysicsShapeType.SHAPE_UNKNOWN)
+    {
+    }
+    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
+    {
+        ptr = xx;
+        type = typ;
+        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
+        isNativeShape = false;
+    }
+    public IntPtr ptr;
+    public BSPhysicsShapeType type;
+    public System.UInt64 shapeKey;
+    public bool isNativeShape;
+    public void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public bool HasPhysicalShape { get { return ptr != IntPtr.Zero; } }
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<p=");
+        buff.Append(ptr.ToString("X"));
+        buff.Append(",s=");
+        buff.Append(type.ToString());
+        buff.Append(",k=");
+        buff.Append(shapeKey.ToString("X"));
+        buff.Append(",n=");
+        buff.Append(isNativeShape.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+// An allocated Bullet btConstraint
+public struct BulletConstraint
+{
+    public BulletConstraint(IntPtr xx)
+    {
+        ptr = xx;
+    }
+    public IntPtr ptr;
+    public void Clear()
+    {
+        ptr = IntPtr.Zero;
+    }
+    public bool HasPhysicalConstraint { get { return ptr != IntPtr.Zero; } }
+}
+// An allocated HeightMapThing which holds various heightmap info.
+// Made a class rather than a struct so there would be only one
+//      instance of this and C# will pass around pointers rather
+//      than making copies.
+public class BulletHeightMapInfo
+{
+    public BulletHeightMapInfo(uint id, float[] hm, IntPtr xx) {
+        ID = id;
+        Ptr = xx;
+        heightMap = hm;
+        terrainRegionBase = OMV.Vector3.Zero;
+        minCoords = new OMV.Vector3(100f, 100f, 25f);
+        maxCoords = new OMV.Vector3(101f, 101f, 26f);
+        minZ = maxZ = 0f;
+        sizeX = sizeY = 256f;
+    }
+    public uint ID;
+    public IntPtr Ptr;
+    public float[] heightMap;
+    public OMV.Vector3 terrainRegionBase;
+    public OMV.Vector3 minCoords;
+    public OMV.Vector3 maxCoords;
+    public float sizeX, sizeY;
+    public float minZ, maxZ;
+    public BulletShape terrainShape;
+    public BulletBody terrainBody;
+}
+// The general class of collsion object.
+public enum CollisionType
+{
+    Avatar,
+    Groundplane,
+    Terrain,
+    Static,
+    Dynamic,
+    VolumeDetect,
+    // Linkset, // A linkset should be either Static or Dynamic
+    LinksetChild,
+    Unknown
+};
+// Hold specification of group and mask collision flags for a CollisionType
+public struct CollisionTypeFilterGroup
+{
+    public CollisionTypeFilterGroup(CollisionType t, uint g, uint m)
+    {
+        type = t;
+        group = g;
+        mask = m;
+    }
+    public CollisionType type;
+    public uint group;
+    public uint mask;
+};
+    /* NOTE: old definitions kept for reference. Delete when things are working.
+    // The collsion filters and masked are defined in one place -- don't want them scattered
+    AvatarGroup             = BCharacterGroup,
+    AvatarMask              = BAllGroup,
+    ObjectGroup             = BSolidGroup,
+    ObjectMask              = BAllGroup,
+    StaticObjectGroup       = BStaticGroup,
+    StaticObjectMask        = AvatarGroup | ObjectGroup,    // static things don't interact with much
+    LinksetGroup            = BLinksetGroup,
+    LinksetMask             = BAllGroup,
+    LinksetChildGroup       = BLinksetChildGroup,
+    LinksetChildMask        = BNoneGroup,   // Linkset children disappear from the world
+    VolumeDetectGroup       = BSensorTrigger,
+    VolumeDetectMask        = ~BSensorTrigger,
+    TerrainGroup            = BTerrainGroup,
+    TerrainMask             = BAllGroup & ~BStaticGroup,  // static objects on the ground don't collide
+    GroundPlaneGroup        = BGroundPlaneGroup,
+    GroundPlaneMask         = BAllGroup
+    */
+public static class BulletSimData
+{
+// Map of collisionTypes to flags for collision groups and masks.
+// As mentioned above, don't use the CollisionFilterGroups definitions directly in the code
+//     but, instead, use references to this dictionary. Finding and debugging
+//     collision flag problems will be made easier.
+public static Dictionary<CollisionType, CollisionTypeFilterGroup> CollisionTypeMasks 
+            = new Dictionary<CollisionType, CollisionTypeFilterGroup>()
+{
+    { CollisionType.Avatar, 
+                new CollisionTypeFilterGroup(CollisionType.Avatar, 
+                                (uint)CollisionFilterGroups.BCharacterGroup, 
+                                (uint)CollisionFilterGroups.BAllGroup)
+    },
+    { CollisionType.Groundplane, 
+                new CollisionTypeFilterGroup(CollisionType.Groundplane, 
+                                (uint)CollisionFilterGroups.BGroundPlaneGroup, 
+                                (uint)CollisionFilterGroups.BAllGroup)
+    },
+    { CollisionType.Terrain, 
+                new CollisionTypeFilterGroup(CollisionType.Terrain, 
+                                (uint)CollisionFilterGroups.BTerrainGroup, 
+                                (uint)(CollisionFilterGroups.BAllGroup & ~CollisionFilterGroups.BStaticGroup))
+    },
+    { CollisionType.Static, 
+                new CollisionTypeFilterGroup(CollisionType.Static, 
+                                (uint)CollisionFilterGroups.BStaticGroup, 
+                                (uint)(CollisionFilterGroups.BCharacterGroup | CollisionFilterGroups.BSolidGroup))
+    },
+    { CollisionType.Dynamic, 
+                new CollisionTypeFilterGroup(CollisionType.Dynamic, 
+                                (uint)CollisionFilterGroups.BSolidGroup, 
+                                (uint)(CollisionFilterGroups.BAllGroup))
+    },
+    { CollisionType.VolumeDetect, 
+                new CollisionTypeFilterGroup(CollisionType.VolumeDetect, 
+                                (uint)CollisionFilterGroups.BSensorTrigger, 
+                                (uint)(~CollisionFilterGroups.BSensorTrigger))
+    },
+    { CollisionType.LinksetChild,
+                new CollisionTypeFilterGroup(CollisionType.LinksetChild, 
+                                (uint)CollisionFilterGroups.BTerrainGroup, 
+                                (uint)(CollisionFilterGroups.BNoneGroup))
+    },
+};
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
new file mode 100755
index 0000000..0d9a156
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -0,0 +1,204 @@
+CURRENT PRIORITIES
+=================================================
+Eliminate all crashes (DONEish)
+        Editing/deleting physical linkset (DONE)
+        Border crossing of physical linkset (DONE)
+Enable vehicle border crossings (at least as poorly as ODE)
+        Avatar created in previous region and not new region when crossing border
+        Vehicle recreated in new sim at small Z value (offset from root value?) (DONE)
+Calibrate turning radius
+limitMotorUp calibration (more down?)
+study PID motors (include 'efficiency' implementation
+        Add to avatar movement
+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.
+20121206.1434: rez Sam-pan into OSGrid BulletSim11 region
+        Immediate simulator crash. Mono does not output any stacktrace and
+        log just stops after reporting taint-time linking of the linkset.
+VEHICLES TODO LIST:
+=================================================
+Border crossing with linked vehicle causes crash
+Neb vehicle taking > 25ms of physics time!!
+Vehicles (Move smoothly)
+Add vehicle collisions so IsColliding is properly reported.
+        Needed for banking, limitMotorUp, movementLimiting, ...
+Some vehicles should not be able to turn if no speed or off ground.
+Cannot edit/move a vehicle being ridden: it jumps back to the origional position.
+Neb car jiggling left and right
+        Happens on terrain and any other mesh object. Flat cubes are much smoother.
+        This has been reduced but not eliminated.
+Light cycle falling over when driving
+Implement referenceFrame for all the motion routines.
+Angular motion around Z moves the vehicle in world Z and not vehicle Z in ODE.
+        Verify that angular motion specified around Z moves in the vehicle coordinates.
+Verify llGetVel() is returning a smooth and good value for vehicle movement.
+llGetVel() should return the root's velocity if requested in a child prim.
+Implement function efficiency for lineaar and angular motion.
+Should vehicle angular/linear movement friction happen after all the components
+        or does it only apply to the basic movement?
+After getting off a vehicle, the root prim is phantom (can be walked through)
+    Need to force a position update for the root prim after compound shape destruction
+Linkset explosion after three "rides" on Nebadon lite vehicle (LinksetConstraint)
+For limitMotorUp, use raycast down to find if vehicle is in the air.
+Remove vehicle angular velocity zeroing in BSPrim.UpdateProperties().
+        A kludge that isn't fixing the real problem of Bullet adding extra motion.
+BULLETSIM TODO LIST:
+=================================================
+Revisit CollisionMargin. Builders notice the 0.04 spacing between prims.
+Avatar height off after unsitting (floats off ground)
+        Editting appearance then moving restores.
+        Must not be initializing height when recreating capsule after unsit.
+Duplicating a physical prim causes old prim to jump away
+        Dup a phys prim and the original become unselected and thus interacts w/ selected prim.
+Scenes with hundred of thousands of static objects take a lot of physics CPU time.
+BSPrim.Force should set a continious force on the prim. The force should be
+        applied each tick.  Some limits?
+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 PID motor for avatar movement (slow to stop, ...)
+setForce should set a constant force. Different than AddImpulse.
+Implement raycast.
+Implement ShapeCollection.Dispose()
+Implement water as a plain so raycasting and collisions can happen with same.
+Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
+        Also osGetPhysicsEngineVerion() maybe.
+Linkset.Position and Linkset.Orientation requre rewrite to properly return
+        child position. LinksetConstraint acts like it's at taint time!!
+LINKSETS
+======================================================
+Linksets should allow collisions to individual children
+        Add LocalID to children shapes in LinksetCompound and create events for individuals
+LinksetCompound: when one of the children changes orientation (like tires
+        turning on a vehicle, the whole compound object is rebuilt. Optimize this
+        so orientation/position of individual children can change without a rebuild.
+Verify/think through scripts in children of linksets. What do they reference
+        and return when getting position, velocity, ...
+Confirm constraint linksets still work after making all the changes for compound linksets.
+Add 'changed' flag or similar to reduce the number of times a linkset is rebuilt.
+        For compound linksets, add ability to remove or reposition individual child shapes.
+Disable activity of passive linkset children.
+        Since the linkset is a compound object, the old prims are left lying
+        around and need to be phantomized so they don't collide, ...
+Speed up creation of large physical linksets
+        For instance, sitting in Neb's car (130 prims) takes several seconds to become physical
+Eliminate collisions between objects in a linkset. (LinksetConstraint)
+    Have UserPointer point to struct with localID and linksetID?
+    Objects in original linkset still collide with each other?
+MORE
+======================================================
+Find/remove avatar collision with ID=0.
+Test avatar walking up stairs. How does compare with SL.
+    Radius of the capsule affects ability to climb edges.
+Tune terrain/object friction to be closer to SL.
+Debounce avatar contact so legs don't keep folding up when standing.
+Implement LSL physics controls. Like STATUS_ROTATE_X.
+Add border extensions to terrain to help region crossings and objects leaving region.
+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 whether SimMotionState needs large if statement (see TODO).
+Implement 'top colliders' info.
+Avatar jump
+Performance measurement and changes to make quicker.
+Implement detailed physics stats (GetStats()).
+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
+Physics Arena central pyramid: why is one side permiable?
+INTERNAL IMPROVEMENT/CLEANUP
+=================================================
+Consider moving prim/character body and shape destruction in destroy()
+        to postTimeTime rather than protecting all the potential sets that
+        might have been queued up.
+Remove unused fields from ShapeData (not used in API2)
+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.
+Make BSBody and BSShape real classes to centralize creation/changin/destruction
+        Convert state and parameter calls from BulletSimAPI direct calls to
+                calls on BSBody and BSShape
+Generalize Dynamics and PID with standardized motors.
+Generalize Linkset and vehicles into PropertyManagers
+    Methods for Refresh, RemoveBodyDependencies, RestoreBodyDependencies
+        Potentially add events for shape destruction, etc.
+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)
+        Linkset implementation using manual prim movement.
+LinkablePrim class? Would that simplify/centralize the linkset logic?
+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().
+Should taints check for existance or activeness of target?
+        When destroying linksets/etc, taints can be generated for objects that are
+        actually gone when the taint happens. Crashes don't happen because the taint closure
+        keeps the object from being freed, but that is just an accident.
+        Possibly have and 'active' flag that is checked by the taint processor?
+THREADING
+=================================================
+Do taint action immediately if not actually executing Bullet.
+        Add lock around Bullet execution and just do taint actions if simulation is not happening.
+DONE DONE DONE DONE
+=================================================
+Cleanup code in BSDynamics by using motors. (Resolution: started)
+Consider implementing terrain with a mesh rather than heightmap. (Resolution: done)
+    Would have better and adjustable resolution.
+Build terrain mesh so heighmap is height of the center of the square meter.
+    Resolution: NOT DONE: SL and ODE define meter square as being at one corner with one diagional.
+Terrain as mesh. (Resolution: done)
+How are static linksets seen by the physics engine?
+    Resolution: they are not linked in physics. When moved, all the children are repositioned.
+Convert BSCharacter to use all API2 (Resolution: done)
+Avatar pushing difficult (too heavy?)
+Use asset service passed to BulletSim to get sculptie bodies, etc. (Resolution: done)
+Remove old code in DLL (all non-API2 stuff). (Resolution: done)
+Measurements of mega-physical prim performance (with graph) (Resolution: done, email)
+Debug Bullet internal stats output (why is timing all wrong?)
+        Resolution: Bullet stats logging only works with a single instance of Bullet (one region).
+Implement meshes or just verify that they work. (Resolution: they do!)
+Do prim hash codes work for sculpties and meshes? (Resolution: yes)
+Linkset implementation using compound shapes. (Resolution: implemented LinksetCompound)
+    Compound shapes will need the LocalID in the shapes and collision
+    processing to get it from there.
+Light cycle not banking (Resolution: It doesn't. Banking is roll adding yaw.)
+Package Bullet source mods for Bullet internal stats output
+        (Resolution: move code into WorldData.h rather than relying on patches)
+Single prim vehicles don't seem to properly vehiclize.
+        (Resolution: mass was not getting set properly for single prim linksets)
+Add material type linkage and input all the material property definitions.
+        Skeleton classes and table are in the sources but are not filled or used.
+        (Resolution: