15 files changed, 1668 insertions, 1120 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 _size;
-            return new OMV.Vector3(Scale.X * 2f, Scale.Y * 2f, Scale.Z);
        }
        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}",
+            DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
-                            LocalID, Scale, _avatarDensity, _avatarVolume, RawMass);
+                            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;
+        OMV.Vector3 newScale = size;
-        newScale.X = PhysicsScene.Params.avatarCapsuleRadius;
+        // newScale.X = PhysicsScene.Params.avatarCapsuleWidth;
-        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius;
+        // 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);
+        // The 1.15f came from ODE. Not sure what this factors in.
-        Scale = newScale;
+        // 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
-                    m_flags &=
+                                | VehicleFlag.HOVER_TERRAIN_ONLY
-                         ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                                | VehicleFlag.HOVER_GLOBAL_HEIGHT
-                           VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
+                                | VehicleFlag.HOVER_UP_ONLY);
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                            | VehicleFlag.LIMIT_ROLL_ONLY
+                            | VehicleFlag.LIMIT_MOTOR_UP);
                    break;
                case Vehicle.TYPE_CAR:
                    m_linearMotorDirection = Vector3.Zero;
@@ -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
+                m_vehicleMass = Prim.Linkset.LinksetMass;
-                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
-                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
-                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
-                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
+            Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
-            // m_lastLinearVelocityVector is the current speed we are moving in that direction
-            if (m_linearMotorDirection.LengthSquared() > 0.001f)
-            {
-                Vector3 origDir = m_linearMotorDirection;       // DEBUG
-                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
-                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
-                Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
-                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
+            // Rotate new object velocity from vehicle relative to world coordinates
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
+            linearMotorContribution *= Prim.ForceOrientation;
-                m_lastLinearVelocityVector += addAmount;
-                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
+            Vector3 pos = Prim.ForcePosition;
-                m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
+            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-                // Apply friction for next time
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
-                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
-                                Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
-                                m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
+            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
-            }
-            else
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
-            {
-                // if what remains of direction is very small, zero it.
+            // ==================================================================
-                m_linearMotorDirection = Vector3.Zero;
+            Vector3 newVelocity = linearMotorContribution
-                m_lastLinearVelocityVector = Vector3.Zero;
+                            + terrainHeightContribution
-                m_newVelocity = Vector3.Zero;
+                            + 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
+            // Other linear forces are applied as forces.
-            // There is some gravity, make a gravity force vector that is applied after object velocity.
+            Vector3 totalDownForce = grav * m_vehicleMass;
-            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
+            if (totalDownForce != Vector3.Zero)
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
+            {
+                Prim.AddForce(totalDownForce, false);
+            }
-            /*
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-             * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
+                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
-            // Preserve the current Z velocity
+                                newVelocity, Prim.Velocity, totalDownForce);
-            Vector3 vel_now = m_prim.Velocity;
-            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
-             */
-            Vector3 pos = Prim.ForcePosition;
+        } // end MoveLinear()
-//            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
+        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, 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;
+                        ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
-                    }
-                    else
-                    {
-                        m_newVelocity.Z = 0f;
                    }
                }
-                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
+        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
-            Vector3 downForce = Vector3.Zero;
+        {
+            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}", 
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
-                                    Prim.LocalID, distanceAboveGround, downForce);
+                                    Prim.LocalID, distanceAboveGround, ret);
            }
-            #endregion // downForce
+            return ret;
+        }
-            // 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()
        // =======================================================================
+        // =======================================================================
        // 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 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
-            Vector3 deflection = Vector3.Zero;
-            Vector3 banking = Vector3.Zero;
-            // If vertical attaction timescale is reasonable and we applied an angular force last time...
+            Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
-            if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
+            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;
+                // Apply to the body.
-                if (Prim.IsColliding)
+                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
-                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
+                // 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
+                verticalError.Normalize();
-                //      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
+                // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
-                //      and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
+                //    is now leaning to one side (rotated around the X axis) and the Y value will
-                //      modulated to prevent a stable inverted body.
+                //    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
-                // Error is 0 (no error) to +/- 2 (max error)
+                //    zero and one.
-                if (verticalError.Z < 0.0f)
+                // 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.X = 2f - verticalError.X;
-                    verticalError.Y = 2.0f - verticalError.Y;
+                    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
+                // Y error means needed rotation around X axis and visa versa.
-                //     then .X increases, so change  Body angular velocity  X based on Y, and Y based on X.
+                verticalAttractionContribution.X =    verticalError.Y;
-                //     Z is not changed.
+                verticalAttractionContribution.Y =  - verticalError.X;
-                vertattr.X =    verticalError.Y;
+                verticalAttractionContribution.Z = 0f;
-                vertattr.Y =  - verticalError.X;
-                vertattr.Z = 0f;
+                // scale by the time scale and timestep
+                Vector3 unscaledContrib = verticalAttractionContribution;
+                verticalAttractionContribution /= m_verticalAttractionTimescale;
+                verticalAttractionContribution *= pTimestep;
-                // scaling appears better usingsquare-law
+                // apply efficiency
-                Vector3 angularVelocity = Prim.ForceRotationalVelocity;
+                Vector3 preEfficiencyContrib = verticalAttractionContribution;
-                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+                float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
-                vertattr.X += bounce * angularVelocity.X;
+                verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
-                vertattr.Y += bounce * angularVelocity.Y;
-                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
+                VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
-                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
+                                            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
+        public Vector3 ComputeAngularBanking(float pTimestep)
+        {
-            #region Banking
+            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);
+                float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
-                    //Changes which way it banks in and out of turns
+                //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}", 
+                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}",
-                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
+                                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;
-using System.Collections.Generic;
+using System.Collections.Generic;
-using System.Text;
+using System.Text;
-using OpenMetaverse;
+using OpenMetaverse;
-namespace OpenSim.Region.Physics.BulletSPlugin
+namespace OpenSim.Region.Physics.BulletSPlugin
-{
+{
-public abstract class BSMotor
+public abstract class BSMotor
-{
+{
-    public virtual void Reset() { }
+    // Timescales and other things can be turned off by setting them to 'infinite'.
-    public virtual void Zero() { }
+    public const float Infinite = 10000f;
-}
+    public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
-// Can all the incremental stepping be replaced with motor classes?
-public class BSVMotor : BSMotor
+    public BSMotor(string useName)
-{
+    {
-    public Vector3 FrameOfReference { get; set; }
+        UseName = useName;
-    public Vector3 Offset { get; set; }
+        PhysicsScene = null;
+    }
-    public float TimeScale { get; set; }
+    public virtual void Reset() { }
-    public float TargetValueDecayTimeScale { get; set; }
+    public virtual void Zero() { }
-    public Vector3 CurrentValueReductionTimescale { get; set; }
-    public float Efficiency { get; set; }
+    public string UseName { get; private set; }
+    // Used only for outputting debug information. Might not be set so check for null.
-    public Vector3 TargetValue { get; private set; }
+    public BSScene PhysicsScene { get; set; }
-    public Vector3 CurrentValue { get; private set; }
+    protected void MDetailLog(string msg, params Object[] parms)
+    {
+        if (PhysicsScene != null)
+        {
-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
+            if (PhysicsScene.VehicleLoggingEnabled)
-    {
+            {
-        TimeScale = timeScale;
+                PhysicsScene.DetailLog(msg, parms);
-        TargetValueDecayTimeScale = decayTimeScale;
+            }
-        CurrentValueReductionTimescale = frictionTimeScale;
+        }
-        Efficiency = efficiency;
+    }
-    }
+}
-    public void SetCurrent(Vector3 current)
+// Can all the incremental stepping be replaced with motor classes?
-    {
+public class BSVMotor : BSMotor
-        CurrentValue = current;
+{
-    }
+    public Vector3 FrameOfReference { get; set; }
-    public void SetTarget(Vector3 target)
+    public Vector3 Offset { get; set; }
-    {
-        TargetValue = target;
+    public float TimeScale { get; set; }
-    }
+    public float TargetValueDecayTimeScale { get; set; }
-    public Vector3 Step(float timeStep)
+    public Vector3 FrictionTimescale { get; set; }
-    {
+    public float Efficiency { get; set; }
-        if (CurrentValue.LengthSquared() > 0.001f)
-        {
+    public Vector3 TargetValue { get; private set; }
-            // Vector3 origDir = Target;       // DEBUG
+    public Vector3 CurrentValue { get; private set; }
-            // Vector3 origVel = CurrentValue;   // DEBUG
+    public BSVMotor(string useName)
-            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete
+        : base(useName)
-            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;
+    {
-            CurrentValue += addAmount;
+        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
+        Efficiency = 1f;
-            float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
+        FrictionTimescale = BSMotor.InfiniteVector;
-            TargetValue *= (1f - decayFactor);
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
-            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;
+    public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
-            CurrentValue *= (Vector3.One - frictionFactor);
+        : this(useName)
-        }
+    {
-        else
+        TimeScale = timeScale;
-        {
+        TargetValueDecayTimeScale = decayTimeScale;
-            // if what remains of direction is very small, zero it.
+        FrictionTimescale = frictionTimeScale;
-            TargetValue = Vector3.Zero;
+        Efficiency = efficiency;
-            CurrentValue = Vector3.Zero;
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
-            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
+    public void SetCurrent(Vector3 current)
-        }
+    {
-        return CurrentValue;
+        CurrentValue = current;
-    }
+    }
-}
+    public void SetTarget(Vector3 target)
+    {
-public class BSFMotor : BSMotor
+        TargetValue = target;
-{
+    }
-    public float TimeScale { get; set; }
+    public Vector3 Step(float timeStep)
-    public float DecayTimeScale { get; set; }
+    {
-    public float Friction { get; set; }
+        Vector3 returnCurrent = Vector3.Zero;
-    public float Efficiency { get; set; }
+        if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
+        {
-    public float Target { get; private set; }
+            Vector3 origTarget = TargetValue;       // DEBUG
-    public float CurrentValue { get; private set; }
+            Vector3 origCurrVal = CurrentValue;   // DEBUG
-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)
+            // Addition =  (desiredVector - currentAppliedVector) / secondsItShouldTakeToComplete
-    {
+            Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
-    }
+            CurrentValue += addAmount;
-    public void SetCurrent(float target)
-    {
+            returnCurrent = CurrentValue;
-    }
-    public void SetTarget(float target)
+            // 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;
-    public float Step(float timeStep)
+            if (TargetValueDecayTimeScale != BSMotor.Infinite)
-    {
+            {
-        return 0f;
+                decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
-    }
+                TargetValue *= (1f - decayFactor);
-}
+            }
-public class BSPIDMotor : BSMotor
-{
+            Vector3 frictionFactor = Vector3.Zero;
-    // TODO: write and use this one
+            if (FrictionTimescale != BSMotor.InfiniteVector)
-    BSPIDMotor()
+            {
-    {
+                // 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
+        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
-    //    pushed to the physics engine. This routine would be used by anyone
+        // TODO: This should be intergrated with a geneal physics action mechanism.
-    //    who is not already pushing the value.
+        // TODO: This should be moderated with PID'ness.
-    private bool PositionSanityCheck(bool inTaintTime)
+        if (ret)
-    {
-        bool ret = false;
-        if (PositionSanityCheck())
        {
-            // The new position value must be pushed into the physics engine but we can't
+            // Apply upforce and overcome gravity.
-            //    just assign to "Position" because of potential call loops.
+            AddForce(upForce - PhysicsScene.DefaultGravity, false, inTaintTime);
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck", delegate()
-            {
-                DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
-            });
-            ret = true;
        }
        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)
    {
-        /*
+        // Updates only for individual prims and for the root object of a linkset.
-        UpdatedProperties changed = 0;
+        if (Linkset.IsRoot(this))
-        // assign to the local variables so the normal set action does not happen
-        // if (_position != entprop.Position)
-        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-        {
-            _position = entprop.Position;
-            changed |= UpdatedProperties.Position;
-        }
-        // if (_orientation != entprop.Rotation)
-        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-        {
-            _orientation = entprop.Rotation;
-            changed |= UpdatedProperties.Rotation;
-        }
-        // if (_velocity != entprop.Velocity)
-        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-        {
-            _velocity = entprop.Velocity;
-            changed |= UpdatedProperties.Velocity;
-        }
-        // if (_acceleration != entprop.Acceleration)
-        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-        {
-            _acceleration = entprop.Acceleration;
-            changed |= UpdatedProperties.Acceleration;
-        }
-        // if (_rotationalVelocity != entprop.RotationalVelocity)
-        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-        {
-            _rotationalVelocity = entprop.RotationalVelocity;
-            changed |= UpdatedProperties.RotationalVel;
-        }
-        if (changed != 0)
        {
-            // Only update the position of single objects and linkset roots
+            // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
-            if (this._parentPrim == null)
+            // 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.
+            // Assign directly to the local variables so the normal set action does not happen
-        if (Linkset.IsRoot(this))
-        {
-            // Assign 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).
+        // Multiply by 55 to give a nominal frame rate of 55.
-        return numSubSteps * m_fixedTimeStep * 1000 * 55;
+        return (float)numSubSteps * m_fixedTimeStep * 1000f * 55f;
-        // return timeStep * 1000 * 55;
    }
    // Something has collided
@@ -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",
+        new ParameterDefn("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
-            0.37f,
+            0.6f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleRadius = cf.GetFloat(p, v); },
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleWidth = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleRadius; },
+            (s) => { return s.m_params[0].avatarCapsuleWidth; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleRadius, p, l, v); } ),
+            (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,
+            ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
-                            ShapeData.FixedShapeKey.KEY_CAPSULE, shapeCallback);
+                            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,
+                    ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
-                                            ShapeData.FixedShapeKey.KEY_SPHERE, shapeCallback);
+                                            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,
+                    ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
-                                            ShapeData.FixedShapeKey.KEY_BOX, shapeCallback);
+                                            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,
+    private BulletShape BuildPhysicalNativeShape(BSPhysObject prim, BSPhysicsShapeType shapeType,
-                                    ShapeData.FixedShapeKey shapeKey)
+                                    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/
+ * Copyright (c) Contributors, http://opensimulator.org/
- * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
- *
+ *
- * Redistribution and use in source and binary forms, with or without
+ * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
+ * modification, are permitted provided that the following conditions are met:
- *     * Redistributions of source code must retain the above copyright
+ *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
+ *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above copyrightD
+ *     * Redistributions in binary form must reproduce the above copyrightD
- *       notice, this list of conditions and the following disclaimer in the
+ *       notice, this list of conditions and the following disclaimer in the
- *       documentation and/or other materials provided with the distribution.
+ *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the OpenSimulator Project nor the
+ *     * Neither the name of the OpenSimulator Project nor the
- *       names of its contributors may be used to endorse or promote products
+ *       names of its contributors may be used to endorse or promote products
- *       derived from this software without specific prior written permission.
+ *       derived from this software without specific prior written permission.
- *
+ *
- * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
- * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * 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
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
+ */
-using System;
+using System;
-using System.Collections.Generic;
+using System.Collections.Generic;
-using System.Linq;
+using System.Linq;
-using System.Text;
+using System.Text;
-namespace OpenSim.Region.Physics.BulletSPlugin
+namespace OpenSim.Region.Physics.BulletSPlugin
-{
+{
-public abstract class BSShape
+public abstract class BSShape
-{
+{
-    public IntPtr ptr { get; set; }
+    public IntPtr ptr { get; set; }
-    public ShapeData.PhysicsShapeType type { get; set; }
+    public BSPhysicsShapeType type { get; set; }
-    public System.UInt64 key { get; set; }
+    public System.UInt64 key { get; set; }
-    public int referenceCount { get; set; }
+    public int referenceCount { get; set; }
-    public DateTime lastReferenced { get; set; }
+    public DateTime lastReferenced { get; set; }
-    protected void Initialize()
+    public BSShape()
-    {
+    {
-        ptr = IntPtr.Zero;
+        ptr = IntPtr.Zero;
-        type = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
+        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
-        key = 0;
+        key = 0;
-        referenceCount = 0;
+        referenceCount = 0;
-        lastReferenced = DateTime.Now;
+        lastReferenced = DateTime.Now;
-    }
+    }
-    // Get a reference to a physical shape. Create if it doesn't exist
+    // Get a reference to a physical shape. Create if it doesn't exist
-    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
-    {
+    {
-        BSShape ret = null;
+        BSShape ret = null;
-        if (prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        if (prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
-        {
+        {
-            // an avatar capsule is close to a native shape (it is not shared)
+            // an avatar capsule is close to a native shape (it is not shared)
-            ret = BSShapeNative.GetReference(physicsScene, prim, ShapeData.PhysicsShapeType.SHAPE_AVATAR,
+            ret = BSShapeNative.GetReference(physicsScene, prim, BSPhysicsShapeType.SHAPE_CAPSULE,
-                                        ShapeData.FixedShapeKey.KEY_CAPSULE);
+                                        FixedShapeKey.KEY_CAPSULE);
-            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);
+            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);
-        }
+        }
-        // Compound shapes are handled special as they are rebuilt from scratch.
+        // Compound shapes are handled special as they are rebuilt from scratch.
-        // This isn't too great a hardship since most of the child shapes will already been created.
+        // This isn't too great a hardship since most of the child shapes will already been created.
-        if (ret == null  && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_COMPOUND)
+        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
+            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added
-            ret = BSShapeCompound.GetReference(prim);
+            ret = BSShapeCompound.GetReference(prim);
-            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
+            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
-        }
+        }
-        if (ret == null)
+        if (ret == null)
-            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
+            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
-        return ret;
+        return ret;
-    }
+    }
-    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
-    {
+    {
-        return null;
+        return null;
-    }
+    }
-    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
-    {
+    {
-        return null;
+        return null;
-    }
+    }
-    // Release the use of a physical shape.
+    // Release the use of a physical shape.
-    public abstract void Dereference(BSScene physicsScene);
+    public abstract void Dereference(BSScene physicsScene);
-    // All shapes have a static call to get a reference to the physical shape
+    // All shapes have a static call to get a reference to the physical shape
-    // protected abstract static BSShape GetReference();
+    // protected abstract static BSShape GetReference();
-    public override string ToString()
+    public override string ToString()
-    {
+    {
-        StringBuilder buff = new StringBuilder();
+        StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
+        buff.Append("<p=");
-        buff.Append(ptr.ToString("X"));
+        buff.Append(ptr.ToString("X"));
-        buff.Append(",s=");
+        buff.Append(",s=");
-        buff.Append(type.ToString());
+        buff.Append(type.ToString());
-        buff.Append(",k=");
+        buff.Append(",k=");
-        buff.Append(key.ToString("X"));
+        buff.Append(key.ToString("X"));
-        buff.Append(",c=");
+        buff.Append(",c=");
-        buff.Append(referenceCount.ToString());
+        buff.Append(referenceCount.ToString());
-        buff.Append(">");
+        buff.Append(">");
-        return buff.ToString();
+        return buff.ToString();
-    }
+    }
-}
+}
-public class BSShapeNull : BSShape
+public class BSShapeNull : BSShape
-{
+{
-    public BSShapeNull()
+    public BSShapeNull() : base()
-    {
+    {
-        base.Initialize();
+    }
-    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
-    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
-    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
+}
-}
+public class BSShapeNative : BSShape
-public class BSShapeNative : BSShape
+{
-{
+    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
-    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
+    public BSShapeNative() : base()
-    public BSShapeNative()
+    {
-    {
+    }
-        base.Initialize();
+    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 
-    }
+                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
-    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);
-        // 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)
-    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
+    {
-                    ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey)
+        ShapeData nativeShapeData = new ShapeData();
-    {
+        nativeShapeData.Type = shapeType;
-        ShapeData nativeShapeData = new ShapeData();
+        nativeShapeData.ID = prim.LocalID;
-        nativeShapeData.Type = shapeType;
+        nativeShapeData.Scale = prim.Scale;
-        nativeShapeData.ID = prim.LocalID;
+        nativeShapeData.Size = prim.Scale;
-        nativeShapeData.Scale = prim.Scale;
+        nativeShapeData.MeshKey = (ulong)shapeKey;
-        nativeShapeData.Size = prim.Scale;
+        nativeShapeData.HullKey = (ulong)shapeKey;
-        nativeShapeData.MeshKey = (ulong)shapeKey;
-        nativeShapeData.HullKey = (ulong)shapeKey;
+       
+        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
-       
+        {
-        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);
-            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
+        }
-            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+        else
-        }
+        {
-        else
+            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);
-        {
+        }
-            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);
+        if (ptr == IntPtr.Zero)
-        }
+        {
-        if (ptr == IntPtr.Zero)
+            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
-        {
+                                    LogHeader, prim.LocalID, shapeType);
-            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
+        }
-                                    LogHeader, prim.LocalID, shapeType);
+        type = shapeType;
-        }
+        key = (UInt64)shapeKey;
-        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)
-    // 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);
-        // Native shapes are not tracked and are released immediately
+        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);
-        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
+        ptr = IntPtr.Zero;
-        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);
+        // Garbage collection will free up this instance.
-        ptr = IntPtr.Zero;
+    }
-        // Garbage collection will free up this instance.
+}
-    }
-}
+public class BSShapeMesh : BSShape
+{
-public class BSShapeMesh : BSShape
+    private static string LogHeader = "[BULLETSIM SHAPE MESH]";
-{
+    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
-    private static string LogHeader = "[BULLETSIM SHAPE MESH]";
-    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
+    public BSShapeMesh() : base()
+    {
-    public BSShapeMesh()
+    }
-    {
+    public static BSShape GetReference() { return new BSShapeNull();  }
-        base.Initialize();
+    public override void Dereference(BSScene physicsScene) { }
-    }
+}
-    public static BSShape GetReference() { return new BSShapeNull();  }
-    public override void Dereference(BSScene physicsScene) { }
+public class BSShapeHull : BSShape
-}
+{
+    private static string LogHeader = "[BULLETSIM SHAPE HULL]";
-public class BSShapeHull : BSShape
+    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
-{
-    private static string LogHeader = "[BULLETSIM SHAPE HULL]";
+    public BSShapeHull() : base()
-    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
+    {
+    }
-    public BSShapeHull()
+    public static BSShape GetReference() { return new BSShapeNull();  }
-    {
+    public override void Dereference(BSScene physicsScene) { }
-        base.Initialize();
+}
-    }
-    public static BSShape GetReference() { return new BSShapeNull();  }
+public class BSShapeCompound : BSShape
-    public override void Dereference(BSScene physicsScene) { }
+{
-}
+    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
+    public BSShapeCompound() : base()
-public class BSShapeCompound : BSShape
+    {
-{
+    }
-    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
+    public static BSShape GetReference(BSPhysObject prim) 
-    public BSShapeCompound()
+    { 
-    {
+        return new BSShapeNull();
-        base.Initialize();
+    }
-    }
+    public override void Dereference(BSScene physicsScene) { }
-    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.
+    // Flags used to know when to recalculate the height.
-    // Used to force recalculation of terrain height after terrain has been modified
+    private bool m_terrainModified = false;
-    private bool m_terrainModified;
    // 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_terrains = new Dictionary<Vector3,BSTerrainPhys>();
-        m_terrainModified = false;
        // 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),
+                    BulletSimAPI.CreateGroundPlaneShape2(BSScene.GROUNDPLANE_ID, 1f, 
-                    ShapeData.PhysicsShapeType.SHAPE_GROUNDPLANE);
+                                    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);
+        // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain.
-        Vector3 maxTerrainCoords = new Vector3(DefaultRegionSize.X, DefaultRegionSize.Y, HEIGHT_INITIALIZATION);
+        BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize);
-        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
+        m_terrains.Add(Vector3.Zero, initialTerrain);
-        float[] initialMap = new float[totalHeights];
-        for (int ii = 0; ii < totalHeights; ii++)
-        {
-            initialMap[ii] = HEIGHT_INITIALIZATION;
-        }
-        UpdateOrCreateTerrain(BSScene.TERRAIN_ID, initialMap, minTerrainCoords, maxTerrainCoords, true);
    }
    // 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);
+                kvp.Value.Dispose();
-                BulletSimAPI.ReleaseHeightMapInfo2(kvp.Value.Ptr);
            }
+            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,
+                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateTerrain(
-                                    localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                                    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);
+        foreach (float height in heightMap)
-        int heightMapSize = heightMap.Length;
-        for (int ii = 0; ii < heightMapSize; ii++)
        {
-            float height = heightMap[ii];
            if (height < minZ) minZ = height;
            if (height > maxZ) maxZ = height;
        }
+        if (minZ == maxZ)
-        // The shape of the terrain is from its base to its extents.
+        {
+            // 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;
+        Vector3 terrainRegionBase = new Vector3(minCoords.X, minCoords.Y, 0f);
-        if (m_heightMaps.TryGetValue(terrainRegionBase, out mapInfo))
+        lock (m_terrains)
        {
-            // If this is terrain we know about, it's easy to update
+            BSTerrainPhys terrainPhys;
+            if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
-            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()
            {
-                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)
+                // if this is a child terrain, calculate a unique terrain id
-                {
+                uint newTerrainID = id;
-                    // Updating an existing terrain.
+                if (newTerrainID >= BSScene.CHILDTERRAIN_ID)
-                    DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,taint,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}",
+                    newTerrainID = ++m_terrainCount;
-                                    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);
-            // Code that must happen at taint-time
+                DetailLog("{0},UpdateTerrain:NewTerrain,taint,newID={1},minCoord={2},maxCoord={3}",
-            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateOrCreateTerrain:NewTerrain", delegate()
+                                            BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
-            {
+                BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
-                DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoords.X, minCoords.Y);
+                m_terrains.Add(terrainRegionBase, newTerrainPhys);
-                // 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);
                m_terrainModified = true;
-            });
+            }
        }
    }
-    // Someday we will have complex terrain with caves and tunnels
+    // TODO: redo terrain implementation selection to allow other base types than heightMap.
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    private BSTerrainPhys BuildPhysicalTerrain(Vector3 terrainRegionBase, uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
    {
-        // For the moment, it's flat and convex
+        PhysicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}", 
-        return GetTerrainHeightAtXY(loc.X, loc.Y);
+                                            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;
+        lock (m_terrains)
-        if (m_heightMaps.TryGetValue(terrainBaseXY, out mapInfo))
        {
-            float regionX = tX - offsetX;
+            BSTerrainPhys physTerrain;
-            float regionY = tY - offsetY;
+            if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
-            int mapIndex = (int)regionY * (int)mapInfo.sizeY + (int)regionX;
-            try
            {
-                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.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
-                PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, x={2}, y={3}",
+                        LogHeader, PhysicsScene.RegionName, tX, tY);
-                                    LogHeader, terrainBaseXY, regionX, regionY);
-                ret = HEIGHT_GETHEIGHT_RET;
            }
-            // 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;
+        type=BSPhysicsShapeType.SHAPE_UNKNOWN;
-        shapeKey = 0;
+        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]