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

16 files changed, 1670 insertions, 1121 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index f33c124..4c195e1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -78,11 +78,17 @@ public sealed class BSCharacter : BSPhysObject
     private float _PIDHoverTao;
 
     public BSCharacter(uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, bool isFlying)
+            : base(parent_scene, localID, avName, "BSCharacter")
     {
-        base.BaseInitialize(parent_scene, localID, avName, "BSCharacter");
         _physicsActorType = (int)ActorTypes.Agent;
         _position = pos;
+
+        // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
+        //     replace with the default values.
         _size = size;
+        if (_size.X == 0f) _size.X = PhysicsScene.Params.avatarCapsuleDepth;
+        if (_size.Y == 0f) _size.Y = PhysicsScene.Params.avatarCapsuleWidth;
+
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
@@ -131,6 +137,10 @@ public sealed class BSCharacter : BSPhysObject
         // Set the velocity and compute the proper friction
         ForceVelocity = _velocity;
 
+        // This will enable or disable the flying buoyancy of the avatar.
+        // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
+        Flying = _flying;
+
         BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.avatarRestitution);
         BulletSimAPI.SetMargin2(PhysShape.ptr, PhysicsScene.Params.collisionMargin);
         BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
@@ -171,8 +181,7 @@ public sealed class BSCharacter : BSPhysObject
         get
         {
             // Avatar capsule size is kept in the scale parameter.
-            // return _size;
-            return new OMV.Vector3(Scale.X * 2f, Scale.Y * 2f, Scale.Z);
+            return _size;
         }
 
         set {
@@ -180,8 +189,8 @@ public sealed class BSCharacter : BSPhysObject
             _size = value;
             ComputeAvatarScale(_size);
             ComputeAvatarVolumeAndMass();
-            DetailLog("{0},BSCharacter.setSize,call,scale={1},density={2},volume={3},mass={4}",
-                            LocalID, Scale, _avatarDensity, _avatarVolume, RawMass);
+            DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
+                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
 
             PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
             {
@@ -199,9 +208,9 @@ public sealed class BSCharacter : BSPhysObject
         set { BaseShape = value; }
     }
     // I want the physics engine to make an avatar capsule
-    public override ShapeData.PhysicsShapeType PreferredPhysicalShape
+    public override BSPhysicsShapeType PreferredPhysicalShape
     {
-        get {return ShapeData.PhysicsShapeType.SHAPE_AVATAR; }
+        get {return BSPhysicsShapeType.SHAPE_CAPSULE; }
     }
 
     public override bool Grabbed {
@@ -610,13 +619,19 @@ public sealed class BSCharacter : BSPhysObject
         // The 'size' given by the simulator is the mid-point of the avatar
         //    and X and Y are unspecified.
 
-        OMV.Vector3 newScale = OMV.Vector3.Zero;
-        newScale.X = PhysicsScene.Params.avatarCapsuleRadius;
-        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius;
+        OMV.Vector3 newScale = size;
+        // newScale.X = PhysicsScene.Params.avatarCapsuleWidth;
+        // newScale.Y = PhysicsScene.Params.avatarCapsuleDepth;
 
         // From the total height, remove the capsule half spheres that are at each end
-        newScale.Z = size.Z- (newScale.X + newScale.Y);
-        Scale = newScale;
+        // The 1.15f came from ODE. Not sure what this factors in.
+        // newScale.Z = (size.Z * 1.15f) - (newScale.X + newScale.Y);
+
+        // The total scale height is the central cylindar plus the caps on the two ends.
+        newScale.Z = size.Z + (Math.Min(size.X, size.Y) * 2f);
+
+        // Convert diameters to radii and height to half height -- the way Bullet expects it.
+        Scale = newScale / 2f;
     }
 
     // set _avatarVolume and _mass based on capsule size, _density and Scale
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..74eb9ab 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -80,10 +80,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,6 +93,7 @@ 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
@@ -124,6 +125,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
 
         //Attractor properties
+        private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
 
@@ -152,10 +154,12 @@ 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 = Math.Max(0.01f, Math.Min(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));
@@ -185,33 +189,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 = Math.Max(0.01f, Math.Min(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_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,6 +238,7 @@ 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
@@ -234,14 +246,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
                     pValue.Z = Math.Max(-12.56f, Math.Min(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 +317,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 0;
                     m_VehicleBuoyancy = 0;
-                    
+
                     m_linearDeflectionEfficiency = 1;
                     m_linearDeflectionTimescale = 1;
 
@@ -319,6 +333,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags = (VehicleFlag)0;
+
                     break;
 
                 case Vehicle.TYPE_SLED:
@@ -351,10 +366,13 @@ 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 | VehicleFlag.HOVER_TERRAIN_ONLY |
-                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                | VehicleFlag.HOVER_TERRAIN_ONLY
+                                | VehicleFlag.HOVER_GLOBAL_HEIGHT
+                                | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                            | VehicleFlag.LIMIT_ROLL_ONLY
+                            | VehicleFlag.LIMIT_MOTOR_UP);
                     break;
                 case Vehicle.TYPE_CAR:
                     m_linearMotorDirection = Vector3.Zero;
@@ -510,6 +528,28 @@ 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)
+
+            // m_bankingMotor = new BSVMotor("BankingMotor", ...);
         }
 
         // Some of the properties of this prim may have changed.
@@ -518,13 +558,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (IsActive)
             {
-                // Friction effects are handled by this vehicle code
-                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
-                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
-
-                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
+                m_vehicleMass = Prim.Linkset.LinksetMass;
 
-                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+                // Friction effects are handled by this vehicle code
+                float friction = 0f;
+                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
+
+                // 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);
+
+                // 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);
+                BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
+
+                VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
+                                Prim.LocalID, friction, localInertia, angularDamping);
             }
         }
 
@@ -551,111 +603,109 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (!IsActive) return;
 
-            // DEBUG
-            // 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;
 
-            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
-                                    Prim.LocalID, 
-                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
-                                    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);
-        }// end Step
+        }
 
         // Apply the effect of the linear motor.
         // Also does hover and float.
         private void MoveLinear(float pTimestep)
         {
-            // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates
-            // 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
+            Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
 
-                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
-                m_lastLinearVelocityVector += addAmount;
+            // Rotate new object velocity from vehicle relative to world coordinates
+            linearMotorContribution *= Prim.ForceOrientation;
 
-                float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
-                m_linearMotorDirection *= (1f - decayFactor);
+            // ==================================================================
+            // Gravity and Buoyancy
+            // There is some gravity, make a gravity force vector that is applied after object velocity.
+            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
 
-                // Rotate new object velocity from vehicle relative to world coordinates
-                m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
+            Vector3 pos = Prim.ForcePosition;
+            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
 
-                // Apply friction for next time
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
 
-                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
-                                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;
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
+
+            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
+
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
+
+            // ==================================================================
+            Vector3 newVelocity = linearMotorContribution
+                            + terrainHeightContribution
+                            + hoverContribution
+                            + limitMotorUpContribution;
+
+            // If not changing some axis, reduce out velocity
+            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;
 
-                VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
+            // ==================================================================
+            // Clamp REALLY high or low velocities
+            float newVelocityLengthSq = newVelocity.LengthSquared();
+            if (newVelocityLengthSq > 1e6f)
+            {
+                newVelocity /= newVelocity.Length();
+                newVelocity *= 1000f;
             }
+            else if (newVelocityLengthSq < 1e-6f)
+                newVelocity = Vector3.Zero;
 
-            // m_newVelocity is velocity computed from linear motor in world coordinates
+            // ==================================================================
+            // Stuff new linear velocity into the vehicle
+            Prim.ForceVelocity = newVelocity;
+            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
 
-            // Gravity and Buoyancy
-            // There is some gravity, make a gravity force vector that is applied after object velocity.
-            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
+            // Other linear forces are applied as forces.
+            Vector3 totalDownForce = grav * m_vehicleMass;
+            if (totalDownForce != Vector3.Zero)
+            {
+                Prim.AddForce(totalDownForce, false);
+            }
 
-            /*
-             * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
-            // Preserve the current Z velocity
-            Vector3 vel_now = m_prim.Velocity;
-            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
-             */
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
+                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
+                                newVelocity, Prim.Velocity, totalDownForce);
 
-            Vector3 pos = Prim.ForcePosition;
-//            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);
+        } // end MoveLinear()
 
+        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight)
+        {
+            Vector3 ret = Vector3.Zero;
             // If below the terrain, move us above the ground a little.
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
             //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
             // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
             {
+                // TODO: correct position by applying force rather than forcing position.
                 pos.Z = terrainHeight + 2;
                 Prim.ForcePosition = pos;
                 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
+            return ret;
+        }
+
+        public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight)
+        {
+            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)
@@ -694,28 +744,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // RA: where does the 50 come from?
                     float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
                     // Replace Vertical speed with correction figure if significant
-                    if (Math.Abs(verticalError) > 0.01f)
+                    if (verticalError > 0.01f)
                     {
-                        m_newVelocity.Z += verticalCorrectionVelocity;
+                        ret = new Vector3(0f, 0f, 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},dir={2},height={3},target={4}",
+                                Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight);
             }
 
+            return ret;
+        }
+
+        public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos)
+        {
+            bool changed = false;
+
             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;
@@ -748,75 +801,45 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
+            return changed;
+        }
 
-            #region downForce
-            Vector3 downForce = Vector3.Zero;
-
+        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
+        {
+            Vector3 ret = Vector3.Zero;
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
                 // If the vehicle is motoring into the sky, get it going back down.
-                // Is this an angular force or both linear and angular??
                 float distanceAboveGround = pos.Z - terrainHeight;
-                if (distanceAboveGround > 2f)
+                if (distanceAboveGround > 1f)
                 {
                     // 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
                 //     (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}", 
-                                    Prim.LocalID, distanceAboveGround, downForce);
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
+                                    Prim.LocalID, distanceAboveGround, ret);
             }
-            #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)
-                m_newVelocity = Vector3.Zero;
-
-            // 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()
+            return ret;
+        }
 
         // =======================================================================
+        // =======================================================================
         // Apply the effect of the angular motor.
         private void MoveAngular(float pTimestep)
         {
             // m_angularMotorDirection         // angular velocity requested by LSL motor
-            // m_angularMotorApply             // application frame counter
             // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-            // m_angularMotorTimescale         // motor angular velocity ramp up rate
+            // m_angularMotorTimescale         // motor angular velocity ramp up time
             // m_angularMotorDecayTimescale    // motor angular velocity decay rate
             // m_angularFrictionTimescale      // body angular velocity  decay rate
             // m_lastAngularVelocity           // what was last applied to body
 
+            /*
             if (m_angularMotorDirection.LengthSquared() > 0.0001)
             {
                 Vector3 origVel = m_angularMotorVelocity;
@@ -835,59 +858,152 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_angularMotorVelocity = Vector3.Zero;
             }
+            */
+
+            Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
 
-            #region Vertical attactor
+            // ==================================================================
+            // NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
+            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
+            {
+                angularMotorContribution.X = 0f;
+                angularMotorContribution.Y = 0f;
+                VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
+            }
 
-            Vector3 vertattr = Vector3.Zero;
-            Vector3 deflection = Vector3.Zero;
-            Vector3 banking = Vector3.Zero;
+            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
 
-            // If vertical attaction timescale is reasonable and we applied an angular force last time...
-            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
+            Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
+
+            Vector3 bankingContribution = ComputeAngularBanking(pTimestep);
+
+            // ==================================================================
+            m_lastVertAttractor = verticalAttractionContribution;
+
+            // Sum velocities
+            m_lastAngularVelocity = angularMotorContribution
+                                    + verticalAttractionContribution
+                                    + bankingContribution
+                                    + deflectionContribution;
+
+            // ==================================================================
+            //Offset section
+            if (m_linearMotorOffset != Vector3.Zero)
+            {
+                //Offset of linear velocity doesn't change the linear velocity,
+                //   but causes a torque to be applied, for example...
+                //
+                //      IIIII     >>>   IIIII
+                //      IIIII     >>>    IIIII
+                //      IIIII     >>>     IIIII
+                //          ^
+                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
+                //
+                //
+                // The torque created is the linear velocity crossed with the offset
+
+                // TODO: this computation should be in the linear section
+                //    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))
+                    torqueFromOffset.X = 0;
+                if (float.IsNaN(torqueFromOffset.Y))
+                    torqueFromOffset.Y = 0;
+                if (float.IsNaN(torqueFromOffset.Z))
+                    torqueFromOffset.Z = 0;
+                torqueFromOffset *= m_vehicleMass;
+                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
+                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+            }
+
+            // ==================================================================
+            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                Prim.ZeroAngularMotion(true);
+                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+            }
+            else
             {
-                float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
-                if (Prim.IsColliding)
-                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
+                // Apply to the body.
+                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
+                // Since we are stuffing the angular velocity directly into the object, the computed
+                //     velocity needs to be scaled by the timestep.
+                // Also remove any motion that is on the object so added motion is only from vehicle.
+                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
+                                                - Prim.ForceRotationalVelocity);
+                // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
+                Prim.ForceRotationalVelocity = applyAngularForce;
+
+                VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
+                                    Prim.LocalID,
+                                    angularMotorContribution, verticalAttractionContribution,
+                                    bankingContribution, deflectionContribution,
+                                    applyAngularForce, m_lastAngularVelocity
+                                    );
+            }
+        }
 
-                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+        public Vector3 ComputeAngularVerticalAttraction(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
 
-                // Create a vector of the vehicle "up" in world coordinates
+            // If vertical attaction timescale is reasonable and we applied an angular force last time...
+            if (m_verticalAttractionTimescale < 500)
+            {
+                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
+                verticalError.Normalize();
+                m_verticalAttractionMotor.SetCurrent(verticalError);
+                m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
+                ret = m_verticalAttractionMotor.Step(pTimestep);
+                /*
+                // Take a vector pointing up and convert it from world to vehicle relative coords.
                 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
-                // 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.Normalize();
+
+                // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
+                //    is now leaning to one side (rotated around the X axis) and the Y value will
+                //    go from zero (nearly straight up) to one (completely to the side) or leaning
+                //    front-to-back (rotated around the Y axis) and the value of X will be 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 = 2.0f - verticalError.X;
-                    verticalError.Y = 2.0f - verticalError.Y;
+                    verticalError.X = 2f - verticalError.X;
+                    verticalError.Y = 2f - 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;
+                // Y error means needed rotation around X axis and visa versa.
+                verticalAttractionContribution.X =    verticalError.Y;
+                verticalAttractionContribution.Y =  - verticalError.X;
+                verticalAttractionContribution.Z = 0f;
+
+                // scale by the time scale and timestep
+                Vector3 unscaledContrib = verticalAttractionContribution;
+                verticalAttractionContribution /= m_verticalAttractionTimescale;
+                verticalAttractionContribution *= pTimestep;
 
-                // scaling appears better usingsquare-law
-                Vector3 angularVelocity = Prim.ForceRotationalVelocity;
-                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-                vertattr.X += bounce * angularVelocity.X;
-                vertattr.Y += bounce * angularVelocity.Y;
+                // apply efficiency
+                Vector3 preEfficiencyContrib = verticalAttractionContribution;
+                float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
+                verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
 
-                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
-                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
+                VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
+                                            Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
+                                            m_verticalAttractionEfficiency, efficencySquared,
+                                            verticalAttractionContribution);
+                 */
 
             }
-            #endregion // Vertical attactor
+            return ret;
+        }
 
-            #region Deflection
+        public Vector3 ComputeAngularDeflection(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
 
             if (m_angularDeflectionEfficiency != 0)
             {
@@ -899,32 +1015,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
 
                 // Scale by efficiency and timescale
-                deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+                ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+
+                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret);
 
-                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
-                                        Prim.LocalID, preferredAxisOfMotion, deflection);
                 // This deflection computation is not correct.
-                deflection = Vector3.Zero;
+                ret = Vector3.Zero;
             }
+            return ret;
+        }
 
-            #endregion
-
-            #region Banking
+        public Vector3 ComputeAngularBanking(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
 
             if (m_bankingEfficiency != 0)
             {
                 Vector3 dir = Vector3.One * Prim.ForceOrientation;
-                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
-                    //Changes which way it banks in and out of turns
+                float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
+                //Changes which way it banks in and out of turns
 
                 //Use the square of the efficiency, as it looks much more how SL banking works
-                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
+                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)
                 {
+                    // The vehicle is stopped
                     /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
                     {
                         Vector3 axisAngle;
@@ -938,101 +1057,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     }*/
                 }
                 else
-                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
+                {
+                    ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
+                }
+
+                //If they are colliding, we probably shouldn't shove the prim around... probably
                 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;
+                    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);
+                    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;
+                    ret += 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);
+                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}",
+                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, ret);
             }
+            return ret;
+        }
 
-            #endregion
-
-            m_lastVertAttractor = vertattr;
-
-            // Sum velocities
-            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
-
-            #region Linear Motor Offset
-
-            //Offset section
-            if (m_linearMotorOffset != Vector3.Zero)
-            {
-                //Offset of linear velocity doesn't change the linear velocity,
-                //   but causes a torque to be applied, for example...
-                //
-                //      IIIII     >>>   IIIII
-                //      IIIII     >>>    IIIII
-                //      IIIII     >>>     IIIII
-                //          ^
-                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
-                //
-                //
-                // The torque created is the linear velocity crossed with the offset
-
-                // NOTE: this computation does should be in the linear section
-                //    because there we know the impulse being applied.
-                Vector3 torqueFromOffset = Vector3.Zero;
-                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
-                if (float.IsNaN(torqueFromOffset.X))
-                    torqueFromOffset.X = 0;
-                if (float.IsNaN(torqueFromOffset.Y))
-                    torqueFromOffset.Y = 0;
-                if (float.IsNaN(torqueFromOffset.Z))
-                    torqueFromOffset.Z = 0;
-                torqueFromOffset *= m_vehicleMass;
-                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
-                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
-            }
-
-            #endregion
-
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
-            {
-                m_lastAngularVelocity.X = 0;
-                m_lastAngularVelocity.Y = 0;
-                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
-            }
-
-            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
-            {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                Prim.ZeroAngularMotion(true);
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
-            }
-            else
-            {
-                // Apply to the body.
-                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
-                // Since we are stuffing the angular velocity directly into the object, the computed
-                //     velocity needs to be scaled by the timestep.
-                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
-                Prim.ForceRotationalVelocity = applyAngularForce;
-
-                // Decay the angular movement for next time
-                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
-                m_lastAngularVelocity *= Vector3.One - decayamount;
-
-                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
-                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
-            }
-        } //end MoveAngular
 
+        // 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;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 436e043..0df4310 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -82,9 +82,9 @@ public abstract class BSLinkset
 
     // Some linksets have a preferred physical shape.
     // Returns SHAPE_UNKNOWN if there is no preference. Causes the correct shape to be selected.
-    public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    public virtual BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
     {
-        return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
+        return BSPhysicsShapeType.SHAPE_UNKNOWN;
     }
 
     // Linksets move around the children so the linkset might need to compute the child position
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 3238c85..b9c2cf9 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -42,12 +42,12 @@ public sealed class BSLinksetCompound : BSLinkset
     }
 
     // For compound implimented linksets, if there are children, use compound shape for the root.
-    public override ShapeData.PhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
     { 
-        ShapeData.PhysicsShapeType ret = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
+        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
         if (IsRoot(requestor) && HasAnyChildren)
         {
-            ret = ShapeData.PhysicsShapeType.SHAPE_COMPOUND;
+            ret = BSPhysicsShapeType.SHAPE_COMPOUND;
         }
         // DetailLog("{0},BSLinksetCompound.PreferredPhysicalShape,call,shape={1}", LinksetRoot.LocalID, ret);
         return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
new file mode 100755
index 0000000..663b6f4
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMaterials.cs
@@ -0,0 +1,191 @@
+/*

+ * 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.

+    public static string[] MaterialNames = { "Stone", "Metal", "Glass", "Wood", 

+                                     "Flesh", "Plastic", "Rubber", "Light", "Avatar" };

+    public static string[] MaterialAttribs = { "Density", "Friction", "Restitution", 

+                                   "ccdMotionThreshold", "ccdSweptSphereRadius" };

+

+    public MaterialAttributes(string t, float d, float f, float r, float ccdM, float ccdS)

+    {

+        type = t;

+        density = d;

+        friction = f;

+        restitution = r;

+        ccdMotionThreshold = ccdM;

+        ccdSweptSphereRadius = ccdS;

+    }

+    public string type;

+    public float density;

+    public float friction;

+    public float restitution;

+    public float ccdMotionThreshold;

+    public float ccdSweptSphereRadius;

+}

+

+public static class BSMaterials

+{

+    public static MaterialAttributes[] Attributes;

+

+    static BSMaterials()

+    {

+        // Attribute sets for both the non-physical and physical instances of materials.

+        Attributes = new MaterialAttributes[(int)MaterialAttributes.Material.NumberOfTypes * 2];

+    }

+

+    // This is where all the default material attributes are defined.

+    public static void InitializeFromDefaults(ConfigurationParameters parms)

+    {

+    // public static string[] MaterialNames = { "Stone", "Metal", "Glass", "Wood", 

+      //                                "Flesh", "Plastic", "Rubber", "Light", "Avatar" };

+        float dFriction = parms.defaultFriction;

+        float dRestitution = parms.defaultRestitution;

+        float dDensity = parms.defaultDensity;

+        float dCcdM = parms.ccdMotionThreshold;

+        float dCcdS = parms.ccdSweptSphereRadius;

+        Attributes[(int)MaterialAttributes.Material.Stone] =

+            new MaterialAttributes("stone",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Metal] =

+            new MaterialAttributes("metal",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Glass] =

+            new MaterialAttributes("glass",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Wood] =

+            new MaterialAttributes("wood",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Flesh] =

+            new MaterialAttributes("flesh",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Plastic] =

+            new MaterialAttributes("plastic",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Rubber] =

+            new MaterialAttributes("rubber",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Light] =

+            new MaterialAttributes("light",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Avatar] =

+            new MaterialAttributes("avatar",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+

+        Attributes[(int)MaterialAttributes.Material.Stone + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("stonePhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Metal + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("metalPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Glass + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("glassPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Wood + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("woodPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Flesh + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("fleshPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Plastic + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("plasticPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Rubber + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("rubberPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Light + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("lightPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+        Attributes[(int)MaterialAttributes.Material.Avatar + (int)MaterialAttributes.Material.NumberOfTypes] =

+            new MaterialAttributes("avatarPhysical",dDensity,dFriction,dRestitution, dCcdM, dCcdS);

+    }

+

+    // 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)

+    {

+        int matType = 0;

+        foreach (string matName in MaterialAttributes.MaterialNames)

+        {

+            foreach (string attribName in MaterialAttributes.MaterialAttribs)

+            {

+                string paramName = matName + attribName;

+                if (pConfig.Contains(paramName))

+                {

+                    float paramValue = pConfig.GetFloat(paramName);

+                    SetAttributeValue(matType, attribName, paramValue);

+                    // set the physical value also

+                    SetAttributeValue(matType + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);

+                }

+                paramName += "Physical";

+                if (pConfig.Contains(paramName))

+                {

+                    float paramValue = pConfig.GetFloat(paramName);

+                    SetAttributeValue(matType + (int)MaterialAttributes.Material.NumberOfTypes, attribName, paramValue);

+                }

+            }

+            matType++;

+        }

+    }

+

+    private static void SetAttributeValue(int matType, string attribName, float val)

+    {

+        MaterialAttributes thisAttrib = Attributes[matType];

+        FieldInfo fieldInfo = thisAttrib.GetType().GetField(attribName);

+        if (fieldInfo != null)

+        {

+            fieldInfo.SetValue(thisAttrib, val);

+            Attributes[matType] = thisAttrib;

+        }

+    }

+

+    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..e91bfa8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -1,104 +1,169 @@
-using System;

-using System.Collections.Generic;

-using System.Text;

-using OpenMetaverse;

-

-namespace OpenSim.Region.Physics.BulletSPlugin

-{

-public abstract class BSMotor

-{

-    public virtual void Reset() { }

-    public virtual void Zero() { }

-}

-// Can all the incremental stepping be replaced with motor classes?

-public class BSVMotor : BSMotor

-{

-    public Vector3 FrameOfReference { get; set; }

-    public Vector3 Offset { get; set; }

-

-    public float TimeScale { get; set; }

-    public float TargetValueDecayTimeScale { get; set; }

-    public Vector3 CurrentValueReductionTimescale { get; set; }

-    public float Efficiency { get; set; }

-

-    public Vector3 TargetValue { get; private set; }

-    public Vector3 CurrentValue { get; private set; }

-

-

-

-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)

-    {

-        TimeScale = timeScale;

-        TargetValueDecayTimeScale = decayTimeScale;

-        CurrentValueReductionTimescale = frictionTimeScale;

-        Efficiency = efficiency;

-    }

-    public void SetCurrent(Vector3 current)

-    {

-        CurrentValue = current;

-    }

-    public void SetTarget(Vector3 target)

-    {

-        TargetValue = target;

-    }

-    public Vector3 Step(float timeStep)

-    {

-        if (CurrentValue.LengthSquared() > 0.001f)

-        {

-            // Vector3 origDir = Target;       // DEBUG

-            // Vector3 origVel = CurrentValue;   // DEBUG

-

-            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete

-            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;

-            CurrentValue += addAmount;

-

-            float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;

-            TargetValue *= (1f - decayFactor);

-

-            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;

-            CurrentValue *= (Vector3.One - frictionFactor);

-        }

-        else

-        {

-            // if what remains of direction is very small, zero it.

-            TargetValue = Vector3.Zero;

-            CurrentValue = Vector3.Zero;

-

-            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);

-        }

-        return CurrentValue;

-    }

-}

-

-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; }

-

-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)

-    {

-    }

-    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

-    BSPIDMotor()

-    {

-    }

-}

-}

+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public abstract class BSMotor
+{
+    // Timescales and other things can be turned off by setting them to 'infinite'.
+    public const float Infinite = 10000f;
+    public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
+
+    public BSMotor(string useName)
+    {
+        UseName = useName;
+        PhysicsScene = null;
+    }
+    public virtual void Reset() { }
+    public virtual void Zero() { }
+
+    public string UseName { get; private set; }
+    // Used only for outputting debug information. Might not be set so check for null.
+    public BSScene PhysicsScene { get; set; }
+    protected void MDetailLog(string msg, params Object[] parms)
+    {
+        if (PhysicsScene != null)
+        {
+            if (PhysicsScene.VehicleLoggingEnabled)
+            {
+                PhysicsScene.DetailLog(msg, parms);
+            }
+        }
+    }
+}
+// Can all the incremental stepping be replaced with motor classes?
+public class BSVMotor : BSMotor
+{
+    public Vector3 FrameOfReference { get; set; }
+    public Vector3 Offset { get; set; }
+
+    public float TimeScale { get; set; }
+    public float TargetValueDecayTimeScale { get; set; }
+    public Vector3 FrictionTimescale { get; set; }
+    public float Efficiency { get; set; }
+
+    public Vector3 TargetValue { get; private set; }
+    public Vector3 CurrentValue { get; private set; }
+
+    public BSVMotor(string useName)
+        : base(useName)
+    {
+        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
+        Efficiency = 1f;
+        FrictionTimescale = BSMotor.InfiniteVector;
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
+    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;
+    }
+    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;
+
+            returnCurrent = CurrentValue;
+
+            // 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;
+                frictionFactor.X = FrictionTimescale.X == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.X) * timeStep;
+                frictionFactor.Y = FrictionTimescale.Y == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Y) * timeStep;
+                frictionFactor.Z = FrictionTimescale.Z == BSMotor.Infinite ? 0f : (1f / FrictionTimescale.Z) * timeStep;
+                CurrentValue *= (Vector3.One - frictionFactor);
+            }
+
+            MDetailLog("{0},BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
+                                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 e803072..f6a890e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -47,7 +47,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  */
 public abstract class BSPhysObject : PhysicsActor
 {
-    protected void BaseInitialize(BSScene parentScene, uint localID, string name, string typeName)
+    protected BSPhysObject()
+    {
+    }
+    protected BSPhysObject(BSScene parentScene, uint localID, string name, string typeName)
     {
         PhysicsScene = parentScene;
         LocalID = localID;
@@ -91,9 +94,9 @@ public abstract class BSPhysObject : PhysicsActor
     public PrimitiveBaseShape BaseShape { get; protected set; }
     // Some types of objects have preferred physical representations.
     // Returns SHAPE_UNKNOWN if there is no preference.
-    public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape
+    public virtual BSPhysicsShapeType PreferredPhysicalShape
     {
-        get { return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN; }
+        get { return BSPhysicsShapeType.SHAPE_UNKNOWN; }
     }
 
     // When the physical properties are updated, an EntityProperty holds the update values.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 14eb505..c62c79a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -47,7 +47,6 @@ public sealed class BSPrim : BSPhysObject
     // _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 OMV.Vector3 _scale; // the multiplier for each mesh dimension for the mesh as created by the meshmerizer
 
     private bool _grabbed;
     private bool _isSelected;
@@ -88,13 +87,13 @@ public sealed class BSPrim : BSPhysObject
 
     public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
+            : base(parent_scene, localID, primName, "BSPrim")
     {
         // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID);
-        base.BaseInitialize(parent_scene, localID, primName, "BSPrim");
         _physicsActorType = (int)ActorTypes.Prim;
         _position = pos;
         _size = size;
-        Scale = new OMV.Vector3(1f, 1f, 1f);   // the scale will be set by CreateGeom depending on object type
+        Scale = size;   // the scale will be set by CreateGeom depending on object type
         _orientation = rotation;
         _buoyancy = 1f;
         _velocity = OMV.Vector3.Zero;
@@ -155,6 +154,8 @@ public sealed class BSPrim : BSPhysObject
     public override OMV.Vector3 Size {
         get { return _size; }
         set {
+            // 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;
             ForceBodyShapeRebuild(false);
         }
@@ -170,7 +171,7 @@ public sealed class BSPrim : BSPhysObject
         }
     }
     // Whatever the linkset wants is what I want.
-    public override ShapeData.PhysicsShapeType PreferredPhysicalShape
+    public override BSPhysicsShapeType PreferredPhysicalShape
         { get { return Linkset.PreferredPhysicalShape(this); } }
 
     public override bool ForceBodyShapeRebuild(bool inTaintTime)
@@ -274,19 +275,19 @@ public sealed class BSPrim : BSPhysObject
             if (!Linkset.IsRoot(this))
                 _position = Linkset.Position(this);
 
-            // don't do the GetObjectPosition for root elements because this function is called a zillion times
+            // don't do the GetObjectPosition for root elements because this function is called a zillion times.
             // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
             return _position;
         }
         set {
-            // If you must push the position into the physics engine, use ForcePosition.
+            // If the position must be forced into the physics engine, use ForcePosition.
             if (_position == value)
             {
                 return;
             }
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
-            PositionSanityCheck();
+            PositionSanityCheck(false);
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
@@ -302,7 +303,7 @@ public sealed class BSPrim : BSPhysObject
         }
         set {
             _position = value;
-            PositionSanityCheck();
+            // PositionSanityCheck();   // Don't do this! Causes a loop and caller should know better.
             BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
             ActivateIfPhysical(false);
         }
@@ -311,52 +312,42 @@ public sealed class BSPrim : BSPhysObject
     // Check that the current position is sane and, if not, modify the position to make it so.
     // Check for being below terrain and being out of bounds.
     // Returns 'true' of the position was made sane by some action.
-    private bool PositionSanityCheck()
+    private bool PositionSanityCheck(bool inTaintTime)
     {
         bool ret = false;
 
-        // If totally below the ground, move the prim up
-        // TODO: figure out the right solution for this... only for dynamic objects?
-        /*
         float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        OMV.Vector3 upForce = OMV.Vector3.Zero;
         if (Position.Z < terrainHeight)
         {
             DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + 2.0f;
+            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;
             ret = true;
         }
-         */
+
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
             float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
             // TODO: a floating motor so object will bob in the water
-            if (Position.Z < waterHeight)
+            if (Math.Abs(Position.Z - waterHeight) > 0.1f)
             {
-                _position.Z = waterHeight;
+                // Upforce proportional to the distance away from the water. Correct the error in 1 sec.
+                upForce.Z = (waterHeight - Position.Z) * 1f;
                 ret = true;
             }
         }
 
         // TODO: check for out of bounds
-        return ret;
-    }
 
-    // A version of the sanity check that also makes sure a new position value is
-    //    pushed to the physics engine. This routine would be used by anyone
-    //    who is not already pushing the value.
-    private bool PositionSanityCheck(bool inTaintTime)
-    {
-        bool ret = false;
-        if (PositionSanityCheck())
+        // 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)
         {
-            // The new position value must be pushed into the physics engine but we can't
-            //    just assign to "Position" because of potential call loops.
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck", delegate()
-            {
-                DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
-            });
-            ret = true;
+            // Apply upforce and overcome gravity.
+            AddForce(upForce - PhysicsScene.DefaultGravity, false, inTaintTime);
         }
         return ret;
     }
@@ -940,6 +931,7 @@ public sealed class BSPrim : BSPhysObject
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
         AddForce(force, pushforce, false);
     }
+    // Applying a force just adds this to the total force on the object.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
@@ -971,6 +963,7 @@ public sealed class BSPrim : BSPhysObject
         });
     }
 
+    // An impulse force is scaled by the mass of the object.
     public void ApplyForceImpulse(OMV.Vector3 impulse, bool inTaintTime)
     {
         OMV.Vector3 applyImpulse = impulse;
@@ -1387,67 +1380,34 @@ public sealed class BSPrim : BSPhysObject
 
     public override void UpdateProperties(EntityProperties entprop)
     {
-        /*
-        UpdatedProperties changed = 0;
-        // 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)
+        // Updates only for individual prims and for the root object of a linkset.
+        if (Linkset.IsRoot(this))
         {
-            // Only update the position of single objects and linkset roots
-            if (this._parentPrim == null)
+            // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
+            // 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 to the local variables so the normal set action does not happen
+            // Assign directly to the local variables so the normal set action does not happen
             _position = entprop.Position;
             _orientation = entprop.Rotation;
             _velocity = entprop.Velocity;
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
+            // The sanity check can change the velocity and/or position.
+            if (PositionSanityCheck(true))
+            {
+                entprop.Position = _position;
+                entprop.Velocity = _velocity;
+            }
+
             // remember the current and last set values
             LastEntityProperties = CurrentEntityProperties;
             CurrentEntityProperties = entprop;
 
-            PositionSanityCheck(true);
-
             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);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 1cc607a..09b1423 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
@@ -234,6 +221,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
         {
@@ -308,6 +296,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 // Do any replacements in the parameters
                 m_physicsLoggingPrefix = m_physicsLoggingPrefix.Replace("%REGIONNAME%", RegionName);
             }
+
+            // The material characteristics.
+            BSMaterials.InitializeFromDefaults(Params);
+            if (pConfig != null)
+            {
+                BSMaterials.InitializefromParameters(pConfig);
+            }
         }
     }
 
@@ -520,9 +515,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.
@@ -568,6 +563,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 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)
@@ -591,9 +587,8 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         // 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).
-        return numSubSteps * m_fixedTimeStep * 1000 * 55;
-        // return timeStep * 1000 * 55;
+        // Multiply by 55 to give a nominal frame rate of 55.
+        return (float)numSubSteps * m_fixedTimeStep * 1000f * 55f;
     }
 
     // Something has collided
@@ -683,7 +678,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     #region Taints
 
     // Calls to the PhysicsActors can't directly call into the physics engine
-    // because it might be busy. We delay changes to a known time.
+    //       because it might be busy. We delay changes to a known time.
     // We rely on C#'s closure to save and restore the context for the delegate.
     public void TaintedObject(String ident, TaintCallback callback)
     {
@@ -712,7 +707,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     // here just before the physics engine is called to step the simulation.
     public void ProcessTaints()
     {
-        InTaintTime = true;
+        InTaintTime = true; // Only used for debugging so locking is not necessary.
         ProcessRegularTaints();
         ProcessPostTaintTaints();
         InTaintTime = false;
@@ -758,6 +753,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 DetailLog("{0},BSScene.ProcessTaints,leftTaintsOnList,numNotProcessed={1}", DetailLogZero, _taintOperations.Count);
             }
              */
+
             // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
@@ -787,8 +783,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     //     will replace any previous operation by the same object.
     public void PostTaintObject(String ident, uint ID, TaintCallback callback)
     {
-        if (!m_initialized) return;
-
         string uniqueIdent = ident + "-" + ID.ToString();
         lock (_taintLock)
         {
@@ -864,13 +858,14 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         }
     }
 
+    // Only used for debugging. Does not change state of anything so locking is not necessary.
     public bool AssertInTaintTime(string whereFrom)
     {
         if (!InTaintTime)
         {
             DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
             m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-            Util.PrintCallStack();
+            Util.PrintCallStack();  // Prints the stack into the DEBUG log file.
         }
         return InTaintTime;
     }
@@ -1069,7 +1064,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 +1079,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; } ),
@@ -1145,8 +1140,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].contactProcessingThreshold, p, l, v); },
             (s,o,v) => { BulletSimAPI.SetContactProcessingThreshold2(o.PhysBody.ptr, v); } ),
 
+            new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
+            (float)BSTerrainPhys.TerrainImplementation.Mesh,
+            (s,cf,p,v) => { s.m_params[0].terrainImplementation = cf.GetFloat(p,v); },
+            (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 */} ),
@@ -1160,13 +1160,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; } ),
@@ -1180,11 +1186,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].avatarRestitution = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].avatarRestitution; },
             (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarRestitution, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleRadius", "Radius of space around an avatar",
-            0.37f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleRadius = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleRadius; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleRadius, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
+            0.6f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleWidth = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleWidth; },
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleWidth, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
+            0.45f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleDepth = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleDepth; },
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleDepth, p, l, v); } ),
         new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar",
             1.5f,
             (s,cf,p,v) => { s.m_params[0].avatarCapsuleHeight = cf.GetFloat(p, v); },
@@ -1196,6 +1207,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,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 29a23c0..892c34b 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -178,7 +178,7 @@ public sealed class BSShapeCollection : IDisposable
         bool ret = false;
         switch (shape.type)
         {
-            case ShapeData.PhysicsShapeType.SHAPE_MESH:
+            case BSPhysicsShapeType.SHAPE_MESH:
                 MeshDesc meshDesc;
                 if (Meshes.TryGetValue(shape.shapeKey, out meshDesc))
                 {
@@ -201,7 +201,7 @@ public sealed class BSShapeCollection : IDisposable
                 meshDesc.lastReferenced = System.DateTime.Now;
                 Meshes[shape.shapeKey] = meshDesc;
                 break;
-            case ShapeData.PhysicsShapeType.SHAPE_HULL:
+            case BSPhysicsShapeType.SHAPE_HULL:
                 HullDesc hullDesc;
                 if (Hulls.TryGetValue(shape.shapeKey, out hullDesc))
                 {
@@ -224,7 +224,7 @@ public sealed class BSShapeCollection : IDisposable
                 hullDesc.lastReferenced = System.DateTime.Now;
                 Hulls[shape.shapeKey] = hullDesc;
                 break;
-            case ShapeData.PhysicsShapeType.SHAPE_UNKNOWN:
+            case BSPhysicsShapeType.SHAPE_UNKNOWN:
                 break;
             default:
                 // Native shapes are not tracked and they don't go into any list
@@ -255,16 +255,16 @@ public sealed class BSShapeCollection : IDisposable
                 {
                     switch (shape.type)
                     {
-                        case ShapeData.PhysicsShapeType.SHAPE_HULL:
+                        case BSPhysicsShapeType.SHAPE_HULL:
                             DereferenceHull(shape, shapeCallback);
                             break;
-                        case ShapeData.PhysicsShapeType.SHAPE_MESH:
+                        case BSPhysicsShapeType.SHAPE_MESH:
                             DereferenceMesh(shape, shapeCallback);
                             break;
-                        case ShapeData.PhysicsShapeType.SHAPE_COMPOUND:
+                        case BSPhysicsShapeType.SHAPE_COMPOUND:
                             DereferenceCompound(shape, shapeCallback);
                             break;
-                        case ShapeData.PhysicsShapeType.SHAPE_UNKNOWN:
+                        case BSPhysicsShapeType.SHAPE_UNKNOWN:
                             break;
                         default:
                             break;
@@ -352,28 +352,28 @@ public sealed class BSShapeCollection : IDisposable
         BulletShape shapeInfo = new BulletShape(cShape);
         if (TryGetMeshByPtr(cShape, out meshDesc))
         {
-            shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_MESH;
+            shapeInfo.type = BSPhysicsShapeType.SHAPE_MESH;
             shapeInfo.shapeKey = meshDesc.shapeKey;
         }
         else
         {
             if (TryGetHullByPtr(cShape, out hullDesc))
             {
-                shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_HULL;
+                shapeInfo.type = BSPhysicsShapeType.SHAPE_HULL;
                 shapeInfo.shapeKey = hullDesc.shapeKey;
             }
             else
             {
                 if (BulletSimAPI.IsCompound2(cShape))
                 {
-                    shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_COMPOUND;
+                    shapeInfo.type = BSPhysicsShapeType.SHAPE_COMPOUND;
                 }
                 else
                 {
                     if (BulletSimAPI.IsNativeShape2(cShape))
                     {
                         shapeInfo.isNativeShape = true;
-                        shapeInfo.type = ShapeData.PhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
+                        shapeInfo.type = BSPhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
                     }
                 }
             }
@@ -381,7 +381,7 @@ public sealed class BSShapeCollection : IDisposable
 
         DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
 
-        if (shapeInfo.type != ShapeData.PhysicsShapeType.SHAPE_UNKNOWN)
+        if (shapeInfo.type != BSPhysicsShapeType.SHAPE_UNKNOWN)
         {
             DereferenceShape(shapeInfo, true, null);
         }
@@ -405,11 +405,11 @@ public sealed class BSShapeCollection : IDisposable
         bool ret = false;
         bool haveShape = false;
 
-        if (!haveShape && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
             // an avatar capsule is close to a native shape (it is not shared)
-            ret = GetReferenceToNativeShape(prim, ShapeData.PhysicsShapeType.SHAPE_AVATAR,
-                            ShapeData.FixedShapeKey.KEY_CAPSULE, shapeCallback);
+            ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
+                            FixedShapeKey.KEY_CAPSULE, shapeCallback);
             DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
             ret = true;
             haveShape = true;
@@ -417,7 +417,7 @@ public sealed class BSShapeCollection : IDisposable
 
         // 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.
-        if (!haveShape && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_COMPOUND)
+        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
         {
             ret = GetReferenceToCompoundShape(prim, shapeCallback);
             DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
@@ -460,11 +460,11 @@ public sealed class BSShapeCollection : IDisposable
                 haveShape = true;
                 if (forceRebuild
                         || prim.Scale != prim.Size
-                        || prim.PhysShape.type != ShapeData.PhysicsShapeType.SHAPE_SPHERE
+                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_SPHERE
                         )
                 {
-                    ret = GetReferenceToNativeShape(prim, ShapeData.PhysicsShapeType.SHAPE_SPHERE,
-                                            ShapeData.FixedShapeKey.KEY_SPHERE, shapeCallback);
+                    ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
+                                            FixedShapeKey.KEY_SPHERE, shapeCallback);
                     DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
                                         prim.LocalID, forceRebuild, prim.PhysShape);
                 }
@@ -474,11 +474,11 @@ public sealed class BSShapeCollection : IDisposable
                 haveShape = true;
                 if (forceRebuild
                         || prim.Scale != prim.Size
-                        || prim.PhysShape.type != ShapeData.PhysicsShapeType.SHAPE_BOX
+                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_BOX
                         )
                 {
-                    ret = GetReferenceToNativeShape( prim, ShapeData.PhysicsShapeType.SHAPE_BOX,
-                                            ShapeData.FixedShapeKey.KEY_BOX, shapeCallback);
+                    ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
+                                            FixedShapeKey.KEY_BOX, shapeCallback);
                     DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
                                         prim.LocalID, forceRebuild, prim.PhysShape);
                 }
@@ -519,15 +519,12 @@ public sealed class BSShapeCollection : IDisposable
     // Creates a native shape and assignes it to prim.BSShape.
     // "Native" shapes are never shared. they are created here and destroyed in DereferenceShape().
     private bool GetReferenceToNativeShape(BSPhysObject prim,
-                            ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey,
+                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey,
                             ShapeDestructionCallback shapeCallback)
     {
         // release any previous shape
         DereferenceShape(prim.PhysShape, true, shapeCallback);
 
-        // Bullet native objects are scaled by the Bullet engine so pass the size in
-        prim.Scale = prim.Size;
-
         BulletShape newShape = BuildPhysicalNativeShape(prim, shapeType, shapeKey);
 
         // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
@@ -538,8 +535,8 @@ public sealed class BSShapeCollection : IDisposable
         return true;
     }
 
-    private BulletShape BuildPhysicalNativeShape(BSPhysObject prim, ShapeData.PhysicsShapeType shapeType,
-                                    ShapeData.FixedShapeKey shapeKey)
+    private BulletShape BuildPhysicalNativeShape(BSPhysObject prim, BSPhysicsShapeType shapeType,
+                                    FixedShapeKey shapeKey)
     {
         BulletShape newShape;
         // Need to make sure the passed shape information is for the native type.
@@ -547,12 +544,13 @@ public sealed class BSShapeCollection : IDisposable
         nativeShapeData.Type = shapeType;
         nativeShapeData.ID = prim.LocalID;
         nativeShapeData.Scale = prim.Scale;
-        nativeShapeData.Size = prim.Scale;
+        nativeShapeData.Size = prim.Scale;  // unneeded, I think.
         nativeShapeData.MeshKey = (ulong)shapeKey;
         nativeShapeData.HullKey = (ulong)shapeKey;
 
-        if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
+            // The proper scale has been calculated in the prim.
             newShape = new BulletShape(
                         BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1f, 1f, prim.Scale)
                         , shapeType);
@@ -560,6 +558,9 @@ public sealed class BSShapeCollection : IDisposable
         }
         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)
@@ -585,7 +586,7 @@ public sealed class BSShapeCollection : IDisposable
         System.UInt64 newMeshKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
 
         // if this new shape is the same as last time, don't recreate the mesh
-        if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == ShapeData.PhysicsShapeType.SHAPE_MESH)
+        if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_MESH)
             return false;
 
         DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
@@ -643,7 +644,7 @@ public sealed class BSShapeCollection : IDisposable
                                     indices.GetLength(0), indices, vertices.Count, verticesAsFloats);
             }
         }
-        BulletShape newShape = new BulletShape(meshPtr, ShapeData.PhysicsShapeType.SHAPE_MESH);
+        BulletShape newShape = new BulletShape(meshPtr, BSPhysicsShapeType.SHAPE_MESH);
         newShape.shapeKey = newMeshKey;
 
         return newShape;
@@ -659,7 +660,7 @@ public sealed class BSShapeCollection : IDisposable
         System.UInt64 newHullKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
 
         // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == ShapeData.PhysicsShapeType.SHAPE_HULL)
+        if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_HULL)
             return false;
 
         DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
@@ -780,7 +781,7 @@ public sealed class BSShapeCollection : IDisposable
             }
         }
 
-        BulletShape newShape = new BulletShape(hullPtr, ShapeData.PhysicsShapeType.SHAPE_HULL);
+        BulletShape newShape = new BulletShape(hullPtr, BSPhysicsShapeType.SHAPE_HULL);
         newShape.shapeKey = newHullKey;
 
         return newShape;        // 'true' means a new shape has been added to this prim
@@ -803,7 +804,7 @@ public sealed class BSShapeCollection : IDisposable
         // DereferenceShape(prim.PhysShape, true, shapeCallback);
 
         BulletShape cShape = new BulletShape(
-            BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr, false), ShapeData.PhysicsShapeType.SHAPE_COMPOUND);
+            BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr, false), BSPhysicsShapeType.SHAPE_COMPOUND);
 
         // Create the shape for the root prim and add it to the compound shape. Cannot be a native shape.
         CreateGeomMeshOrHull(prim, shapeCallback);
@@ -894,7 +895,7 @@ public sealed class BSShapeCollection : IDisposable
 
         // While we figure out the real problem, stick a simple native shape on the object.
         BulletShape fillinShape =
-            BuildPhysicalNativeShape(prim, ShapeData.PhysicsShapeType.SHAPE_BOX, ShapeData.FixedShapeKey.KEY_BOX);
+            BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
 
         return fillinShape;
     }
@@ -940,7 +941,7 @@ public sealed class BSShapeCollection : IDisposable
             else
             {
                 bodyPtr = BulletSimAPI.CreateGhostFromShape2(sim.ptr, shape.ptr,
-                                        prim.LocalID, prim.ForcePosition, prim.ForceOrientation);
+                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
                 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/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
index 5e2c4a8..96cd55e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -1,213 +1,208 @@
-/*

- * 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.Linq;

-using System.Text;

-

-namespace OpenSim.Region.Physics.BulletSPlugin

-{

-public abstract class BSShape

-{

-    public IntPtr ptr { get; set; }

-    public ShapeData.PhysicsShapeType type { get; set; }

-    public System.UInt64 key { get; set; }

-    public int referenceCount { get; set; }

-    public DateTime lastReferenced { get; set; }

-

-    protected void Initialize()

-    {

-        ptr = IntPtr.Zero;

-        type = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;

-        key = 0;

-        referenceCount = 0;

-        lastReferenced = DateTime.Now;

-    }

-

-    // Get a reference to a physical shape. Create if it doesn't exist

-    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

-    {

-        BSShape ret = null;

-

-        if (prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)

-        {

-            // an avatar capsule is close to a native shape (it is not shared)

-            ret = BSShapeNative.GetReference(physicsScene, prim, ShapeData.PhysicsShapeType.SHAPE_AVATAR,

-                                        ShapeData.FixedShapeKey.KEY_CAPSULE);

-            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);

-        }

-

-        // 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.

-        if (ret == null  && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_COMPOUND)

-        {

-            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added

-            ret = BSShapeCompound.GetReference(prim);

-            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);

-        }

-

-        if (ret == null)

-            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);

-

-        return ret;

-    }

-    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

-    {

-        return null;

-    }

-    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

-    {

-        return null;

-    }

-

-    // Release the use of a physical shape.

-    public abstract void Dereference(BSScene physicsScene);

-

-    // All shapes have a static call to get a reference to the physical shape

-    // protected abstract static BSShape GetReference();

-

-    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(key.ToString("X"));

-        buff.Append(",c=");

-        buff.Append(referenceCount.ToString());

-        buff.Append(">");

-        return buff.ToString();

-    }

-}

-

-public class BSShapeNull : BSShape

-{

-    public BSShapeNull()

-    {

-        base.Initialize();

-    }

-    public static BSShape GetReference() { return new BSShapeNull();  }

-    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }

-}

-

-public class BSShapeNative : BSShape

-{

-    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";

-    public BSShapeNative()

-    {

-        base.Initialize();

-    }

-    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 

-                    ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey) 

-    {

-        // Native shapes are not shared and are always built anew.

-        return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);

-    }

-

-    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,

-                    ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey)

-    {

-        ShapeData nativeShapeData = new ShapeData();

-        nativeShapeData.Type = shapeType;

-        nativeShapeData.ID = prim.LocalID;

-        nativeShapeData.Scale = prim.Scale;

-        nativeShapeData.Size = prim.Scale;

-        nativeShapeData.MeshKey = (ulong)shapeKey;

-        nativeShapeData.HullKey = (ulong)shapeKey;

-

-       

-        if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)

-        {

-            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);

-            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);

-        }

-        else

-        {

-            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);

-        }

-        if (ptr == IntPtr.Zero)

-        {

-            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",

-                                    LogHeader, prim.LocalID, shapeType);

-        }

-        type = shapeType;

-        key = (UInt64)shapeKey;

-    }

-    // Make this reference to the physical shape go away since native shapes are not shared.

-    public override void Dereference(BSScene physicsScene)

-    {

-        // Native shapes are not tracked and are released immediately

-        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);

-        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);

-        ptr = IntPtr.Zero;

-        // Garbage collection will free up this instance.

-    }

-}

-

-public class BSShapeMesh : BSShape

-{

-    private static string LogHeader = "[BULLETSIM SHAPE MESH]";

-    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();

-

-    public BSShapeMesh()

-    {

-        base.Initialize();

-    }

-    public static BSShape GetReference() { return new BSShapeNull();  }

-    public override void Dereference(BSScene physicsScene) { }

-}

-

-public class BSShapeHull : BSShape

-{

-    private static string LogHeader = "[BULLETSIM SHAPE HULL]";

-    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();

-

-    public BSShapeHull()

-    {

-        base.Initialize();

-    }

-    public static BSShape GetReference() { return new BSShapeNull();  }

-    public override void Dereference(BSScene physicsScene) { }

-}

-

-public class BSShapeCompound : BSShape

-{

-    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";

-    public BSShapeCompound()

-    {

-        base.Initialize();

-    }

-    public static BSShape GetReference(BSPhysObject prim) 

-    { 

-        return new BSShapeNull();

-    }

-    public override void Dereference(BSScene physicsScene) { }

-}

-}

+/*
+ * 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.Linq;
+using System.Text;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public abstract class BSShape
+{
+    public IntPtr ptr { get; set; }
+    public BSPhysicsShapeType type { get; set; }
+    public System.UInt64 key { get; set; }
+    public int referenceCount { get; set; }
+    public DateTime lastReferenced { get; set; }
+
+    public BSShape()
+    {
+        ptr = IntPtr.Zero;
+        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        key = 0;
+        referenceCount = 0;
+        lastReferenced = DateTime.Now;
+    }
+
+    // Get a reference to a physical shape. Create if it doesn't exist
+    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        BSShape ret = null;
+
+        if (prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
+        {
+            // an avatar capsule is close to a native shape (it is not shared)
+            ret = BSShapeNative.GetReference(physicsScene, prim, BSPhysicsShapeType.SHAPE_CAPSULE,
+                                        FixedShapeKey.KEY_CAPSULE);
+            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);
+        }
+
+        // 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.
+        if (ret == null  && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
+        {
+            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added
+            ret = BSShapeCompound.GetReference(prim);
+            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
+        }
+
+        if (ret == null)
+            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
+
+        return ret;
+    }
+    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        return null;
+    }
+    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        return null;
+    }
+
+    // Release the use of a physical shape.
+    public abstract void Dereference(BSScene physicsScene);
+
+    // All shapes have a static call to get a reference to the physical shape
+    // protected abstract static BSShape GetReference();
+
+    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(key.ToString("X"));
+        buff.Append(",c=");
+        buff.Append(referenceCount.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+
+public class BSShapeNull : BSShape
+{
+    public BSShapeNull() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
+}
+
+public class BSShapeNative : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
+    public BSShapeNative() : base()
+    {
+    }
+    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 
+                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
+    {
+        // Native shapes are not shared and are always built anew.
+        return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
+    }
+
+    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
+                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
+    {
+        ShapeData nativeShapeData = new ShapeData();
+        nativeShapeData.Type = shapeType;
+        nativeShapeData.ID = prim.LocalID;
+        nativeShapeData.Scale = prim.Scale;
+        nativeShapeData.Size = prim.Scale;
+        nativeShapeData.MeshKey = (ulong)shapeKey;
+        nativeShapeData.HullKey = (ulong)shapeKey;
+
+       
+        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
+        {
+            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
+            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+        }
+        else
+        {
+            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);
+        }
+        if (ptr == IntPtr.Zero)
+        {
+            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
+                                    LogHeader, prim.LocalID, shapeType);
+        }
+        type = shapeType;
+        key = (UInt64)shapeKey;
+    }
+    // Make this reference to the physical shape go away since native shapes are not shared.
+    public override void Dereference(BSScene physicsScene)
+    {
+        // Native shapes are not tracked and are released immediately
+        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
+        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);
+        ptr = IntPtr.Zero;
+        // Garbage collection will free up this instance.
+    }
+}
+
+public class BSShapeMesh : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE MESH]";
+    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
+
+    public BSShapeMesh() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { }
+}
+
+public class BSShapeHull : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE HULL]";
+    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
+
+    public BSShapeHull() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { }
+}
+
+public class BSShapeCompound : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
+    public BSShapeCompound() : base()
+    {
+    }
+    public static BSShape GetReference(BSPhysObject prim) 
+    { 
+        return new BSShapeNull();
+    }
+    public override void Dereference(BSScene physicsScene) { }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
new file mode 100755
index 0000000..1450f66
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -0,0 +1,170 @@
+/*
+ * 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 OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using OpenSim.Region.Physics.Manager;
+
+using Nini.Config;
+using log4net;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSTerrainHeightmap : BSTerrainPhys
+{
+    static string LogHeader = "[BULLETSIM TERRAIN HEIGHTMAP]";
+
+    BulletHeightMapInfo m_mapInfo = null;
+
+    // Constructor to build a default, flat heightmap terrain.
+    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize)
+        : base(physicsScene, regionBase, id)
+    {
+        Vector3 minTerrainCoords = new Vector3(0f, 0f, BSTerrainManager.HEIGHT_INITIALIZATION - BSTerrainManager.HEIGHT_EQUAL_FUDGE);
+        Vector3 maxTerrainCoords = new Vector3(regionSize.X, regionSize.Y, BSTerrainManager.HEIGHT_INITIALIZATION);
+        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
+        float[] initialMap = new float[totalHeights];
+        for (int ii = 0; ii < totalHeights; ii++)
+        {
+            initialMap[ii] = BSTerrainManager.HEIGHT_INITIALIZATION;
+        }
+        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo.minCoords = minTerrainCoords;
+        m_mapInfo.maxCoords = maxTerrainCoords;
+        m_mapInfo.terrainRegionBase = TerrainBase;
+        // Don't have to free any previous since we just got here.
+        BuildHeightmapTerrain();
+    }
+
+    // This minCoords and maxCoords passed in give the size of the terrain (min and max Z
+    //         are the high and low points of the heightmap).
+    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+                                                    Vector3 minCoords, Vector3 maxCoords)
+        : base(physicsScene, regionBase, id)
+    {
+        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo.minCoords = minCoords;
+        m_mapInfo.maxCoords = maxCoords;
+        m_mapInfo.minZ = minCoords.Z;
+        m_mapInfo.maxZ = maxCoords.Z;
+        m_mapInfo.terrainRegionBase = TerrainBase;
+
+        // Don't have to free any previous since we just got here.
+        BuildHeightmapTerrain();
+    }
+
+    public override void Dispose()
+    {
+        ReleaseHeightMapTerrain();
+    }
+
+    // Using the information in m_mapInfo, create the physical representation of the heightmap.
+    private void BuildHeightmapTerrain()
+    {
+        m_mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, m_mapInfo.ID,
+                                m_mapInfo.minCoords, m_mapInfo.maxCoords, 
+                                m_mapInfo.heightMap, PhysicsScene.Params.terrainCollisionMargin);
+
+        // Create the terrain shape from the mapInfo
+        m_mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(m_mapInfo.Ptr),
+                                                    BSPhysicsShapeType.SHAPE_TERRAIN);
+
+        // The terrain object initial position is at the center of the object
+        Vector3 centerPos;
+        centerPos.X = m_mapInfo.minCoords.X + (m_mapInfo.sizeX / 2f);
+        centerPos.Y = m_mapInfo.minCoords.Y + (m_mapInfo.sizeY / 2f);
+        centerPos.Z = m_mapInfo.minZ + ((m_mapInfo.maxZ - m_mapInfo.minZ) / 2f);
+
+        m_mapInfo.terrainBody = new BulletBody(m_mapInfo.ID,
+                BulletSimAPI.CreateBodyWithDefaultMotionState2(m_mapInfo.terrainShape.ptr,
+                                            m_mapInfo.ID, centerPos, Quaternion.Identity));
+
+        // Set current terrain attributes
+        BulletSimAPI.SetFriction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainFriction);
+        BulletSimAPI.SetHitFraction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+        BulletSimAPI.SetRestitution2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
+        BulletSimAPI.SetCollisionFlags2(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+        // Return the new terrain to the world of physical objects
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+
+        // redo its bounding box now that it is in the world
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+
+        BulletSimAPI.SetCollisionFilterMask2(m_mapInfo.terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainFilter,
+                            (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);
+
+        return;
+    }
+
+    // If there is information in m_mapInfo pointing to physical structures, release same.
+    private void ReleaseHeightMapTerrain()
+    {
+        if (m_mapInfo != null)
+        {
+            if (m_mapInfo.terrainBody.ptr != IntPtr.Zero)
+            {
+                BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+                // Frees both the body and the shape.
+                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+                BulletSimAPI.ReleaseHeightMapInfo2(m_mapInfo.Ptr);
+            }
+        }
+        m_mapInfo = null;
+    }
+
+    // The passed position is relative to the base of the region.
+    public override float GetHeightAtXYZ(Vector3 pos)
+    {
+        float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+
+        int mapIndex = (int)pos.Y * (int)m_mapInfo.sizeY + (int)pos.X;
+        try
+        {
+            ret = m_mapInfo.heightMap[mapIndex];
+        }
+        catch
+        {
+            // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+                                LogHeader, m_mapInfo.terrainRegionBase, pos);
+            ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+        }
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 7c34af2..cd623f1 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -40,6 +40,32 @@ using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
+
+// The physical implementation of the terrain is wrapped in this class.
+public abstract class BSTerrainPhys : IDisposable
+{
+    public enum TerrainImplementation
+    {
+        Heightmap   = 0,
+        Mesh        = 1
+    }
+
+    public BSScene PhysicsScene { get; private set; }
+    // Base of the region in world coordinates. Coordinates inside the region are relative to this.
+    public Vector3 TerrainBase { get; private set; }
+    public uint ID { get; private set; }
+
+    public BSTerrainPhys(BSScene physicsScene, Vector3 regionBase, uint id)
+    {
+        PhysicsScene = physicsScene;
+        TerrainBase = regionBase;
+        ID = id;
+    }
+    public abstract void Dispose();
+    public abstract float GetHeightAtXYZ(Vector3 pos);
+}
+
+// ==========================================================================================
 public sealed class BSTerrainManager
 {
     static string LogHeader = "[BULLETSIM TERRAIN MANAGER]";
@@ -54,8 +80,6 @@ public sealed class BSTerrainManager
     //    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);
 
@@ -67,11 +91,10 @@ public sealed class BSTerrainManager
 
     // If doing mega-regions, if we're region zero we will be managing multiple
     //    region terrains since region zero does the physics for the whole mega-region.
-    private Dictionary<Vector2, BulletHeightMapInfo> m_heightMaps;
+    private Dictionary<Vector3, BSTerrainPhys> m_terrains;
 
-    // True of the terrain has been modified.
-    // Used to force recalculation of terrain height after terrain has been modified
-    private bool m_terrainModified;
+    // Flags used to know when to recalculate the height.
+    private bool m_terrainModified = false;
 
     // If we are doing mega-regions, terrains are added from TERRAIN_ID to m_terrainCount.
     // This is incremented before assigning to new region so it is the last ID allocated.
@@ -89,8 +112,7 @@ public sealed class BSTerrainManager
     public BSTerrainManager(BSScene physicsScene)
     {
         PhysicsScene = physicsScene;
-        m_heightMaps = new Dictionary<Vector2,BulletHeightMapInfo>();
-        m_terrainModified = false;
+        m_terrains = new Dictionary<Vector3,BSTerrainPhys>();
 
         // Assume one region of default size
         m_worldOffset = Vector3.Zero;
@@ -99,17 +121,15 @@ public sealed class BSTerrainManager
     }
 
     // Create the initial instance of terrain and the underlying ground plane.
-    // The objects are allocated in the unmanaged space and the pointers are tracked
-    //    by the managed code.
-    // The terrains and the groundPlane are not added to the list of PhysObjects.
     // 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.
     public void CreateInitialGroundPlaneAndTerrain()
     {
         // 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),
-                    ShapeData.PhysicsShapeType.SHAPE_GROUNDPLANE);
+                    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,
                                                             Vector3.Zero, Quaternion.Identity));
@@ -121,15 +141,9 @@ public sealed class BSTerrainManager
         BulletSimAPI.SetCollisionFilterMask2(m_groundPlane.ptr,
                         (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
 
-        Vector3 minTerrainCoords = new Vector3(0f, 0f, HEIGHT_INITIALIZATION - HEIGHT_EQUAL_FUDGE);
-        Vector3 maxTerrainCoords = new Vector3(DefaultRegionSize.X, DefaultRegionSize.Y, HEIGHT_INITIALIZATION);
-        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
-        float[] initialMap = new float[totalHeights];
-        for (int ii = 0; ii < totalHeights; ii++)
-        {
-            initialMap[ii] = HEIGHT_INITIALIZATION;
-        }
-        UpdateOrCreateTerrain(BSScene.TERRAIN_ID, initialMap, minTerrainCoords, maxTerrainCoords, true);
+        // 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);
+        m_terrains.Add(Vector3.Zero, initialTerrain);
     }
 
     // Release all the terrain structures we might have allocated
@@ -150,21 +164,22 @@ public sealed class BSTerrainManager
     // Release all the terrain we have allocated
     public void ReleaseTerrain()
     {
-        foreach (KeyValuePair<Vector2, BulletHeightMapInfo> kvp in m_heightMaps)
+        lock (m_terrains)
         {
-            if (BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, kvp.Value.terrainBody.ptr))
+            foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
             {
-                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, kvp.Value.terrainBody.ptr);
-                BulletSimAPI.ReleaseHeightMapInfo2(kvp.Value.Ptr);
+                kvp.Value.Dispose();
             }
+            m_terrains.Clear();
         }
-        m_heightMaps.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)
             {
@@ -176,8 +191,9 @@ public sealed class BSTerrainManager
                 {
                     DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}",
                                     BSScene.DetailLogZero, m_worldOffset, m_worldMax);
-                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateOrCreateTerrain(BSScene.CHILDTERRAIN_ID,
-                                    localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateTerrain(
+                                    BSScene.CHILDTERRAIN_ID, localHeightMap, 
+                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true);
                 }
             }
             else
@@ -185,7 +201,7 @@ public sealed class BSTerrainManager
                 // If not doing the mega-prim thing, just change the terrain
                 DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
 
-                UpdateOrCreateTerrain(BSScene.TERRAIN_ID, localHeightMap,
+                UpdateTerrain(BSScene.TERRAIN_ID, localHeightMap,
                                     m_worldOffset, m_worldOffset + DefaultRegionSize, true);
             }
         });
@@ -195,56 +211,64 @@ public sealed class BSTerrainManager
     //     based on the passed information. The 'id' should be either the terrain id or
     //     BSScene.CHILDTERRAIN_ID. If the latter, a new child terrain ID will be allocated and used.
     //     The latter feature is for creating child terrains for mega-regions.
-    // If called with a mapInfo in m_heightMaps but the terrain has no body yet (mapInfo.terrainBody.Ptr == 0)
-    //     then a new body and shape is created and the mapInfo is filled.
-    //     This call is used for doing the initial terrain creation.
     // If called with a mapInfo in m_heightMaps and there is an existing terrain body, a new
     //     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.)
-    private void UpdateOrCreateTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
+    // Called during taint-time.
+    private void UpdateTerrain(uint id, float[] heightMap, 
+                            Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
     {
-        DetailLog("{0},BSTerrainManager.UpdateOrCreateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
+        DetailLog("{0},BSTerrainManager.UpdateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
                             BSScene.DetailLogZero, minCoords, maxCoords, inTaintTime);
 
+        // 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 replace passed min and max Z with the actual terrain min/max Z.
         float minZ = float.MaxValue;
         float maxZ = float.MinValue;
-        Vector2 terrainRegionBase = new Vector2(minCoords.X, minCoords.Y);
-
-        int heightMapSize = heightMap.Length;
-        for (int ii = 0; ii < heightMapSize; ii++)
+        foreach (float height in heightMap)
         {
-            float height = heightMap[ii];
             if (height < minZ) minZ = height;
             if (height > maxZ) maxZ = height;
         }
-
-        // The shape of the terrain is from its base to its extents.
+        if (minZ == maxZ)
+        {
+            // If min and max are the same, reduce min a little bit so a good bounding box is created.
+            minZ -= BSTerrainManager.HEIGHT_EQUAL_FUDGE;
+        }
         minCoords.Z = minZ;
         maxCoords.Z = maxZ;
 
-        BulletHeightMapInfo mapInfo;
-        if (m_heightMaps.TryGetValue(terrainRegionBase, out mapInfo))
+        Vector3 terrainRegionBase = new Vector3(minCoords.X, minCoords.Y, 0f);
+
+        lock (m_terrains)
         {
-            // If this is terrain we know about, it's easy to update
-
-            mapInfo.heightMap = heightMap;
-            mapInfo.minCoords = minCoords;
-            mapInfo.maxCoords = maxCoords;
-            mapInfo.minZ = minZ;
-            mapInfo.maxZ = maxZ;
-            mapInfo.sizeX = maxCoords.X - minCoords.X;
-            mapInfo.sizeY = maxCoords.Y - minCoords.Y;
-            DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,call,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}",
-                        BSScene.DetailLogZero, terrainRegionBase, mapInfo.minCoords, mapInfo.maxCoords, mapInfo.sizeX, mapInfo.sizeY);
-
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateOrCreateTerrain:UpdateExisting", delegate()
+            BSTerrainPhys terrainPhys;
+            if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
             {
-                if (MegaRegionParentPhysicsScene != null)
+                // 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.
+                terrainPhys.Dispose();
+
+                if (MegaRegionParentPhysicsScene == null)
+                {
+                    BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
+                    m_terrains.Add(terrainRegionBase, newTerrainPhys);
+
+                    m_terrainModified = true;
+                }
+                else
                 {
                     // It's possible that Combine() was called after this code was queued.
                     // If we are a child of combined regions, we don't create any terrain for us.
-                    DetailLog("{0},UpdateOrCreateTerrain:AmACombineChild,taint", BSScene.DetailLogZero);
+                    DetailLog("{0},BSTerrainManager.UpdateTerrain:AmACombineChild,taint", BSScene.DetailLogZero);
 
                     // Get rid of any terrain that may have been allocated for us.
                     ReleaseGroundPlaneAndTerrain();
@@ -252,135 +276,56 @@ public sealed class BSTerrainManager
                     // I hate doing this, but just bail
                     return;
                 }
+            }
+            else
+            {
+                // 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
 
-                if (mapInfo.terrainBody.ptr != IntPtr.Zero)
-                {
-                    // Updating an existing terrain.
-                    DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,taint,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}",
-                                    BSScene.DetailLogZero, terrainRegionBase, mapInfo.minCoords, mapInfo.maxCoords, mapInfo.sizeX, mapInfo.sizeY);
-
-                    // Remove from the dynamics world because we're going to mangle this object
-                    BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                    // Get rid of the old terrain
-                    BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-                    BulletSimAPI.ReleaseHeightMapInfo2(mapInfo.Ptr);
-                    mapInfo.Ptr = IntPtr.Zero;
-
-                    /*
-                    // NOTE: This routine is half here because I can't get the terrain shape replacement
-                    //   to work. In the short term, the above three lines completely delete the old
-                    //   terrain and the code below recreates one from scratch.
-                    // Hopefully the Bullet community will help me out on this one.
-
-                    // First, release the old collision shape (there is only one terrain)
-                    BulletSimAPI.DeleteCollisionShape2(m_physicsScene.World.Ptr, mapInfo.terrainShape.Ptr);
-
-                    // Fill the existing height map info with the new location and size information
-                    BulletSimAPI.FillHeightMapInfo2(m_physicsScene.World.Ptr, mapInfo.Ptr, mapInfo.ID,
-                                    mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
-
-                    // Create a terrain shape based on the new info
-                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr));
-
-                    // Stuff the shape into the existing terrain body
-                    BulletSimAPI.SetBodyShape2(m_physicsScene.World.Ptr, mapInfo.terrainBody.Ptr, mapInfo.terrainShape.Ptr);
-                    */
-                }
-                // else
-                {
-                    // Creating a new terrain.
-                    DetailLog("{0},UpdateOrCreateTerrain:CreateNewTerrain,taint,baseX={1},baseY={2},minZ={3},maxZ={4}",
-                                    BSScene.DetailLogZero, mapInfo.minCoords.X, mapInfo.minCoords.Y, minZ, maxZ);
-
-                    mapInfo.ID = id;
-                    mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, mapInfo.ID,
-                        mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
-
-                    // Create the terrain shape from the mapInfo
-                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr),
-                                ShapeData.PhysicsShapeType.SHAPE_TERRAIN);
-
-                    // The terrain object initial position is at the center of the object
-                    Vector3 centerPos;
-                    centerPos.X = minCoords.X + (mapInfo.sizeX / 2f);
-                    centerPos.Y = minCoords.Y + (mapInfo.sizeY / 2f);
-                    centerPos.Z = minZ + ((maxZ - minZ) / 2f);
-
-                    mapInfo.terrainBody = new BulletBody(mapInfo.ID,
-                            BulletSimAPI.CreateBodyWithDefaultMotionState2(mapInfo.terrainShape.ptr,
-                                                        id, centerPos, Quaternion.Identity));
-                }
-
-                // Make sure the entry is in the heightmap table
-                m_heightMaps[terrainRegionBase] = mapInfo;
-
-                // Set current terrain attributes
-                BulletSimAPI.SetFriction2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainFriction);
-                BulletSimAPI.SetHitFraction2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-                BulletSimAPI.SetRestitution2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
-                BulletSimAPI.SetCollisionFlags2(mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
-
-                // Return the new terrain to the world of physical objects
-                BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                // redo its bounding box now that it is in the world
-                BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                BulletSimAPI.SetCollisionFilterMask2(mapInfo.terrainBody.ptr,
-                                    (uint)CollisionFilterGroups.TerrainFilter,
-                                    (uint)CollisionFilterGroups.TerrainMask);
-
-                // Make sure the new shape is processed.
-                // BulletSimAPI.Activate2(mapInfo.terrainBody.ptr, true);
-                // BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.ISLAND_SLEEPING);
-                BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
-
-                m_terrainModified = true;
-            });
-        }
-        else
-        {
-            // 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
-
-            // 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},UpdateOrCreateTerrain:NewTerrain,call,id={1}, minC={2}, maxC={3}",
-                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
+                // if this is a child terrain, calculate a unique terrain id
+                uint newTerrainID = id;
+                if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
+                    newTerrainID = ++m_terrainCount;
 
-            // Code that must happen at taint-time
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateOrCreateTerrain:NewTerrain", delegate()
-            {
-                DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoords.X, minCoords.Y);
-                // Create a new mapInfo that will be filled with the new info
-                mapInfo = new BulletHeightMapInfo(id, heightMapX,
-                        BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, newTerrainID,
-                                    minCoordsX, maxCoordsX, heightMapX, TERRAIN_COLLISION_MARGIN));
-                // Put the unfilled heightmap info into the collection of same
-                m_heightMaps.Add(terrainRegionBase, mapInfo);
-                // Build the terrain
-                UpdateOrCreateTerrain(newTerrainID, heightMap, minCoords, maxCoords, true);
+                DetailLog("{0},UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
+                                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
+                BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
+                m_terrains.Add(terrainRegionBase, newTerrainPhys);
 
                 m_terrainModified = true;
-            });
+            }
         }
     }
 
-    // Someday we will have complex terrain with caves and tunnels
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    // TODO: redo terrain implementation selection to allow other base types than heightMap.
+    private BSTerrainPhys BuildPhysicalTerrain(Vector3 terrainRegionBase, uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
     {
-        // For the moment, it's flat and convex
-        return GetTerrainHeightAtXY(loc.X, loc.Y);
+        PhysicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}", 
+                                            LogHeader, PhysicsScene.RegionName, terrainRegionBase, 
+                                            (BSTerrainPhys.TerrainImplementation)PhysicsScene.Params.terrainImplementation);
+        BSTerrainPhys newTerrainPhys = null;
+        switch ((int)PhysicsScene.Params.terrainImplementation)
+        {
+            case (int)BSTerrainPhys.TerrainImplementation.Heightmap:
+                newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, id,
+                                            heightMap, minCoords, maxCoords);
+                break;
+            case (int)BSTerrainPhys.TerrainImplementation.Mesh:
+                newTerrainPhys = new BSTerrainMesh(PhysicsScene, terrainRegionBase, id,
+                                            heightMap, minCoords, maxCoords);
+                break;
+            default:
+                PhysicsScene.Logger.ErrorFormat("{0} Bad terrain implementation specified. Type={1}/{2},Region={3}/{4}",
+                                            LogHeader, 
+                                            (int)PhysicsScene.Params.terrainImplementation, 
+                                            PhysicsScene.Params.terrainImplementation,
+                                            PhysicsScene.RegionName, terrainRegionBase);
+                break;
+        }
+        return newTerrainPhys;
     }
 
+
     // Given an X and Y, find the height of the terrain.
     // Since we could be handling multiple terrains for a mega-region,
     //    the base of the region is calcuated assuming all regions are
@@ -390,12 +335,15 @@ public sealed class BSTerrainManager
     private float lastHeightTX = 999999f;
     private float lastHeightTY = 999999f;
     private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
-    private float GetTerrainHeightAtXY(float tX, float tY)
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
     {
+        float tX = loc.X;
+        float tY = loc.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)
             return lastHeight;
+        m_terrainModified = false;
 
         lastHeightTX = tX;
         lastHeightTY = tY;
@@ -403,34 +351,21 @@ public sealed class BSTerrainManager
 
         int offsetX = ((int)(tX / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
         int offsetY = ((int)(tY / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector2 terrainBaseXY = new Vector2(offsetX, offsetY);
+        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
 
-        BulletHeightMapInfo mapInfo;
-        if (m_heightMaps.TryGetValue(terrainBaseXY, out mapInfo))
+        lock (m_terrains)
         {
-            float regionX = tX - offsetX;
-            float regionY = tY - offsetY;
-            int mapIndex = (int)regionY * (int)mapInfo.sizeY + (int)regionX;
-            try
+            BSTerrainPhys physTerrain;
+            if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
             {
-                ret = mapInfo.heightMap[mapIndex];
+                ret = physTerrain.GetHeightAtXYZ(loc - terrainBaseXYZ);
             }
-            catch
+            else
             {
-                // Sometimes they give us wonky values of X and Y. Give a warning and return something.
-                PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, x={2}, y={3}",
-                                    LogHeader, terrainBaseXY, regionX, regionY);
-                ret = HEIGHT_GETHEIGHT_RET;
+                PhysicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
+                        LogHeader, PhysicsScene.RegionName, tX, tY);
             }
-            // DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXY,bX={1},baseY={2},szX={3},szY={4},regX={5},regY={6},index={7},ht={8}",
-            //         BSScene.DetailLogZero, offsetX, offsetY, mapInfo.sizeX, mapInfo.sizeY, regionX, regionY, mapIndex, ret);
-        }
-        else
-        {
-            PhysicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
-                    LogHeader, PhysicsScene.RegionName, tX, tY);
         }
-        m_terrainModified = false;
         lastHeight = ret;
         return ret;
     }
@@ -466,7 +401,7 @@ public sealed class BSTerrainManager
     // Unhook all the combining that I know about.
     public void UnCombine(PhysicsScene pScene)
     {
-        // Just like ODE, for the moment a NOP
+        // Just like ODE, we don't do anything yet.
         DetailLog("{0},BSTerrainManager.UnCombine", BSScene.DetailLogZero);
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
new file mode 100755
index 0000000..5f6675d
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -0,0 +1,262 @@
+/*
+ * 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 OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using OpenSim.Region.Physics.Manager;
+
+using Nini.Config;
+using log4net;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSTerrainMesh : BSTerrainPhys
+{
+    static string LogHeader = "[BULLETSIM TERRAIN MESH]";
+
+    private float[] m_savedHeightMap;
+    int m_sizeX;
+    int m_sizeY;
+
+    BulletShape m_terrainShape;
+    BulletBody m_terrainBody;
+
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize) 
+        : base(physicsScene, regionBase, id)
+    {
+    }
+
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id /* parameters for making mesh */)
+        : base(physicsScene, regionBase, id)
+    {
+    }
+
+    // Create terrain mesh from a heightmap.
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+                                                    Vector3 minCoords, Vector3 maxCoords)
+        : base(physicsScene, regionBase, id)
+    {
+        int indicesCount;
+        int[] indices;
+        int verticesCount;
+        float[] vertices;
+
+        m_savedHeightMap = initialMap;
+
+        m_sizeX = (int)(maxCoords.X - minCoords.X);
+        m_sizeY = (int)(maxCoords.Y - minCoords.Y);
+
+        if (!BSTerrainMesh.ConvertHeightmapToMesh(PhysicsScene, initialMap,
+                            m_sizeX, m_sizeY,
+                            (float)m_sizeX, (float)m_sizeY,
+                            Vector3.Zero, 1.0f,
+                            out indicesCount, out indices, out verticesCount, out vertices))
+        {
+            // DISASTER!!
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap", ID);
+            PhysicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh! base={1}", LogHeader, TerrainBase);
+            // 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),
+                                        BSPhysicsShapeType.SHAPE_MESH);
+        if (m_terrainShape.ptr == IntPtr.Zero)
+        {
+            // DISASTER!!
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape", ID);
+            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain mesh! base={1}", LogHeader, TerrainBase);
+            // Something is very messed up and a crash is in our future.
+            return;
+        }
+
+        Vector3 pos = regionBase;
+        Quaternion rot = Quaternion.Identity;
+
+        m_terrainBody = new BulletBody(id, BulletSimAPI.CreateBodyWithDefaultMotionState2( m_terrainShape.ptr, ID, pos, rot));
+        if (m_terrainBody.ptr == IntPtr.Zero)
+        {
+            // DISASTER!!
+            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase);
+            // Something is very messed up and a crash is in our future.
+            return;
+        }
+
+        // Set current terrain attributes
+        BulletSimAPI.SetFriction2(m_terrainBody.ptr, PhysicsScene.Params.terrainFriction);
+        BulletSimAPI.SetHitFraction2(m_terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+        BulletSimAPI.SetRestitution2(m_terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
+        BulletSimAPI.SetCollisionFlags2(m_terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+        // Static objects are not very massive.
+        BulletSimAPI.SetMassProps2(m_terrainBody.ptr, 0f, Vector3.Zero);
+
+        // 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
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+
+        BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainFilter,
+                            (uint)CollisionFilterGroups.TerrainMask);
+
+        // Make it so the terrain will not move or be considered for movement.
+        BulletSimAPI.ForceActivationState2(m_terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
+    }
+
+    public override void Dispose()
+    {
+        if (m_terrainBody.ptr != IntPtr.Zero)
+        {
+            BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+            // Frees both the body and the shape.
+            BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+        }
+    }
+
+    public override float GetHeightAtXYZ(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.
+        float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+
+        int mapIndex = (int)pos.Y * m_sizeY + (int)pos.X;
+        try
+        {
+            ret = m_savedHeightMap[mapIndex];
+        }
+        catch
+        {
+            // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+                                                LogHeader, TerrainBase, pos);
+            ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+        }
+        return ret;
+    }
+
+    // Convert the passed heightmap to mesh information suitable for CreateMeshShape2().
+    // Return 'true' if successfully created.
+    public static bool ConvertHeightmapToMesh(
+                                BSScene physicsScene,
+                                float[] heightMap, int sizeX, int sizeY,    // parameters of incoming heightmap
+                                float extentX, float extentY,               // zero based range for output vertices
+                                Vector3 extentBase,                         // base to be added to all vertices
+                                float magnification,                        // number of vertices to create between heightMap coords
+                                out int indicesCountO, out int[] indicesO,
+                                out int verticesCountO, out float[] verticesO)
+    {
+        bool ret = false;
+
+        int indicesCount = 0;
+        int verticesCount = 0;
+        int[] indices = new int[0];
+        float[] vertices = new float[0];
+
+        // 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.
+            int totalVertices = (sizeX + 1) * (sizeY + 1);
+            vertices = new float[totalVertices * 3];
+            int totalIndices = sizeX * sizeY * 6;
+            indices = new int[totalIndices];
+
+            float magX = (float)sizeX / extentX;
+            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 go around sizeX + 1 times
+                {
+                    int offset = yy * sizeX + xx;
+                    // 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;
+                    verticesCount += 3;
+                }
+            }
+            verticesCount = verticesCount / 3;
+
+            for (int yy = 0; yy < sizeY; yy++)
+            {
+                for (int xx = 0; xx < 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;
+                    indices[indicesCount + 2] = offset + sizeX + 1; // accounting for the extra column
+                    indices[indicesCount + 3] = offset + 1;
+                    indices[indicesCount + 4] = offset + sizeX + 2;
+                    indices[indicesCount + 5] = offset + sizeX + 1;
+                    indicesCount += 6;
+                }
+            }
+
+            ret = true;
+        }
+        catch (Exception e)
+        {
+            physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
+                                                LogHeader, physicsScene.RegionName, extentBase, e);
+        }
+
+        indicesCountO = indicesCount;
+        indicesO = indices;
+        verticesCountO = verticesCount;
+        verticesO = vertices;
+
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 07149d8..1e003e6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -88,11 +88,11 @@ public struct BulletShape
     public BulletShape(IntPtr xx)
     {
         ptr = xx;
-        type=ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
-        shapeKey = 0;
+        type=BSPhysicsShapeType.SHAPE_UNKNOWN;
+        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
         isNativeShape = false;
     }
-    public BulletShape(IntPtr xx, ShapeData.PhysicsShapeType typ)
+    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
     {
         ptr = xx;
         type = typ;
@@ -100,7 +100,7 @@ public struct BulletShape
         isNativeShape = false;
     }
     public IntPtr ptr;
-    public ShapeData.PhysicsShapeType type;
+    public BSPhysicsShapeType type;
     public System.UInt64 shapeKey;
     public bool isNativeShape;
     public override string ToString()
@@ -152,7 +152,7 @@ public class BulletHeightMapInfo
         ID = id;
         Ptr = xx;
         heightMap = hm;
-        terrainRegionBase = new Vector2(0f, 0f);
+        terrainRegionBase = Vector3.Zero;
         minCoords = new Vector3(100f, 100f, 25f);
         maxCoords = new Vector3(101f, 101f, 26f);
         minZ = maxZ = 0f;
@@ -161,7 +161,7 @@ public class BulletHeightMapInfo
     public uint ID;
     public IntPtr Ptr;
     public float[] heightMap;
-    public Vector2 terrainRegionBase;
+    public Vector3 terrainRegionBase;
     public Vector3 minCoords;
     public Vector3 maxCoords;
     public float sizeX, sizeY;
@@ -178,26 +178,39 @@ public struct ConvexHull
         int VertexCount;
         Vector3[] Vertices;
 }
+public enum BSPhysicsShapeType
+{
+        SHAPE_UNKNOWN   = 0,
+        SHAPE_CAPSULE   = 1,
+        SHAPE_BOX       = 2,
+        SHAPE_CONE      = 3,
+        SHAPE_CYLINDER  = 4,
+        SHAPE_SPHERE    = 5,
+        SHAPE_MESH      = 6,
+        SHAPE_HULL      = 7,
+    // following defined by BulletSim
+        SHAPE_GROUNDPLANE  = 20,
+        SHAPE_TERRAIN   = 21,
+        SHAPE_COMPOUND  = 22,
+        SHAPE_HEIGHTMAP = 23,
+};
+
+// The native shapes have predefined shape hash keys
+public enum FixedShapeKey : ulong
+{
+    KEY_NONE        = 0,
+    KEY_BOX         = 1,
+    KEY_SPHERE      = 2,
+    KEY_CONE        = 3,
+    KEY_CYLINDER    = 4,
+    KEY_CAPSULE     = 5,
+}
+
 [StructLayout(LayoutKind.Sequential)]
 public struct ShapeData
 {
-    public enum PhysicsShapeType
-    {
-                SHAPE_UNKNOWN   = 0,
-                SHAPE_AVATAR    = 1,
-                SHAPE_BOX       = 2,
-                SHAPE_CONE      = 3,
-                SHAPE_CYLINDER  = 4,
-                SHAPE_SPHERE    = 5,
-                SHAPE_MESH      = 6,
-                SHAPE_HULL      = 7,
-        // following defined by BulletSim
-                SHAPE_GROUNDPLANE  = 20,
-                SHAPE_TERRAIN   = 21,
-                SHAPE_COMPOUND  = 22,
-    };
     public uint ID;
-    public PhysicsShapeType Type;
+    public BSPhysicsShapeType Type;
     public Vector3 Position;
     public Quaternion Rotation;
     public Vector3 Velocity;
@@ -216,16 +229,6 @@ public struct ShapeData
     // note that bools are passed as floats since bool size changes by language and architecture
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
-
-    // The native shapes have predefined shape hash keys
-    public enum FixedShapeKey : ulong
-    {
-        KEY_BOX         = 1,
-        KEY_SPHERE      = 2,
-        KEY_CONE        = 3,
-        KEY_CYLINDER    = 4,
-        KEY_CAPSULE     = 5,
-    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct SweepHit
@@ -280,17 +283,23 @@ public struct ConfigurationParameters
         public float ccdSweptSphereRadius;
     public float contactProcessingThreshold;
 
+    public float terrainImplementation;
     public float terrainFriction;
     public float terrainHitFraction;
     public float terrainRestitution;
+    public float terrainCollisionMargin;
+
     public float avatarFriction;
     public float avatarStandingFriction;
     public float avatarDensity;
     public float avatarRestitution;
-    public float avatarCapsuleRadius;
+    public float avatarCapsuleWidth;
+    public float avatarCapsuleDepth;
     public float avatarCapsuleHeight;
         public float avatarContactProcessingThreshold;
 
+    public float vehicleAngularDamping;
+
         public float maxPersistantManifoldPoolSize;
         public float maxCollisionAlgorithmPoolSize;
         public float shouldDisableContactPoolDynamicAllocation;
@@ -348,7 +357,7 @@ 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_NONE                          = 0,
@@ -388,13 +397,13 @@ public enum CollisionFilterGroups : uint
     ObjectFilter            = BSolidFilter,
     ObjectMask              = BAllFilter,
     StaticObjectFilter      = BStaticFilter,
-    StaticObjectMask        = BAllFilter,
+    StaticObjectMask        = BAllFilter & ~BStaticFilter,   // static objects don't collide with each other
     LinksetFilter           = BLinksetFilter,
-    LinksetMask             = BAllFilter & ~BLinksetFilter,
+    LinksetMask             = BAllFilter & ~BLinksetFilter, // linkset objects don't collide with each other
     VolumeDetectFilter      = BSensorTrigger,
     VolumeDetectMask        = ~BSensorTrigger,
     TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter & ~BStaticFilter,
+    TerrainMask             = BAllFilter & ~BStaticFilter,  // static objects on the ground don't collide
     GroundPlaneFilter       = BGroundPlaneFilter,
     GroundPlaneMask         = BAllFilter
 
@@ -429,140 +438,6 @@ static class BulletSimAPI {
 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-[return: MarshalAs(UnmanagedType.LPStr)]
-public static extern string GetVersion();
-
-/* Remove the linkage to the old api methods
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern uint Initialize(Vector3 maxPosition, IntPtr parms,
-                        int maxCollisions, IntPtr collisionArray,
-                        int maxUpdates, IntPtr updateArray,
-                        DebugLogCallback logRoutine);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void CreateInitialGroundPlaneAndTerrain(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightmap(uint worldID, [MarshalAs(UnmanagedType.LPArray)] float[] heightMap);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void Shutdown(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateParameter(uint worldID, uint localID,
-                        [MarshalAs(UnmanagedType.LPStr)]string paramCode, float value);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int PhysicsStep(uint worldID, float timeStep, int maxSubSteps, float fixedTimeStep,
-                        out int updatedEntityCount,
-                        out IntPtr updatedEntitiesPtr,
-                        out int collidersCount,
-                        out IntPtr collidersPtr);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateHull(uint worldID, System.UInt64 meshKey,
-                            int hullCount, [MarshalAs(UnmanagedType.LPArray)] float[] hulls
-    );
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateMesh(uint worldID, System.UInt64 meshKey,
-                            int indexCount, [MarshalAs(UnmanagedType.LPArray)] int[] indices,
-                            int verticesCount, [MarshalAs(UnmanagedType.LPArray)] float[] vertices
-    );
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyHull(uint worldID, System.UInt64 meshKey);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyMesh(uint worldID, System.UInt64 meshKey);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateObject(uint worldID, ShapeData shapeData);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Quaternion GetObjectOrientation(uint WorldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectVelocity(uint worldID, uint id, Vector3 velocity);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectAngularVelocity(uint worldID, uint id, Vector3 angularVelocity);
-
-// Set the current force acting on the object
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectForce(uint worldID, uint id, Vector3 force);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectScaleMass(uint worldID, uint id, Vector3 scale, float mass, bool isDynamic);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectCollidable(uint worldID, uint id, bool phantom);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectDynamic(uint worldID, uint id, bool isDynamic, float mass);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectGhost(uint worldID, uint id, bool ghostly);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectProperties(uint worldID, uint id, bool isStatic, bool isSolid, bool genCollisions, float mass);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectBuoyancy(uint worldID, uint id, float buoyancy);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool HasObject(uint worldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyObject(uint worldID, uint id);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern SweepHit ConvexSweepTest(uint worldID, uint id, Vector3 to, float extraMargin);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern RaycastHit RayTest(uint worldID, uint id, Vector3 from, Vector3 to);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 RecoverFromPenetration(uint worldID, uint id);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpBulletStatistics();
-*/
-// Log a debug message
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetDebugLogCallback(DebugLogCallback callback);
-
-// ===============================================================================
-// ===============================================================================
-// ===============================================================================
-// A new version of the API that enables moving all the logic out of the C++ code and into
-//    the C# code. This will make modifications easier for the next person.
-// This interface passes the actual pointers to the objects in the unmanaged
-//    address space. All the management (calls for creation/destruction/lookup)
-//    is done in the C# code.
-// The names have a "2" tacked on. This will be removed as the C# code gets rebuilt
-//    and the old code is removed.
-
-// Functions use while converting from API1 to API2. Can be removed when totally converted.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetSimHandle2(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandle2(IntPtr world, uint id);
-
 // ===============================================================================
 // Initialization and simulation
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -611,6 +486,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]
diff --git a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
index cbc6b95..16404c6 100644
--- a/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
+++ b/OpenSim/Region/Physics/OdePlugin/Tests/ODETestClass.cs
@@ -32,13 +32,14 @@ using OpenMetaverse;
 using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 using OpenSim.Region.Physics.OdePlugin;
+using OpenSim.Tests.Common;
 using log4net;
 using System.Reflection;
 
 namespace OpenSim.Region.Physics.OdePlugin.Tests
 {
     [TestFixture]
-    public class ODETestClass
+    public class ODETestClass : OpenSimTestCase
     {
         private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);