9 files changed, 1983 insertions, 993 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index e2f7af9..526dbad 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -34,13 +34,12 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSCharacter : PhysicsActor
+public class BSCharacter : BSPhysObject
 {
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS CHAR]";
 
-    private BSScene _scene;
-    public BSScene Scene { get { return _scene; } }
+    public BSScene Scene { get; private set; }
     private String _avName;
     // private bool _stopped;
     private Vector3 _size;
@@ -74,11 +73,9 @@ public class BSCharacter : PhysicsActor
     private bool _kinematic;
     private float _buoyancy;
 
-    private BulletBody m_body;
-    public BulletBody Body { 
-        get { return m_body; }
-        set { m_body = value; }
-    }
+    public override BulletBody BSBody { get; set; }
+    public override BulletShape BSShape { get; set; }
+    public override BSLinkset Linkset { get; set; }
 
     private int _subscribedEventsMs = 0;
     private int _nextCollisionOkTime = 0;
@@ -95,7 +92,8 @@ public class BSCharacter : PhysicsActor
     {
         _localID = localID;
         _avName = avName;
-        _scene = parent_scene;
+        Scene = parent_scene;
+        _physicsActorType = (int)ActorTypes.Agent;
         _position = pos;
         _size = size;
         _flying = isFlying;
@@ -104,10 +102,12 @@ public class BSCharacter : PhysicsActor
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         // The dimensions of the avatar capsule are kept in the scale.
         // Physics creates a unit capsule which is scaled by the physics engine.
-        _scale = new Vector3(_scene.Params.avatarCapsuleRadius, _scene.Params.avatarCapsuleRadius, size.Z);
-        _density = _scene.Params.avatarDensity;
+        _scale = new Vector3(Scene.Params.avatarCapsuleRadius, Scene.Params.avatarCapsuleRadius, size.Z);
+        _density = Scene.Params.avatarDensity;
         ComputeAvatarVolumeAndMass();   // set _avatarVolume and _mass based on capsule size, _density and _scale
 
+        Linkset = new BSLinkset(Scene, this);
+
         ShapeData shapeData = new ShapeData();
         shapeData.ID = _localID;
         shapeData.Type = ShapeData.PhysicsShapeType.SHAPE_AVATAR;
@@ -118,29 +118,31 @@ public class BSCharacter : PhysicsActor
         shapeData.Mass = _mass;
         shapeData.Buoyancy = _buoyancy;
         shapeData.Static = ShapeData.numericFalse;
-        shapeData.Friction = _scene.Params.avatarFriction;
-        shapeData.Restitution = _scene.Params.avatarRestitution;
+        shapeData.Friction = Scene.Params.avatarFriction;
+        shapeData.Restitution = Scene.Params.avatarRestitution;
 
         // do actual create at taint time
-        _scene.TaintedObject("BSCharacter.create", delegate()
+        Scene.TaintedObject("BSCharacter.create", delegate()
         {
-            BulletSimAPI.CreateObject(parent_scene.WorldID, shapeData);
+            DetailLog("{0},BSCharacter.create", _localID);
+            BulletSimAPI.CreateObject(Scene.WorldID, shapeData);
 
-            m_body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
-            // avatars get all collisions no matter what
-            BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            // Set the buoyancy for flying. This will be refactored when all the settings happen in C#
+            BulletSimAPI.SetObjectBuoyancy(Scene.WorldID, LocalID, _buoyancy);
+
+            BSBody = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(Scene.World.Ptr, LocalID));
         });
             
         return;
     }
 
     // called when this character is being destroyed and the resources should be released
-    public void Destroy()
+    public override void Destroy()
     {
-        // DetailLog("{0},BSCharacter.Destroy", LocalID);
-        _scene.TaintedObject("BSCharacter.destroy", delegate()
+        DetailLog("{0},BSCharacter.Destroy", LocalID);
+        Scene.TaintedObject("BSCharacter.destroy", delegate()
         {
-            BulletSimAPI.DestroyObject(_scene.WorldID, _localID);
+            BulletSimAPI.DestroyObject(Scene.WorldID, _localID);
         });
     }
 
@@ -169,9 +171,9 @@ public class BSCharacter : PhysicsActor
 
             ComputeAvatarVolumeAndMass();
 
-            _scene.TaintedObject("BSCharacter.setSize", delegate()
+            Scene.TaintedObject("BSCharacter.setSize", delegate()
             {
-                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, LocalID, _scale, _mass, true);
+                BulletSimAPI.SetObjectScaleMass(Scene.WorldID, LocalID, _scale, _mass, true);
             });
 
         } 
@@ -200,17 +202,17 @@ public class BSCharacter : PhysicsActor
 
     public override Vector3 Position { 
         get {
-            // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+            // _position = BulletSimAPI.GetObjectPosition(Scene.WorldID, _localID);
             return _position; 
         } 
         set {
             _position = value;
             PositionSanityCheck();
 
-            _scene.TaintedObject("BSCharacter.setPosition", delegate()
+            Scene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                BulletSimAPI.SetObjectTranslation(Scene.WorldID, _localID, _position, _orientation);
             });
         } 
     }
@@ -223,16 +225,35 @@ public class BSCharacter : PhysicsActor
         bool ret = false;
         
         // If below the ground, move the avatar up
-        float terrainHeight = Scene.GetTerrainHeightAtXYZ(_position);
-        if (_position.Z < terrainHeight)
+        float terrainHeight = Scene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        if (Position.Z < terrainHeight)
         {
-            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},orient={2}", LocalID, _position, _orientation);
+            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
             _position.Z = terrainHeight + 2.0f;
             ret = true;
         }
 
         // TODO: check for out of bounds
+        return ret;
+    }
 
+    // A version of the sanity check that also makes sure a new position value is
+    //    pushed back to the physics engine. This routine would be used by anyone
+    //    who is not already pushing the value.
+    private bool PositionSanityCheck2()
+    {
+        bool ret = false;
+        if (PositionSanityCheck())
+        {
+            // The new position value must be pushed into the physics engine but we can't
+            //    just assign to "Position" because of potential call loops.
+            Scene.TaintedObject("BSCharacter.PositionSanityCheck", delegate()
+            {
+                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                BulletSimAPI.SetObjectTranslation(Scene.WorldID, _localID, _position, _orientation);
+            });
+            ret = true;
+        }
         return ret;
     }
 
@@ -241,6 +262,10 @@ public class BSCharacter : PhysicsActor
             return _mass; 
         } 
     }
+
+    // used when we only want this prim's mass and not the linkset thing
+    public override float MassRaw { get {return _mass; } }
+
     public override Vector3 Force { 
         get { return _force; } 
         set {
@@ -273,10 +298,10 @@ public class BSCharacter : PhysicsActor
         set {
             _velocity = value;
             // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
-            _scene.TaintedObject("BSCharacter.setVelocity", delegate()
+            Scene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
                 DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetObjectVelocity(_scene.WorldID, _localID, _velocity);
+                BulletSimAPI.SetObjectVelocity(Scene.WorldID, _localID, _velocity);
             });
         } 
     }
@@ -299,10 +324,10 @@ public class BSCharacter : PhysicsActor
         set {
             _orientation = value;
             // m_log.DebugFormat("{0}: set orientation to {1}", LogHeader, _orientation);
-            _scene.TaintedObject("BSCharacter.setOrientation", delegate()
+            Scene.TaintedObject("BSCharacter.setOrientation", delegate()
             {
-                // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                // _position = BulletSimAPI.GetObjectPosition(Scene.WorldID, _localID);
+                BulletSimAPI.SetObjectTranslation(Scene.WorldID, _localID, _position, _orientation);
             });
         } 
     }
@@ -319,14 +344,13 @@ public class BSCharacter : PhysicsActor
     public override bool Flying { 
         get { return _flying; } 
         set {
-            if (_flying != value)
-            {
-                _flying = value;
-                // simulate flying by changing the effect of gravity
-                this.Buoyancy = ComputeBuoyancyFromFlying(_flying);
-            }
+            _flying = value;
+            // simulate flying by changing the effect of gravity
+            this.Buoyancy = ComputeBuoyancyFromFlying(_flying);
         } 
     }
+    // Flying is implimented by changing the avatar's buoyancy.
+    // Would this be done better with a vehicle type?
     private float ComputeBuoyancyFromFlying(bool ifFlying) {
         return ifFlying ? 1f : 0f;
     }
@@ -340,11 +364,11 @@ public class BSCharacter : PhysicsActor
         set { _throttleUpdates = value; } 
     }
     public override bool IsColliding {
-        get { return (_collidingStep == _scene.SimulationStep); } 
+        get { return (_collidingStep == Scene.SimulationStep); } 
         set { _isColliding = value; } 
     }
     public override bool CollidingGround {
-        get { return (_collidingGroundStep == _scene.SimulationStep); } 
+        get { return (_collidingGroundStep == Scene.SimulationStep); } 
         set { _collidingGround = value; } 
     }
     public override bool CollidingObj { 
@@ -366,10 +390,10 @@ public class BSCharacter : PhysicsActor
     public override float Buoyancy { 
         get { return _buoyancy; } 
         set { _buoyancy = value; 
-            _scene.TaintedObject("BSCharacter.setBuoyancy", delegate()
+            Scene.TaintedObject("BSCharacter.setBuoyancy", delegate()
             {
                 DetailLog("{0},BSCharacter.setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
-                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _buoyancy);
+                BulletSimAPI.SetObjectBuoyancy(Scene.WorldID, LocalID, _buoyancy);
             });
         } 
     }
@@ -413,10 +437,10 @@ public class BSCharacter : PhysicsActor
             _force.Y += force.Y;
             _force.Z += force.Z;
             // m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
-            _scene.TaintedObject("BSCharacter.AddForce", delegate()
+            Scene.TaintedObject("BSCharacter.AddForce", delegate()
             {
                 DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
-                BulletSimAPI.AddObjectForce2(Body.Ptr, _force);
+                BulletSimAPI.SetObjectForce2(BSBody.Ptr, _force);
             });
         }
         else
@@ -441,18 +465,23 @@ public class BSCharacter : PhysicsActor
 
             Scene.TaintedObject("BSCharacter.SubscribeEvents", delegate()
             {
-                BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
             });
         }
     }
+
+    public override void ZeroMotion()
+    {
+        return;
+    }
+
     // Stop collision events
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
-        // Avatars get all their collision events
-        // Scene.TaintedObject("BSCharacter.UnSubscribeEvents", delegate()
-        // {
-        //     BulletSimAPI.RemoveFromCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
-        // });
+        Scene.TaintedObject("BSCharacter.UnSubscribeEvents", delegate()
+        {
+            BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        });
     }
     // Return 'true' if someone has subscribed to events
     public override bool SubscribedEvents() {
@@ -478,7 +507,7 @@ public class BSCharacter : PhysicsActor
 
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
-    public void UpdateProperties(EntityProperties entprop)
+    public override void UpdateProperties(EntityProperties entprop)
     {
         _position = entprop.Position;
         _orientation = entprop.Rotation;
@@ -488,43 +517,46 @@ public class BSCharacter : PhysicsActor
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
-        /*
-        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
-                entprop.Acceleration, entprop.RotationalVelocity);
-         */
+        // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
+        PositionSanityCheck2();
+
+        float heightHere = Scene.TerrainManager.GetTerrainHeightAtXYZ(_position);   // only for debug
+        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5},terrain={6}",
+                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity, heightHere);
     }
 
     // Called by the scene when a collision with this object is reported
     // The collision, if it should be reported to the character, is placed in a collection
     //   that will later be sent to the simulator when SendCollisions() is called.
     CollisionEventUpdate collisionCollection = null;
-    public void Collide(uint collidingWith, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth)
+    public override bool Collide(uint collidingWith, BSPhysObject collidee, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth)
     {
-        // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith);
+        bool ret = false;
 
         // The following makes IsColliding() and IsCollidingGround() work
-        _collidingStep = _scene.SimulationStep;
-        if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID)
+        _collidingStep = Scene.SimulationStep;
+        if (collidingWith <= Scene.TerrainManager.HighestTerrainID)
         {
-            _collidingGroundStep = _scene.SimulationStep;
+            _collidingGroundStep = Scene.SimulationStep;
         }
         // DetailLog("{0},BSCharacter.Collison,call,with={1}", LocalID, collidingWith);
 
         // throttle collisions to the rate specified in the subscription
-        if (_subscribedEventsMs != 0) {
-            int nowTime = _scene.SimulationNowTime;
+        if (SubscribedEvents()) {
+            int nowTime = Scene.SimulationNowTime;
             if (nowTime >= _nextCollisionOkTime) {
                 _nextCollisionOkTime = nowTime + _subscribedEventsMs;
 
                 if (collisionCollection == null)
                     collisionCollection = new CollisionEventUpdate();
                 collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                ret = true;
             }
         }
+        return ret;
     }
 
-    public void SendCollisions()
+    public override void SendCollisions()
     {
         /*
         if (collisionCollection != null && collisionCollection.Count > 0)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 25084d8..1376a29 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -48,11 +48,10 @@ public abstract class BSConstraint : IDisposable
     {
         if (m_enabled)
         {
-            // BulletSimAPI.RemoveConstraint(m_world.ID, m_body1.ID, m_body2.ID);
+            m_enabled = false;
             bool success = BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
             m_world.scene.DetailLog("{0},BSConstraint.Dispose,taint,body1={1},body2={2},success={3}", BSScene.DetailLogZero, m_body1.ID, m_body2.ID, success);
             m_constraint.Ptr = System.IntPtr.Zero;
-            m_enabled = false;
         }
     }
 
@@ -75,6 +74,17 @@ public abstract class BSConstraint : IDisposable
         return ret;
     }
 
+    public virtual bool SetSolverIterations(float cnt)
+    {
+        bool ret = false;
+        if (m_enabled)
+        {
+            BulletSimAPI.SetConstraintNumSolverIterations2(m_constraint.Ptr, cnt);
+            ret = true;
+        }
+        return ret;
+    }
+
     public virtual bool CalculateTransforms()
     {
         bool ret = false;
@@ -97,8 +107,9 @@ public abstract class BSConstraint : IDisposable
             ret = CalculateTransforms();
             if (ret)
             {
-                // m_world.scene.PhysicsLogging.Write("{0},BSConstraint.RecomputeConstraintVariables,taint,enabling,A={1},B={2}",
-                //                 BSScene.DetailLogZero, Body1.ID, Body2.ID);
+                // Setting an object's mass to zero (making it static like when it's selected)
+                //     automatically disables the constraints.
+                // If the link is enabled, be sure to set the constraint itself to enabled.
                 BulletSimAPI.SetConstraintEnable2(m_constraint.Ptr, m_world.scene.NumericBool(true));
             }
             else
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 5a9f135..61006f0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -57,6 +57,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private int frcount = 0;                                        // Used to limit dynamics debug output to
                                                                         // every 100th frame
 
+        private BSScene m_physicsScene;
         private BSPrim m_prim;      // the prim this dynamic controller belongs to
 
         // Vehicle properties
@@ -74,13 +75,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                                                         // HOVER_UP_ONLY
                                                                         // LIMIT_MOTOR_UP
                                                                         // LIMIT_ROLL_ONLY
-        private VehicleFlag m_Hoverflags = (VehicleFlag)0;
         private Vector3 m_BlockingEndPoint = Vector3.Zero;
         private Quaternion m_RollreferenceFrame = Quaternion.Identity;
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
-        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
+        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;
@@ -124,15 +124,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
 
-        public BSDynamics(BSPrim myPrim)
+        public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
+            m_physicsScene = myScene;
             m_prim = myPrim;
             m_type = Vehicle.TYPE_NONE;
         }
 
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
-            DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -229,9 +230,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }//end ProcessFloatVehicleParam
 
-        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
         {
-            DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -266,7 +267,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
         {
-            DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
+            VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
@@ -280,164 +281,27 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         internal void ProcessVehicleFlags(int pParam, bool remove)
         {
-            DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
+            VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
+            VehicleFlag parm = (VehicleFlag)pParam;
             if (remove)
             {
                 if (pParam == -1)
                 {
                     m_flags = (VehicleFlag)0;
-                    m_Hoverflags = (VehicleFlag)0;
-                    return;
                 }
-                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
-                {
-                    if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
-                {
-                    if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY)
-                {
-                    if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
-                {
-                    if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.MOUSELOOK_BANK);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
-                {
-                    if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.MOUSELOOK_STEER);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
-                {
-                    if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP);
-                }
-                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
-                {
-                    if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED);
-                }
-                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
-                {
-                    if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_X);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
-                {
-                    if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_Y);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
-                {
-                    if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_Z);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
-                {
-                    if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0)
-                        m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
-                {
-                    if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.NO_DEFLECTION);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
+                else
                 {
-                    if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0)
-                        m_flags &= ~(VehicleFlag.LOCK_ROTATION);
+                    m_flags &= ~parm;
                 }
             }
-            else
-            {
-                if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
-                {
-                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
-                {
-                    m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
-                {
-                    m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
-                {
-                    m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
-                {
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
-                {
-                    m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags);
-                }
-                if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
-                {
-                    m_flags |= (VehicleFlag.NO_X);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
-                {
-                    m_flags |= (VehicleFlag.NO_Y);
-                }
-                if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
-                {
-                    m_flags |= (VehicleFlag.NO_Z);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
-                {
-                    m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT);
-                }
-                if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
-                {
-                    m_flags |= (VehicleFlag.NO_DEFLECTION);
-                }
-                if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
-                {
-                    m_flags |= (VehicleFlag.LOCK_ROTATION);
-                }
+            else {
+                m_flags |= parm;
             }
         }//end ProcessVehicleFlags
 
         internal void ProcessTypeChange(Vehicle pType)
         {
-            DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
+            VDetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
             // Set Defaults For Type
             m_type = pType;
             switch (pType)
@@ -478,10 +342,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 10;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &=
+                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
+                    m_flags &=
                          ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
                     break;
                 case Vehicle.TYPE_CAR:
                     m_linearFrictionTimescale = new Vector3(100, 2, 1000);
@@ -506,10 +370,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 1;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
                                 VehicleFlag.LIMIT_MOTOR_UP);
-                    m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY);
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.HOVER_UP_ONLY);
                     break;
                 case Vehicle.TYPE_BOAT:
                     m_linearFrictionTimescale = new Vector3(10, 3, 2);
@@ -534,12 +398,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.8f;
                     // m_bankingTimescale = 1;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
                             VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
                                 VehicleFlag.LIMIT_MOTOR_UP);
-                    m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY);
+                    m_flags |= (VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
                     m_linearFrictionTimescale = new Vector3(200, 10, 5);
@@ -564,7 +428,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 2;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                         VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
@@ -592,15 +456,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // m_bankingMix = 0.7f;
                     // m_bankingTimescale = 5;
                     // m_referenceFrame = Quaternion.Identity;
-                    m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                         VehicleFlag.HOVER_UP_ONLY);
                     m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
-                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
+                    m_flags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
         }//end SetDefaultsForType
 
+        // One step of the vehicle properties for the next 'pTimestep' seconds.
         internal void Step(float pTimestep)
         {
             if (m_type == Vehicle.TYPE_NONE) return;
@@ -613,29 +478,30 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             MoveAngular(pTimestep);
             LimitRotation(pTimestep);
 
-            DetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 
+            // remember the position so next step we can limit absolute movement effects
+            m_lastPositionVector = m_prim.Position;
+
+            VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 
                     m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
         }// end Step
 
         private void MoveLinear(float pTimestep)
         {
-            // requested m_linearMotorDirection is significant
-            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
-            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
+            // m_linearMotorDirection is the direction we are moving relative to the vehicle coordinates
+            // m_lastLinearVelocityVector is the speed we are moving in that direction
+            if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
                 Vector3 origDir = m_linearMotorDirection;
                 Vector3 origVel = m_lastLinearVelocityVector;
 
                 // add drive to body
-                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
-                // lastLinearVelocityVector is the current body velocity vector?
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale / pTimestep);
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale / pTimestep);
+                // lastLinearVelocityVector is the current body velocity vector
                 // RA: Not sure what the *10 is for. A correction for pTimestep?
                 // m_lastLinearVelocityVector += (addAmount*10);  
                 m_lastLinearVelocityVector += addAmount;  
 
-                // This will work temporarily, but we really need to compare speed on an axis
-                // KF: Limit body velocity to applied velocity?
                 // Limit the velocity vector to less than the last set linear motor direction
                 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                     m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
@@ -644,120 +510,83 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                     m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
 
+                /*
                 // decay applied velocity
-                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
+                Vector3 decayfraction = Vector3.One/(m_linearMotorDecayTimescale / pTimestep);
+                // (RA: do not know where the 0.5f comes from)
                 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-
-                /*
-                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
-                m_lastLinearVelocityVector += addAmount;
-
-                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
-                m_linearMotorDirection *= decayfraction;
-
                  */
+                float keepfraction = 1.0f - (1.0f / (m_linearMotorDecayTimescale / pTimestep));
+                m_linearMotorDirection *= keepfraction;
 
-                DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
-                    m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
+                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},notDecay={4},dir={5},vel={6}",
+                    m_prim.LocalID, origDir, origVel, addAmount, keepfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
             }
             else
             {
-                // if what remains of applied is small, zero it.
-                // if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
-                //     m_lastLinearVelocityVector = Vector3.Zero;
+                // if what remains of direction is very small, zero it.
                 m_linearMotorDirection = Vector3.Zero;
                 m_lastLinearVelocityVector = Vector3.Zero;
+                VDetailLog("{0},MoveLinear,zeroed", m_prim.LocalID);
             }
 
-            // convert requested object velocity to world-referenced vector
+            // convert requested object velocity to object relative vector
             Quaternion rotq = m_prim.Orientation;
-            m_dir = m_lastLinearVelocityVector * rotq;
+            m_newVelocity = m_lastLinearVelocityVector * rotq;
 
             // Add the various forces into m_dir which will be our new direction vector (velocity)
 
             // add Gravity and Buoyancy
-            // KF: So far I have found no good method to combine a script-requested
-            // .Z velocity and gravity. Therefore only 0g will used script-requested
-            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
-            Vector3 grav = Vector3.Zero;
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
+            Vector3 grav = m_prim.Scene.DefaultGravity * (m_prim.Mass * (1f - m_VehicleBuoyancy));
+
+            /*
+             * RA: Not sure why one would do this
             // Preserve the current Z velocity
             Vector3 vel_now = m_prim.Velocity;
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
+             */
 
             Vector3 pos = m_prim.Position;
-            Vector3 posChange = pos;
 //            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);
-            double Zchange = Math.Abs(posChange.Z);
-            if (m_BlockingEndPoint != Vector3.Zero)
-            {
-                bool changed = false;
-                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
-                {
-                    pos.X -= posChange.X + 1;
-                    changed = true;
-                }
-                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
-                {
-                    pos.Y -= posChange.Y + 1;
-                    changed = true;
-                }
-                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
-                {
-                    pos.Z -= posChange.Z + 1;
-                    changed = true;
-                }
-                if (pos.X <= 0)
-                {
-                    pos.X += posChange.X + 1;
-                    changed = true;
-                }
-                if (pos.Y <= 0)
-                {
-                    pos.Y += posChange.Y + 1;
-                    changed = true;
-                }
-                if (changed)
-                {
-                    m_prim.Position = pos;
-                    DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
-                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
-                }
-            }
 
             // If below the terrain, move us above the ground a little.
-            if (pos.Z < m_prim.Scene.GetTerrainHeightAtXYZ(pos))
+            float terrainHeight = m_prim.Scene.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.
+            //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
+            // Vector3 rotatedSize = m_prim.Size * m_prim.Orientation;
+            // if (rotatedSize.Z < terrainHeight)
+            if (pos.Z < terrainHeight)
             {
-                pos.Z = m_prim.Scene.GetTerrainHeightAtXYZ(pos) + 2;
+                pos.Z = terrainHeight + 2;
                 m_prim.Position = pos;
-                DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
+                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", m_prim.LocalID, terrainHeight, pos);
             }
 
             // Check if hovering
-            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
+            if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
             {
                 // We should hover, get the target height
-                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
                 }
-                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = m_prim.Scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
                 }
-                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
+                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
                     m_VhoverTargetHeight = m_VhoverHeight;
                 }
 
-                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
+                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
                     // If body is aready heigher, use its height as target height
                     if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
                 }
-                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
+                if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
                     if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                     {
@@ -770,82 +599,92 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Replace Vertical speed with correction figure if significant
                     if (Math.Abs(herr0) > 0.01f)
                     {
-                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                        m_newVelocity.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                         //KF: m_VhoverEfficiency is not yet implemented
                     }
                     else
                     {
-                        m_dir.Z = 0f;
+                        m_newVelocity.Z = 0f;
                     }
                 }
 
-                DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
+            }
 
-//                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
-//                m_VhoverTimescale = 0f;        // time to acheive height
-//                pTimestep  is time since last frame,in secs
+            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;
+                    changed = true;
+                }
+                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
+                {
+                    pos.Y -= posChange.Y + 1;
+                    changed = true;
+                }
+                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
+                {
+                    pos.Z -= posChange.Z + 1;
+                    changed = true;
+                }
+                if (pos.X <= 0)
+                {
+                    pos.X += posChange.X + 1;
+                    changed = true;
+                }
+                if (pos.Y <= 0)
+                {
+                    pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
+                    m_prim.Position = pos;
+                    VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
+                }
             }
 
+            float Zchange = Math.Abs(posChange.Z);
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                //Start Experimental Values
                 if (Zchange > .3)
-                {
                     grav.Z = (float)(grav.Z * 3);
-                }
                 if (Zchange > .15)
-                {
                     grav.Z = (float)(grav.Z * 2);
-                }
                 if (Zchange > .75)
-                {
                     grav.Z = (float)(grav.Z * 1.5);
-                }
                 if (Zchange > .05)
-                {
                     grav.Z = (float)(grav.Z * 1.25);
-                }
                 if (Zchange > .025)
-                {
                     grav.Z = (float)(grav.Z * 1.125);
-                }
-                float terraintemp = m_prim.Scene.GetTerrainHeightAtXYZ(pos);
-                float postemp = (pos.Z - terraintemp);
+                float postemp = (pos.Z - terrainHeight);
                 if (postemp > 2.5f)
-                {
                     grav.Z = (float)(grav.Z * 1.037125);
-                }
-                DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
-                //End Experimental Values
+                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
             }
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
-            {
-                m_dir.X = 0;
-            }
+                m_newVelocity.X = 0;
             if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-            {
-                m_dir.Y = 0;
-            }
+                m_newVelocity.Y = 0;
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-            {
-                m_dir.Z = 0;
-            }
-
-            m_lastPositionVector = m_prim.Position;
+                m_newVelocity.Z = 0;
 
             // Apply velocity
-            m_prim.Velocity = m_dir;
+            m_prim.Velocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
             // m_prim.AddForce(grav, false);
-            // m_prim.Force = grav;
 
             // Apply friction
-            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
-            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
+            m_lastLinearVelocityVector *= keepFraction;
 
-            DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
-                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4},1Mdecay={5}", 
+                    m_prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav, keepFraction);
 
         } // end MoveLinear()
 
@@ -870,30 +709,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // There are m_angularMotorApply steps.
                 Vector3 origAngularVelocity = m_angularMotorVelocity;
                 // ramp up to new value
-                //   current velocity  +=                         error                       /    (time to get there / step interval)
-                //                               requested speed            -  last motor speed
+                //   current velocity    +=                         error                          /    (  time to get there   / step interval)
+                //                               requested speed       -       last motor speed
                 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
 
-                DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}", 
-                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
+                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},dir={5},vel={6}", 
+                        m_prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
 
-                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
-                                            // velocity may still be acheived.
+                // This is done so that if script request rate is less than phys frame rate the expected
+                //    velocity may still be acheived.
+                m_angularMotorApply--;
             }
             else
             {
                 // No motor recently applied, keep the body velocity
                 // and decay the velocity
                 m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
+                if (m_angularMotorVelocity.LengthSquared() < 0.00001)
+                    m_angularMotorVelocity = Vector3.Zero;
             } // end motor section
 
             // Vertical attractor section
             Vector3 vertattr = Vector3.Zero;
             if (m_verticalAttractionTimescale < 300)
             {
-                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
+                float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
                 // get present body rotation
                 Quaternion rotq = m_prim.Orientation;
                 // make a vector pointing up
@@ -924,7 +766,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
-                DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
+                VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
                             m_prim.LocalID, verterr, bounce, vertattr);
 
             } // else vertical attractor is off
@@ -942,13 +784,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_lastAngularVelocity.X = 0;
                 m_lastAngularVelocity.Y = 0;
-                DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
             }
 
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
             }
 
              // apply friction
@@ -958,7 +800,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Apply to the body
             m_prim.RotationalVelocity = m_lastAngularVelocity;
 
-            DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
+            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)
@@ -996,20 +838,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_rot.Y = 0;
                 changed = true;
             }
-            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
-            {
-                m_rot.X = 0;
-                m_rot.Y = 0;
-                changed = true;
-            }
             if (changed)
+            {
                 m_prim.Orientation = m_rot;
+                VDetailLog("{0},LimitRotation,done,orig={1},new={2}", m_prim.LocalID, rotq, m_rot);
+            }
 
-            DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
         }
 
         // Invoke the detailed logger and output something if it's enabled.
-        private void DetailLog(string msg, params Object[] args)
+        private void VDetailLog(string msg, params Object[] args)
         {
             if (m_prim.Scene.VehicleLoggingEnabled)
                 m_prim.Scene.PhysicsLogging.Write(msg, args);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 087b9bb..7e784eb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -36,14 +36,17 @@ public class BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET]";
 
-    private BSPrim m_linksetRoot;
-    public BSPrim LinksetRoot { get { return m_linksetRoot; } }
+    private BSPhysObject m_linksetRoot;
+    public BSPhysObject LinksetRoot { get { return m_linksetRoot; } }
 
     private BSScene m_physicsScene;
     public BSScene PhysicsScene { get { return m_physicsScene; } }
 
+    static int m_nextLinksetID = 1;
+    public int LinksetID { get; private set; }
+
     // The children under the root in this linkset
-    private List<BSPrim> m_children;
+    private List<BSPhysObject> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -71,19 +74,23 @@ public class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    public BSLinkset(BSScene scene, BSPrim parent)
+    public BSLinkset(BSScene scene, BSPhysObject parent)
     {
         // A simple linkset of one (no children)
+        LinksetID = m_nextLinksetID++;
+        // We create LOTS of linksets.
+        if (m_nextLinksetID < 0) 
+            m_nextLinksetID = 1;
         m_physicsScene = scene;
         m_linksetRoot = parent;
-        m_children = new List<BSPrim>();
+        m_children = new List<BSPhysObject>();
         m_mass = parent.MassRaw;
     }
 
     // Link to a linkset where the child knows the parent.
     // Parent changing should not happen so do some sanity checking.
     // We return the parent's linkset so the child can track its membership.
-    public BSLinkset AddMeToLinkset(BSPrim child)
+    public BSLinkset AddMeToLinkset(BSPhysObject child)
     {
         lock (m_linksetActivityLock)
         {
@@ -95,7 +102,7 @@ public class BSLinkset
     // Remove a child from a linkset.
     // Returns a new linkset for the child which is a linkset of one (just the
     //    orphened child).
-    public BSLinkset RemoveMeFromLinkset(BSPrim child)
+    public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
     {
         lock (m_linksetActivityLock)
         {
@@ -122,7 +129,7 @@ public class BSLinkset
     }
 
     // Return 'true' if the passed object is the root object of this linkset
-    public bool IsRoot(BSPrim requestor)
+    public bool IsRoot(BSPhysObject requestor)
     {
         return (requestor.LocalID == m_linksetRoot.LocalID);
     }
@@ -133,12 +140,12 @@ public class BSLinkset
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
     // Return 'true' if this child is in this linkset
-    public bool HasChild(BSPrim child)
+    public bool HasChild(BSPhysObject child)
     {
         bool ret = false;
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 if (child.LocalID == bp.LocalID)
                 {
@@ -153,7 +160,7 @@ public class BSLinkset
     private float ComputeLinksetMass()
     {
         float mass = m_linksetRoot.MassRaw;
-        foreach (BSPrim bp in m_children)
+        foreach (BSPhysObject bp in m_children)
         {
             mass += bp.MassRaw;
         }
@@ -167,7 +174,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 com += bp.Position * bp.MassRaw;
                 totalMass += bp.MassRaw;
@@ -185,7 +192,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim bp in m_children)
+            foreach (BSPhysObject bp in m_children)
             {
                 com += bp.Position * bp.MassRaw;
             }
@@ -195,11 +202,33 @@ public class BSLinkset
         return com;
     }
 
+    // The object is going dynamic (physical). Do any setup necessary
+    //     for a dynamic linkset.
+    // Only the state of the passed object can be modified. The rest of the linkset
+    //     has not yet been fully constructed.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public bool MakeDynamic(BSPhysObject child)
+    {
+        bool ret = false;
+        return ret;
+    }
+
+    // The object is going static (non-physical). Do any setup necessary
+    //     for a static linkset.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public bool MakeStatic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
-    public void Refresh(BSPrim requestor)
+    public void Refresh(BSPhysObject requestor)
     {
-        // If there are no children, there aren't any constraints to recompute
+        // If there are no children, there can't be any constraints to recompute
         if (!HasAnyChildren)
             return;
 
@@ -218,15 +247,16 @@ public class BSLinkset
     //    from a linkset to make sure the constraints know about the new mass and
     //    geometry.
     // Must only be called at taint time!!
-    private bool RecomputeLinksetConstraintVariables()
+    private void RecomputeLinksetConstraintVariables()
     {
         float linksetMass = LinksetMass;
         lock (m_linksetActivityLock)
         {
-            foreach (BSPrim child in m_children)
+            bool somethingMissing = false;
+            foreach (BSPhysObject child in m_children)
             {
                 BSConstraint constrain;
-                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.Body, child.Body, out constrain))
+                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
                 {
                     // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}", 
                     //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
@@ -234,32 +264,51 @@ public class BSLinkset
                 }
                 else
                 {
-                    // Non-fatal error that can happen when children are being added to the linkset but
+                    // Non-fatal error that happens when children are being added to the linkset but
                     //    their constraints have not been created yet.
                     // Caused by the fact that m_children is built at run time but building constraints
                     //    happens at taint time.
-                    // m_physicsScene.Logger.ErrorFormat("[BULLETSIM LINKSET] RecomputeLinksetConstraintVariables: constraint not found for root={0}, child={1}",
-                    //                                 m_linksetRoot.Body.ID, child.Body.ID);
+                    somethingMissing = true;
+                    break;
                 }
             }
+
+            // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
+            if (!somethingMissing)
+            {
+                // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
+                OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
+                BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.Ptr, centerOfMass, OMV.Quaternion.Identity);
+                foreach (BSPhysObject child in m_children)
+                {
+                    BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.Ptr, centerOfMass, OMV.Quaternion.Identity);
+                }
+                /*
+                // The root prim takes on the weight of the whole linkset
+                OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(LinksetRoot.BSShape.Ptr, linksetMass);
+                BulletSimAPI.SetMassProps2(LinksetRoot.BSBody.Ptr, linksetMass, inertia);
+                OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
+                BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.Ptr, centerOfMass, OMV.Quaternion.Identity);
+                BulletSimAPI.UpdateInertiaTensor2(LinksetRoot.BSBody.Ptr);
+                 */
+            }
         }
-        return false;
+        return;
     }
 
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
-    private void AddChildToLinkset(BSPrim child)
+    private void AddChildToLinkset(BSPhysObject child)
     {
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            BSPrim rootx = LinksetRoot; // capture the root as of now
-            BSPrim childx = child;
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject childx = child;
             m_physicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
-                // DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
-                // DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
                 PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the child
             });
         }
@@ -271,7 +320,7 @@ public class BSLinkset
     //    it's still connected to the linkset.
     // Normal OpenSimulator operation will never do this because other SceneObjectPart information
     //    has to be updated also (like pointer to prim's parent).
-    private void RemoveChildFromOtherLinkset(BSPrim pchild)
+    private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
     {
         pchild.Linkset = new BSLinkset(m_physicsScene, pchild);
         RemoveChildFromLinkset(pchild);
@@ -279,21 +328,20 @@ public class BSLinkset
 
     // I am the root of a linkset and one of my children is being removed.
     // Safe to call even if the child is not really in my linkset.
-    private void RemoveChildFromLinkset(BSPrim child)
+    private void RemoveChildFromLinkset(BSPhysObject child)
     {
         if (m_children.Remove(child))
         {
-            BSPrim rootx = LinksetRoot; // capture the root as of now
-            BSPrim childx = child;
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject childx = child;
             m_physicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
-                // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
-                // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
                 PhysicallyUnlinkAChildFromRoot(rootx, childx);
+                RecomputeLinksetConstraintVariables();
             });
 
-            RecomputeLinksetConstraintVariables();
         }
         else
         {
@@ -305,7 +353,7 @@ public class BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPrim rootPrim, BSPrim childPrim)
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion();
@@ -319,19 +367,18 @@ public class BSLinkset
 
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2},rLoc={3},cLoc={4},midLoc={5}", 
             rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID, rootPrim.Position, childPrim.Position, midPoint);
         BS6DofConstraint constrain = new BS6DofConstraint(
-                        m_physicsScene.World, rootPrim.Body, childPrim.Body,
+                        m_physicsScene.World, rootPrim.BSBody, childPrim.BSBody,
                         midPoint,
                         true,
                         true
                         );
-        /* NOTE: attempt to build constraint with full frame computation, etc.
+        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
          *     Using the midpoint is easier since it lets the Bullet code use the transforms
          *     of the objects.
-         * Code left here as an example.
+         * Code left as a warning to future programmers.
         // ==================================================================================
         // relative position normalized to the root prim
         OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
@@ -343,7 +390,6 @@ public class BSLinkset
 
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
         BS6DofConstraint constrain = new BS6DofConstraint(
                         PhysicsScene.World, rootPrim.Body, childPrim.Body,
@@ -374,40 +420,34 @@ public class BSLinkset
                         PhysicsScene.Params.linkConstraintTransMotorMaxVel,
                         PhysicsScene.Params.linkConstraintTransMotorMaxForce);
         constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
+        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        {
+            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+        }
 
         RecomputeLinksetConstraintVariables();
     }
 
     // Remove linkage between myself and a particular child
     // Called at taint time!
-    private void PhysicallyUnlinkAChildFromRoot(BSPrim rootPrim, BSPrim childPrim)
+    private void PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
-        // DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
-        //             LogHeader, rootPrim.LocalID, childPrim.LocalID);
         DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
 
         // Find the constraint for this link and get rid of it from the overall collection and from my list
-        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody);
 
         // Make the child refresh its location
-        BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
+        BulletSimAPI.PushUpdate2(childPrim.BSBody.Ptr);
     }
 
     // Remove linkage between myself and any possible children I might have
     // Called at taint time!
-    private void PhysicallyUnlinkAllChildrenFromRoot(BSPrim rootPrim)
+    private void PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
     {
-        // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
         DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
 
-        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
-    }
-
-    // Invoke the detailed logger and output something if it's enabled.
-    private void DebugLog(string msg, params Object[] args)
-    {
-        if (m_physicsScene.ShouldDebugLog)
-            m_physicsScene.Logger.DebugFormat(msg, args);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
new file mode 100755
index 0000000..3fe71e1
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OMV = OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+// Class to wrap all objects.
+// The rest of BulletSim doesn't need to keep checking for avatars or prims
+// unless the difference is significant.
+public abstract class BSPhysObject : PhysicsActor
+{
+    public abstract BSLinkset Linkset { get; set; }
+
+    public abstract bool Collide(uint collidingWith, BSPhysObject collidee,
+            OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth);
+    public abstract void SendCollisions();
+
+    // Return the object mass without calculating it or side effects
+    public abstract float MassRaw { get; }
+
+    // Reference to the physical body (btCollisionObject) of this object
+    public abstract BulletBody BSBody { get; set; }
+    // Reference to the physical shape (btCollisionShape) of this object
+    public abstract BulletShape BSShape { get; set; }
+
+    public abstract void ZeroMotion();
+
+    public virtual void StepVehicle(float timeStep) { }
+
+    public abstract void UpdateProperties(EntityProperties entprop);
+
+    public abstract void Destroy();
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 9c20004..26a581f 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -37,13 +37,11 @@ using OpenSim.Region.Physics.ConvexDecompositionDotNet;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
     [Serializable]
-public sealed class BSPrim : PhysicsActor
+public sealed class BSPrim : BSPhysObject
 {
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS PRIM]";
 
-    private void DebugLog(string mm, params Object[] xx) { if (_scene.ShouldDebugLog) m_log.DebugFormat(mm, xx); }
-
     private IMesh _mesh;
     private PrimitiveBaseShape _pbs;
     private ShapeData.PhysicsShapeType _shapeType;
@@ -90,23 +88,16 @@ public sealed class BSPrim : PhysicsActor
     private float _buoyancy;
 
     // Membership in a linkset is controlled by this class.
-    private BSLinkset _linkset;
-    public BSLinkset Linkset
-    {
-        get { return _linkset; }
-        set { _linkset = value; }
-    }
+    public override BSLinkset Linkset { get; set; }
 
     private int _subscribedEventsMs = 0;
     private int _nextCollisionOkTime = 0;
     long _collidingStep;
     long _collidingGroundStep;
+    CollisionFlags m_currentCollisionFlags = 0;
 
-    private BulletBody m_body;
-    public BulletBody Body { 
-        get { return m_body; }
-        set { m_body = value; }
-    }
+    public override BulletBody BSBody { get; set; }
+    public override BulletShape BSShape { get; set; }
 
     private BSDynamics _vehicle;
 
@@ -124,6 +115,7 @@ public sealed class BSPrim : PhysicsActor
         // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID);
         _localID = localID;
         _avName = primName;
+        _physicsActorType = (int)ActorTypes.Prim;
         _scene = parent_scene;
         _position = pos;
         _size = size;
@@ -141,35 +133,37 @@ public sealed class BSPrim : PhysicsActor
         _friction = _scene.Params.defaultFriction;  // TODO: compute based on object material
         _density = _scene.Params.defaultDensity;    // TODO: compute based on object material
         _restitution = _scene.Params.defaultRestitution;
-        _linkset = new BSLinkset(_scene, this);     // a linkset of one
-        _vehicle = new BSDynamics(this);            // add vehicleness
+        Linkset = new BSLinkset(Scene, this);     // a linkset of one
+        _vehicle = new BSDynamics(Scene, this);            // add vehicleness
         _mass = CalculateMass();
-        // do the actual object creation at taint time
         DetailLog("{0},BSPrim.constructor,call", LocalID);
+        // do the actual object creation at taint time
         _scene.TaintedObject("BSPrim.create", delegate()
         {
-            RecreateGeomAndObject();
+            CreateGeomAndObject(true);
 
             // Get the pointer to the physical body for this object.
             // At the moment, we're still letting BulletSim manage the creation and destruction
             //    of the object. Someday we'll move that into the C# code.
-            m_body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+            BSBody = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+            BSShape = new BulletShape(BulletSimAPI.GetCollisionShape2(BSBody.Ptr));
+            m_currentCollisionFlags = BulletSimAPI.GetCollisionFlags2(BSBody.Ptr);
         });
     }
 
     // called when this prim is being destroyed and we should free all the resources
-    public void Destroy()
+    public override void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
 
         // Undo any links between me and any other object
-        BSPrim parentBefore = _linkset.LinksetRoot;
-        int childrenBefore = _linkset.NumberOfChildren;
+        BSPhysObject parentBefore = Linkset.LinksetRoot;
+        int childrenBefore = Linkset.NumberOfChildren;
 
-        _linkset = _linkset.RemoveMeFromLinkset(this);
+        Linkset = Linkset.RemoveMeFromLinkset(this);
 
         DetailLog("{0},BSPrim.Destroy,call,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}",
-            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
+            LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
 
         // Undo any vehicle properties
         this.VehicleType = (int)Vehicle.TYPE_NONE;
@@ -192,9 +186,10 @@ public sealed class BSPrim : PhysicsActor
             _scene.TaintedObject("BSPrim.setSize", delegate()
             {
                 _mass = CalculateMass();   // changing size changes the mass
-                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, (IsPhysical ? _mass : 0f), IsPhysical);
-                // DetailLog("{0}: BSPrim.setSize: size={1}, mass={2}, physical={3}", LocalID, _size, _mass, IsPhysical);
-                RecreateGeomAndObject();
+                // Since _size changed, the mesh needs to be rebuilt. If rebuilt, all the correct
+                //   scale and margins are set.
+                CreateGeomAndObject(true);
+                DetailLog("{0}: BSPrim.setSize: size={1}, scale={2}, mass={3}, physical={4}", LocalID, _size, _scale, _mass, IsPhysical);
             });
         } 
     }
@@ -204,7 +199,7 @@ public sealed class BSPrim : PhysicsActor
             _scene.TaintedObject("BSPrim.setShape", delegate()
             {
                 _mass = CalculateMass();   // changing the shape changes the mass
-                RecreateGeomAndObject();
+                CreateGeomAndObject(false);
             });
         } 
     }
@@ -232,14 +227,13 @@ public sealed class BSPrim : PhysicsActor
         BSPrim parent = obj as BSPrim;
         if (parent != null)
         {
-            DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, parent.LocalID);
-            BSPrim parentBefore = _linkset.LinksetRoot;
-            int childrenBefore = _linkset.NumberOfChildren;
+            BSPhysObject parentBefore = Linkset.LinksetRoot;
+            int childrenBefore = Linkset.NumberOfChildren;
 
-            _linkset = parent.Linkset.AddMeToLinkset(this);
+            Linkset = parent.Linkset.AddMeToLinkset(this);
 
             DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}", 
-                LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
+                LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
         }
         return; 
     }
@@ -248,16 +242,14 @@ public sealed class BSPrim : PhysicsActor
     public override void delink() {
         // TODO: decide if this parent checking needs to happen at taint time
         // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
-        DebugLog("{0}: delink {1}/{2}. Parent={3}", LogHeader, _avName, _localID, 
-                            _linkset.LinksetRoot._avName+"/"+_linkset.LinksetRoot.LocalID.ToString());
 
-        BSPrim parentBefore = _linkset.LinksetRoot;
-        int childrenBefore = _linkset.NumberOfChildren;
+        BSPhysObject parentBefore = Linkset.LinksetRoot;
+        int childrenBefore = Linkset.NumberOfChildren;
         
-        _linkset = _linkset.RemoveMeFromLinkset(this);
+        Linkset = Linkset.RemoveMeFromLinkset(this);
 
         DetailLog("{0},BSPrim.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ", 
-            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
+            LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
         return; 
     }
 
@@ -265,30 +257,30 @@ public sealed class BSPrim : PhysicsActor
     // Do it to the properties so the values get set in the physics engine.
     // Push the setting of the values to the viewer.
     // Called at taint time!
-    public void ZeroMotion()
+    public override void ZeroMotion()
     {
         _velocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        BulletSimAPI.SetVelocity2(Body.Ptr, OMV.Vector3.Zero);
-        BulletSimAPI.SetAngularVelocity2(Body.Ptr, OMV.Vector3.Zero);
-        BulletSimAPI.SetInterpolation2(Body.Ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
-        BulletSimAPI.ClearForces2(Body.Ptr);
+        BulletSimAPI.SetLinearVelocity2(BSBody.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetAngularVelocity2(BSBody.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetInterpolationVelocity2(BSBody.Ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
+        BulletSimAPI.ClearForces2(BSBody.Ptr);
     }
 
     public override void LockAngularMotion(OMV.Vector3 axis)
     { 
-        // DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
+        DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
         return;
     }
 
     public override OMV.Vector3 Position { 
         get { 
-            if (!_linkset.IsRoot(this))
+            if (!Linkset.IsRoot(this))
                 // child prims move around based on their parent. Need to get the latest location
-                _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+                _position = BulletSimAPI.GetPosition2(BSBody.Ptr);
 
             // don't do the GetObjectPosition for root elements because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
@@ -299,8 +291,8 @@ public sealed class BSPrim : PhysicsActor
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             _scene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                BulletSimAPI.SetTranslation2(BSBody.Ptr, _position, _orientation);
             });
         } 
     }
@@ -311,23 +303,24 @@ public sealed class BSPrim : PhysicsActor
     { 
         get
         {
-            return _linkset.LinksetMass;
+            // return Linkset.LinksetMass;
+            return _mass;
         }
     }
 
     // used when we only want this prim's mass and not the linkset thing
-    public float MassRaw { get { return _mass; } }
+    public override float MassRaw { get { return _mass; }  }
 
     // Is this used?
     public override OMV.Vector3 CenterOfMass
     {
-        get { return _linkset.CenterOfMass; }
+        get { return Linkset.CenterOfMass; }
     }
 
     // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
-        get { return _linkset.GeometricCenter; }
+        get { return Linkset.GeometricCenter; }
     }
 
     public override OMV.Vector3 Force { 
@@ -336,9 +329,8 @@ public sealed class BSPrim : PhysicsActor
             _force = value;
             _scene.TaintedObject("BSPrim.setForce", delegate()
             {
-                // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                // BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
-                BulletSimAPI.SetObjectForce2(Body.Ptr, _force);
+                DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
+                BulletSimAPI.SetObjectForce2(BSBody.Ptr, _force);
             });
         } 
     }
@@ -364,14 +356,14 @@ public sealed class BSPrim : PhysicsActor
     {
         _scene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
-            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
         });
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) 
     {
         _scene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
-            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
         });
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
@@ -391,7 +383,7 @@ public sealed class BSPrim : PhysicsActor
 
     // Called each simulation step to advance vehicle characteristics.
     // Called from Scene when doing simulation step so we're in taint processing time.
-    public void StepVehicle(float timeStep)
+    public override void StepVehicle(float timeStep)
     {
         if (IsPhysical)
             _vehicle.Step(timeStep);
@@ -414,15 +406,15 @@ public sealed class BSPrim : PhysicsActor
             _velocity = value;
             _scene.TaintedObject("BSPrim.setVelocity", delegate()
             {
-                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
+                DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
+                BulletSimAPI.SetLinearVelocity2(BSBody.Ptr, _velocity);
             });
         } 
     }
     public override OMV.Vector3 Torque { 
         get { return _torque; } 
         set { _torque = value; 
-            // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
+            DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         } 
     }
     public override float CollisionScore { 
@@ -436,10 +428,10 @@ public sealed class BSPrim : PhysicsActor
     }
     public override OMV.Quaternion Orientation { 
         get {
-            if (!_linkset.IsRoot(this))
+            if (!Linkset.IsRoot(this))
             {
                 // Children move around because tied to parent. Get a fresh value.
-                _orientation = BulletSimAPI.GetObjectOrientation(_scene.WorldID, LocalID);
+                _orientation = BulletSimAPI.GetOrientation2(BSBody.Ptr);
             }
             return _orientation;
         } 
@@ -449,15 +441,14 @@ public sealed class BSPrim : PhysicsActor
             _scene.TaintedObject("BSPrim.setOrientation", delegate()
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
-                // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
+                DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                BulletSimAPI.SetTranslation2(BSBody.Ptr, _position, _orientation);
             });
         } 
     }
     public override int PhysicsActorType { 
         get { return _physicsActorType; } 
-        set { _physicsActorType = value; 
-        } 
+        set { _physicsActorType = value; } 
     }
     public override bool IsPhysical { 
         get { return _isPhysical; } 
@@ -484,30 +475,139 @@ public sealed class BSPrim : PhysicsActor
 
     // Make gravity work if the object is physical and not selected
     // No locking here because only called when it is safe
+    // There are four flags we're interested in:
+    //     IsStatic: Object does not move, otherwise the object has mass and moves
+    //     isSolid: other objects bounce off of this object
+    //     isVolumeDetect: other objects pass through but can generate collisions
+    //     collisionEvents: whether this object returns collision events
     private void SetObjectDynamic()
     {
-        // RA: remove this for the moment.
-        // The problem is that dynamic objects are hulls so if we are becoming physical
-        //    the shape has to be checked and possibly built.
-        //    Maybe a VerifyCorrectPhysicalShape() routine?
-        // RecreateGeomAndObject();
+        // If it's becoming dynamic, it will need hullness
+        VerifyCorrectPhysicalShape();
+        UpdatePhysicalParameters();
+    }
 
+    private void UpdatePhysicalParameters()
+    {
+        /*
         // Bullet wants static objects to have a mass of zero
         float mass = IsStatic ? 0f : _mass;
 
         BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), mass);
+         */
+        BulletSimAPI.RemoveObjectFromWorld2(Scene.World.Ptr, BSBody.Ptr);
+
+        // Set up the object physicalness (does gravity and collisions move this object)
+        MakeDynamic(IsStatic);
+
+        // Make solid or not (do things bounce off or pass through this object)
+        MakeSolid(IsSolid);
+
+        // Arrange for collisions events if the simulator wants them
+        EnableCollisions(SubscribedEvents());
+
+        BulletSimAPI.AddObjectToWorld2(Scene.World.Ptr, BSBody.Ptr);
 
-        // recompute any linkset parameters
-        _linkset.Refresh(this);
+        // Recompute any linkset parameters.
+        // When going from non-physical to physical, this re-enables the constraints that
+        //     had been automatically disabled when the mass was set to zero.
+        Linkset.Refresh(this);
 
-        CollisionFlags cf = BulletSimAPI.GetCollisionFlags2(Body.Ptr);
-        // DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}, cf={4}", LocalID, IsStatic, IsSolid, mass, cf);
+        DetailLog("{0},BSPrim.UpdatePhysicalParameters,taint,static={1},solid={2},mass={3}, cf={4}", 
+                        LocalID, IsStatic, IsSolid, _mass, m_currentCollisionFlags);
+    }
+
+    // "Making dynamic" means changing to and from static.
+    // When static, gravity does not effect the object and it is fixed in space.
+    // When dynamic, the object can fall and be pushed by others.
+    // This is independent of its 'solidness' which controls what passes through
+    //    this object and what interacts with it.
+    private void MakeDynamic(bool makeStatic)
+    {
+        if (makeStatic)
+        {
+            // Become a Bullet 'static' object type
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
+            // Stop all movement
+            BulletSimAPI.ClearAllForces2(BSBody.Ptr);
+            // Center of mass is at the center of the object
+            BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.BSBody.Ptr, _position, _orientation);
+            // Mass is zero which disables a bunch of physics stuff in Bullet
+            BulletSimAPI.SetMassProps2(BSBody.Ptr, 0f, OMV.Vector3.Zero);
+            // There is no inertia in a static object
+            BulletSimAPI.UpdateInertiaTensor2(BSBody.Ptr);
+            // There can be special things needed for implementing linksets
+            Linkset.MakeStatic(this);
+            // The activation state is 'sleeping' so Bullet will not try to act on it
+            BulletSimAPI.ForceActivationState2(BSBody.Ptr, ActivationState.ISLAND_SLEEPING);
+        }
+        else
+        {
+            // Not a Bullet static object
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+            // Set various physical properties so internal things will get computed correctly as they are set
+            BulletSimAPI.SetFriction2(BSBody.Ptr, Scene.Params.defaultFriction);
+            BulletSimAPI.SetRestitution2(BSBody.Ptr, Scene.Params.defaultRestitution);
+            // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
+            BulletSimAPI.SetInterpolationLinearVelocity2(BSBody.Ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetInterpolationAngularVelocity2(BSBody.Ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetInterpolationVelocity2(BSBody.Ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
+
+            // A dynamic object has mass
+            IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.Ptr);
+            OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Linkset.LinksetMass);
+            BulletSimAPI.SetMassProps2(BSBody.Ptr, _mass, inertia);
+            // Inertia is based on our new mass
+            BulletSimAPI.UpdateInertiaTensor2(BSBody.Ptr);
+
+            // Various values for simulation limits
+            BulletSimAPI.SetDamping2(BSBody.Ptr, Scene.Params.linearDamping, Scene.Params.angularDamping);
+            BulletSimAPI.SetDeactivationTime2(BSBody.Ptr, Scene.Params.deactivationTime);
+            BulletSimAPI.SetSleepingThresholds2(BSBody.Ptr, Scene.Params.linearSleepingThreshold, Scene.Params.angularSleepingThreshold);
+            BulletSimAPI.SetContactProcessingThreshold2(BSBody.Ptr, Scene.Params.contactProcessingThreshold);
+
+            // There can be special things needed for implementing linksets
+            Linkset.MakeDynamic(this);
+
+            // Force activation of the object so Bullet will act on it.
+            BulletSimAPI.Activate2(BSBody.Ptr, true);
+        }
+    }
+
+    // "Making solid" means that other object will not pass through this object.
+    private void MakeSolid(bool makeSolid)
+    {
+        if (makeSolid)
+        {
+            // Easy in Bullet -- just remove the object flag that controls collision response
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+        }
+        else
+        {
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+        }
+    }
+
+    // Turn on or off the flag controlling whether collision events are returned to the simulator.
+    private void EnableCollisions(bool wantsCollisionEvents)
+    {
+        if (wantsCollisionEvents)
+        {
+            m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
+        else
+        {
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        }
     }
 
     // prims don't fly
     public override bool Flying { 
         get { return _flying; } 
-        set { _flying = value; } 
+        set {
+            _flying = value;
+        } 
     }
     public override bool SetAlwaysRun { 
         get { return _setAlwaysRun; } 
@@ -558,8 +658,8 @@ public sealed class BSPrim : PhysicsActor
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             _scene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                // DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
+                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                BulletSimAPI.SetAngularVelocity2(BSBody.Ptr, _rotationalVelocity);
             });
         } 
     }
@@ -575,8 +675,11 @@ public sealed class BSPrim : PhysicsActor
             _buoyancy = value;
             _scene.TaintedObject("BSPrim.setBuoyancy", delegate()
             {
-                // DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
-                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+                DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                // Buoyancy is faked by changing the gravity applied to the object
+                float grav = Scene.Params.gravity * (1f - _buoyancy);
+                BulletSimAPI.SetGravity2(BSBody.Ptr, new OMV.Vector3(0f, 0f, grav));
+                // BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
             });
         } 
     }
@@ -624,7 +727,7 @@ public sealed class BSPrim : PhysicsActor
         }
         else
         {
-            m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
+            m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
         _scene.TaintedObject("BSPrim.AddForce", delegate()
@@ -638,17 +741,18 @@ public sealed class BSPrim : PhysicsActor
                 }
                 m_accumulatedForces.Clear();
             }
-            // DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, _force);
-            BulletSimAPI.AddObjectForce2(Body.Ptr, fSum);
+            DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, fSum);
+            // For unknown reasons, "ApplyCentralForce" adds this force to the total force on the object.
+            BulletSimAPI.ApplyCentralForce2(BSBody.Ptr, fSum);
         });
     }
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { 
-        // DetailLog("{0},BSPrim.AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
+        DetailLog("{0},BSPrim.AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
         // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
     }
     public override void SetMomentum(OMV.Vector3 momentum) { 
-        // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
+        DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
     }
     public override void SubscribeEvents(int ms) { 
         _subscribedEventsMs = ms;
@@ -659,7 +763,7 @@ public sealed class BSPrim : PhysicsActor
 
             Scene.TaintedObject("BSPrim.SubscribeEvents", delegate()
             {
-                BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                m_currentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
             });
         }
     }
@@ -667,7 +771,7 @@ public sealed class BSPrim : PhysicsActor
         _subscribedEventsMs = 0;
         Scene.TaintedObject("BSPrim.UnSubscribeEvents", delegate()
         {
-            BulletSimAPI.RemoveFromCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            m_currentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         });
     }
     public override bool SubscribedEvents() { 
@@ -974,39 +1078,47 @@ public sealed class BSPrim : PhysicsActor
     }// end CalculateMass
     #endregion Mass Calculation
 
-    // Create the geometry information in Bullet for later use
-    // The objects needs a hull if it's physical otherwise a mesh is enough
-    // No locking here because this is done when we know physics is not simulating
-    // if 'forceRebuild' is true, the geometry is rebuilt. Otherwise a previously built version is used
-    // Returns 'true' if the geometry was rebuilt
+    // Create the geometry information in Bullet for later use.
+    // The objects needs a hull if it's physical otherwise a mesh is enough.
+    // No locking here because this is done when we know physics is not simulating.
+    // if 'forceRebuild' is true, the geometry is rebuilt. Otherwise a previously built version is used.
+    // Returns 'true' if the geometry was rebuilt.
+    // Called at taint-time!
     private bool CreateGeom(bool forceRebuild)
     {
-        // the mesher thought this was too simple to mesh. Use a native Bullet collision shape.
         bool ret = false;
-        if (!_scene.NeedsMeshing(_pbs))
+        bool haveShape = false;
+
+        // If the prim attributes are simple, this could be a simple Bullet native shape
+        if ((_pbs.SculptEntry && !Scene.ShouldMeshSculptedPrim)
+                || (_pbs.ProfileBegin == 0 && _pbs.ProfileEnd == 0
+                    && _pbs.ProfileHollow == 0
+                    && _pbs.PathTwist == 0 && _pbs.PathTwistBegin == 0
+                    && _pbs.PathBegin == 0 && _pbs.PathEnd == 0
+                    && _pbs.PathTaperX == 0 && _pbs.PathTaperY == 0
+                    && _pbs.PathScaleX == 100 && _pbs.PathScaleY == 100
+                    && _pbs.PathShearX == 0 && _pbs.PathShearY == 0) )
         {
             if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
             {
-                // if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
-                // {
-                    // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
-                    if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
-                    {
-                        // DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
-                        _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
-                        // Bullet native objects are scaled by the Bullet engine so pass the size in
-                        _scale = _size;
-                        // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
-                        ret = true;
-                    }
-                // }
+                haveShape = true;
+                if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
+                {
+                    DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
+                    _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                    // Bullet native objects are scaled by the Bullet engine so pass the size in
+                    _scale = _size;
+                    // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
+                    ret = true;
+                }
             }
             else
             {
                 // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, type={2}, size={3}", LogHeader, LocalID, _shapeType, _size);
+                haveShape = true;
                 if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX))
                 {
-                    // DetailLog("{0},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
+                    DetailLog("{0},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                     _scale = _size;
                     // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
@@ -1014,16 +1126,16 @@ public sealed class BSPrim : PhysicsActor
                 }
             }
         }
-        else
+        // If a simple shape isn't happening, create a mesh and possibly a hull
+        if (!haveShape)
         {
             if (IsPhysical)
             {
                 if (forceRebuild || _hullKey == 0)
                 {
                     // physical objects require a hull for interaction.
-                    // This will create the mesh if it doesn't already exist
-                    CreateGeomHull();
-                    ret = true;
+                    // This also creates the mesh if it doesn't already exist
+                    ret = CreateGeomHull();
                 }
             }
             else
@@ -1031,8 +1143,7 @@ public sealed class BSPrim : PhysicsActor
                 if (forceRebuild || _meshKey == 0)
                 {
                     // Static (non-physical) objects only need a mesh for bumping into
-                    CreateGeomMesh();
-                    ret = true;
+                    ret = CreateGeomMesh();
                 }
             }
         }
@@ -1040,21 +1151,30 @@ public sealed class BSPrim : PhysicsActor
     }
 
     // No locking here because this is done when we know physics is not simulating
-    private void CreateGeomMesh()
+    // Returns 'true' of a mesh was actually rebuild (we could also have one of these specs).
+    // Called at taint-time!
+    private bool CreateGeomMesh()
     {
-        float lod = _pbs.SculptEntry ? _scene.SculptLOD : _scene.MeshLOD;
+        // level of detail based on size and type of the object
+        float lod = _scene.MeshLOD;
+        if (_pbs.SculptEntry) 
+            lod = _scene.SculptLOD;
+        float maxAxis = Math.Max(_size.X, Math.Max(_size.Y, _size.Z));
+        if (maxAxis > _scene.MeshMegaPrimThreshold) 
+            lod = _scene.MeshMegaPrimLOD;
+
         ulong newMeshKey = (ulong)_pbs.GetMeshKey(_size, lod);
         // m_log.DebugFormat("{0}: CreateGeomMesh: lID={1}, oldKey={2}, newKey={3}", LogHeader, _localID, _meshKey, newMeshKey);
 
         // if this new shape is the same as last time, don't recreate the mesh
-        if (_meshKey == newMeshKey) return;
+        if (_meshKey == newMeshKey) return false;
 
-        // DetailLog("{0},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
+        DetailLog("{0},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
         // Since we're recreating new, get rid of any previously generated shape
         if (_meshKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
-            // DetailLog("{0},BSPrim.CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
+            DetailLog("{0},BSPrim.CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;
             _meshKey = 0;
@@ -1084,27 +1204,27 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
-        // DetailLog("{0},BSPrim.CreateGeomMesh,done", LocalID);
-        return;
+        return true;
     }
 
     // No locking here because this is done when we know physics is not simulating
-    private void CreateGeomHull()
+    // Returns 'true' of a mesh was actually rebuild (we could also have one of these specs).
+    private bool CreateGeomHull()
     {
         float lod = _pbs.SculptEntry ? _scene.SculptLOD : _scene.MeshLOD;
         ulong newHullKey = (ulong)_pbs.GetMeshKey(_size, lod);
         // m_log.DebugFormat("{0}: CreateGeomHull: lID={1}, oldKey={2}, newKey={3}", LogHeader, _localID, _hullKey, newHullKey);
 
         // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == _hullKey) return;
+        if (newHullKey == _hullKey) return false;
 
-        // DetailLog("{0},BSPrim.CreateGeomHull,create,oldKey={1},newKey={2}", LocalID, _hullKey, newHullKey);
+        DetailLog("{0},BSPrim.CreateGeomHull,create,oldKey={1},newKey={2}", LocalID, _hullKey, newHullKey);
         
         // Since we're recreating new, get rid of any previously generated shape
         if (_hullKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey);
-            // DetailLog("{0},BSPrim.CreateGeomHull,deleteOldHull,key={1}", LocalID, _hullKey);
+            DetailLog("{0},BSPrim.CreateGeomHull,deleteOldHull,key={1}", LocalID, _hullKey);
             BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
             _hullKey = 0;
         }
@@ -1198,8 +1318,8 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
-        // DetailLog("{0},BSPrim.CreateGeomHull,done", LocalID);
-        return;
+        DetailLog("{0},BSPrim.CreateGeomHull,done", LocalID);
+        return true;
     }
 
     // Callback from convex hull creater with a newly created hull.
@@ -1210,6 +1330,19 @@ public sealed class BSPrim : PhysicsActor
         return;
     }
 
+    private void VerifyCorrectPhysicalShape()
+    {
+        if (!IsStatic)
+        {
+            // if not static, it will need a hull to efficiently collide with things
+            if (_hullKey == 0)
+            {
+                CreateGeomAndObject(false);
+            }
+
+        }
+    }
+
     // Create an object in Bullet if it has not already been created
     // No locking here because this is done when the physics engine is not simulating
     // Returns 'true' if an object was actually created.
@@ -1223,8 +1356,9 @@ public sealed class BSPrim : PhysicsActor
         // m_log.DebugFormat("{0}: CreateObject: lID={1}, shape={2}", LogHeader, _localID, shape.Type);
         bool ret = BulletSimAPI.CreateObject(_scene.WorldID, shape);
 
-        // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
-        Body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+        // the CreateObject() may have recreated the rigid body. Make sure we have the latest address.
+        BSBody = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+        BSShape = new BulletShape(BulletSimAPI.GetCollisionShape2(BSBody.Ptr));
 
         return ret;
     }
@@ -1248,15 +1382,20 @@ public sealed class BSPrim : PhysicsActor
         shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue;
     }
 
-
     // Rebuild the geometry and object.
     // This is called when the shape changes so we need to recreate the mesh/hull.
     // No locking here because this is done when the physics engine is not simulating
-    private void RecreateGeomAndObject()
+    private void CreateGeomAndObject(bool forceRebuild)
     {
-        // m_log.DebugFormat("{0}: RecreateGeomAndObject. lID={1}", LogHeader, _localID);
-        if (CreateGeom(true))
+        // m_log.DebugFormat("{0}: CreateGeomAndObject. lID={1}, force={2}", LogHeader, _localID, forceRebuild);
+        // Create the geometry that will make up the object
+        if (CreateGeom(forceRebuild))
+        {
+            // Create the object and place it into the world
             CreateObject();
+            // Make sure the properties are set on the new object
+            UpdatePhysicalParameters();
+        }
         return;
     }
 
@@ -1277,7 +1416,7 @@ public sealed class BSPrim : PhysicsActor
     const float ACCELERATION_TOLERANCE = 0.01f;
     const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f;
 
-    public void UpdateProperties(EntityProperties entprop)
+    public override void UpdateProperties(EntityProperties entprop)
     {
         /*
         UpdatedProperties changed = 0;
@@ -1325,7 +1464,7 @@ public sealed class BSPrim : PhysicsActor
         // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
 
         // Updates only for individual prims and for the root object of a linkset.
-        if (_linkset.IsRoot(this))
+        if (Linkset.IsRoot(this))
         {
             // Assign to the local variables so the normal set action does not happen
             _position = entprop.Position;
@@ -1334,17 +1473,17 @@ public sealed class BSPrim : PhysicsActor
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
-            // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
-            //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
-            // DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-            //         LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+
+            // BulletSimAPI.DumpRigidBody2(Scene.World.Ptr, BSBody.Ptr);
 
             base.RequestPhysicsterseUpdate();
         }
             /*
         else
         {
-            // For debugging, we also report the movement of children
+            // For debugging, we can also report the movement of children
             DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                     entprop.Acceleration, entprop.RotationalVelocity);
@@ -1353,36 +1492,45 @@ public sealed class BSPrim : PhysicsActor
     }
 
     // I've collided with something
+    // Called at taint time from within the Step() function
     CollisionEventUpdate collisionCollection;
-    public void Collide(uint collidingWith, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
+    public override bool Collide(uint collidingWith, BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
-        // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith);
+        bool ret = false;
 
         // The following lines make IsColliding() and IsCollidingGround() work
-        _collidingStep = _scene.SimulationStep;
-        if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID)
+        _collidingStep = Scene.SimulationStep;
+        if (collidingWith <= Scene.TerrainManager.HighestTerrainID)
         {
-            _collidingGroundStep = _scene.SimulationStep;
+            _collidingGroundStep = Scene.SimulationStep;
         }
 
         // DetailLog("{0},BSPrim.Collison,call,with={1}", LocalID, collidingWith);
 
-        // if someone is subscribed to collision events....
-        if (_subscribedEventsMs != 0) {
+        // prims in the same linkset cannot collide with each other
+        if (collidee != null && (this.Linkset.LinksetID == collidee.Linkset.LinksetID))
+        {
+            return ret;
+        }
+
+        // if someone has subscribed for collision events....
+        if (SubscribedEvents()) {
             // throttle the collisions to the number of milliseconds specified in the subscription
-            int nowTime = _scene.SimulationNowTime;
+            int nowTime = Scene.SimulationNowTime;
             if (nowTime >= _nextCollisionOkTime) {
                 _nextCollisionOkTime = nowTime + _subscribedEventsMs;
 
                 if (collisionCollection == null)
                     collisionCollection = new CollisionEventUpdate();
                 collisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+                ret = true;
             }
         }
+        return ret;
     }
 
     // The scene is telling us it's time to pass our collected collisions into the simulator
-    public void SendCollisions()
+    public override void SendCollisions()
     {
         if (collisionCollection != null && collisionCollection.Count > 0)
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index a31c578..52997dd 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -39,8 +39,6 @@ using log4net;
 using OpenMetaverse;
 
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
-// Debug linkset 
-// Test with multiple regions in one simulator 
 // Adjust character capsule size when height is adjusted (ScenePresence.SetHeight)
 // Test sculpties
 // Compute physics FPS reasonably
@@ -54,13 +52,10 @@ using OpenMetaverse;
 // Use collision masks for collision with terrain and phantom objects 
 // Check out llVolumeDetect. Must do something for that.
 // Should prim.link() and prim.delink() membership checking happen at taint time?
-// changing the position and orientation of a linked prim must rebuild the constraint with the root.
 // 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 the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions)
 // Implement LockAngularMotion
 // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
-// Does NeedsMeshing() really need to exclude all the different shapes?
 // 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?
@@ -73,62 +68,56 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS SCENE]";
 
-    public void DebugLog(string mm, params Object[] xx) { if (ShouldDebugLog) m_log.DebugFormat(mm, xx); }
+    // The name of the region we're working for.
+    public string RegionName { get; private set; }
 
     public string BulletSimVersion = "?";
 
-    private Dictionary<uint, BSCharacter> m_avatars = new Dictionary<uint, BSCharacter>();
-    private Dictionary<uint, BSPrim> m_prims = new Dictionary<uint, BSPrim>();
-    private HashSet<BSCharacter> m_avatarsWithCollisions = new HashSet<BSCharacter>();
-    private HashSet<BSPrim> m_primsWithCollisions = new HashSet<BSPrim>();
-    private List<BSPrim> m_vehicles = new List<BSPrim>();
-    private float[] m_heightMap;
-    private float m_waterLevel;
-    private uint m_worldID;
-    public uint WorldID { get { return m_worldID; } }
+    public Dictionary<uint, BSPhysObject> PhysObjects = new Dictionary<uint, BSPhysObject>();
+
+    private HashSet<BSPhysObject> m_objectsWithCollisions = new HashSet<BSPhysObject>();
+    // Following is a kludge and can  be removed when avatar animation updating is
+    //    moved to a better place.
+    private HashSet<BSPhysObject> m_avatarsWithCollisions = new HashSet<BSPhysObject>();
+
+    // List of all the objects that have vehicle properties and should be called
+    //    to update each physics step.
+    private List<BSPhysObject> m_vehicles = new List<BSPhysObject>();
 
     // let my minuions use my logger
     public ILog Logger { get { return m_log; } }
 
-    private bool m_initialized = false;
-
+    // If non-zero, the number of simulation steps between calls to the physics
+    //    engine to output detailed physics stats. Debug logging level must be on also.
     private int m_detailedStatsStep = 0;
 
     public IMesher mesher;
-    private float m_meshLOD;
-    public float MeshLOD
-    {
-        get { return m_meshLOD; }
-    }
-    private float m_sculptLOD;
-    public float SculptLOD
-    {
-        get { return m_sculptLOD; }
-    }
+    // Level of Detail values kept as float because that's what the Meshmerizer wants
+    public float MeshLOD { get; private set; }
+    public float MeshMegaPrimLOD { get; private set; }
+    public float MeshMegaPrimThreshold { get; private set; }
+    public float SculptLOD { get; private set; }
 
-    private BulletSim m_worldSim;
-    public BulletSim World
-    {
-        get { return m_worldSim; }
-    }
-    private BSConstraintCollection m_constraintCollection;
-    public BSConstraintCollection Constraints
-    { 
-        get { return m_constraintCollection; }
-    }
+    public uint WorldID { get; private set; }
+    public BulletSim World { get; private set; }
 
+    // All the constraints that have been allocated in this instance.
+    public BSConstraintCollection Constraints { get; private set; }
+
+    // Simulation parameters
     private int m_maxSubSteps;
     private float m_fixedTimeStep;
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
 
-    public float LastSimulatedTimestep { get; private set; }
-
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
-    private int m_simulationNowTime;
-    public int SimulationNowTime { get { return m_simulationNowTime; } }
+    public int SimulationNowTime { get; private set; }
+
+    // True if initialized and ready to do simulation steps
+    private bool m_initialized = false;
 
+    // Pinned memory used to pass step information between managed and unmanaged
     private int m_maxCollisionsPerFrame;
     private CollisionDesc[] m_collisionArray;
     private GCHandle m_collisionArrayPinnedHandle;
@@ -137,14 +126,18 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private EntityProperties[] m_updateArray;
     private GCHandle m_updateArrayPinnedHandle;
 
-    private bool _meshSculptedPrim = true;         // cause scuplted prims to get meshed
-    private bool _forceSimplePrimMeshing = false;   // if a cube or sphere, let Bullet do internal shapes
+    public bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
+    public bool ShouldForceSimplePrimMeshing { get; private set; }   // if a cube or sphere, let Bullet do internal shapes
 
     public float PID_D { get; private set; }    // derivative
     public float PID_P { get; private set; }    // proportional
 
     public const uint TERRAIN_ID = 0;       // OpenSim senses terrain with a localID of zero
     public const uint GROUNDPLANE_ID = 1;
+    public const uint CHILDTERRAIN_ID = 2;  // Terrain allocated based on our mega-prim childre start here
+
+    private float m_waterLevel;
+    public BSTerrainManager TerrainManager { get; private set; }
 
     public ConfigurationParameters Params
     {
@@ -154,13 +147,18 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         get { return new Vector3(0f, 0f, Params.gravity); }
     }
-
-    private float m_maximumObjectMass;
-    public float MaximumObjectMass
+    // Just the Z value of the gravity
+    public float DefaultGravityZ
     {
-        get { return m_maximumObjectMass; }
+        get { return Params.gravity; }
     }
 
+    public float MaximumObjectMass { get; private set; }
+
+    // When functions in the unmanaged code must be called, it is only
+    //   done at a known time just before the simulation step. The taint
+    //   system saves all these function calls and executes them in
+    //   order before the simulation.
     public delegate void TaintCallback();
     private struct TaintCallbackEntry
     {
@@ -172,15 +170,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             callback = c;
         }
     }
+    private Object _taintLock = new Object();   // lock for using the next object
     private List<TaintCallbackEntry> _taintedObjects;
-    private Object _taintLock = new Object();
 
     // A pointer to an instance if this structure is passed to the C++ code
+    // Used to pass basic configuration values to the unmanaged code.
     ConfigurationParameters[] m_params;
     GCHandle m_paramsHandle;
 
-    public bool ShouldDebugLog { get; private set; }
-
+    // Handle to the callback used by the unmanaged code to call into the managed code.
+    // Used for debug logging.
+    // Need to store the handle in a persistant variable so it won't be freed.
     private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
 
     // Sometimes you just have to log everything.
@@ -189,13 +189,15 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private string m_physicsLoggingDir;
     private string m_physicsLoggingPrefix;
     private int m_physicsLoggingFileMinutes;
+    // 'true' of the vehicle code is to log lots of details
+    public bool VehicleLoggingEnabled { get; private set; }
 
-    private bool m_vehicleLoggingEnabled;
-    public bool VehicleLoggingEnabled { get { return m_vehicleLoggingEnabled; } }
-
+    #region Construction and Initialization
     public BSScene(string identifier)
     {
         m_initialized = false;
+        // we are passed the name of the region we're working for.
+        RegionName = identifier;
     }
 
     public override void Initialise(IMesher meshmerizer, IConfigSource config)
@@ -213,6 +215,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
         m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
 
+        mesher = meshmerizer;
+        _taintedObjects = new List<TaintCallbackEntry>();
+
         // Enable very detailed logging.
         // By creating an empty logger when not logging, the log message invocation code
         // can be left in and every call doesn't have to check for null.
@@ -225,38 +230,43 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             PhysicsLogging = new Logging.LogWriter();
         }
 
-        // Get the version of the DLL
-        // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
-        // BulletSimVersion = BulletSimAPI.GetVersion();
-        // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
-
-        // if Debug, enable logging from the unmanaged code
+        // If Debug logging level, enable logging from the unmanaged code
+        m_DebugLogCallbackHandle = null;
         if (m_log.IsDebugEnabled || PhysicsLogging.Enabled)
         {
             m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader);
             if (PhysicsLogging.Enabled)
+                // The handle is saved in a variable to make sure it doesn't get freed after this call
                 m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLoggerPhysLog);
             else
                 m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
-            // the handle is saved in a variable to make sure it doesn't get freed after this call
-            BulletSimAPI.SetDebugLogCallback(m_DebugLogCallbackHandle);
         }
 
-        _taintedObjects = new List<TaintCallbackEntry>();
+        // Get the version of the DLL
+        // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
+        // BulletSimVersion = BulletSimAPI.GetVersion();
+        // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
 
-        mesher = meshmerizer;
-        // The bounding box for the simulated world
+        // The bounding box for the simulated world. The origin is 0,0,0 unless we're
+        //    a child in a mega-region.
+        // Turns out that Bullet really doesn't care about the extents of the simulated
+        //    area. It tracks active objects no matter where they are.
         Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, 8192f);
 
         // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader);
-        m_worldID = BulletSimAPI.Initialize(worldExtent, m_paramsHandle.AddrOfPinnedObject(),
+        WorldID = BulletSimAPI.Initialize(worldExtent, m_paramsHandle.AddrOfPinnedObject(),
                                         m_maxCollisionsPerFrame, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
-                                        m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject());
+                                        m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject(),
+                                        m_DebugLogCallbackHandle);
 
         // Initialization to support the transition to a new API which puts most of the logic
         //   into the C# code so it is easier to modify and add to.
-        m_worldSim = new BulletSim(m_worldID, this, BulletSimAPI.GetSimHandle2(m_worldID));
-        m_constraintCollection = new BSConstraintCollection(World);
+        World = new BulletSim(WorldID, this, BulletSimAPI.GetSimHandle2(WorldID));
+
+        Constraints = new BSConstraintCollection(World);
+
+        TerrainManager = new BSTerrainManager(this);
+        TerrainManager.CreateInitialGroundPlaneAndTerrain();
 
         m_initialized = true;
     }
@@ -281,10 +291,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 // Very detailed logging for physics debugging
                 m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
                 m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
-                m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-");
+                m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-%REGIONNAME%-");
                 m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
                 // Very detailed logging for vehicle debugging
-                m_vehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
+                VehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
+
+                // Do any replacements in the parameters
+                m_physicsLoggingPrefix = m_physicsLoggingPrefix.Replace("%REGIONNAME%", RegionName);
             }
         }
     }
@@ -316,6 +329,38 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         PhysicsLogging.Write("[BULLETS UNMANAGED]:" + msg);
     }
 
+    public override void Dispose()
+    {
+        // m_log.DebugFormat("{0}: Dispose()", LogHeader);
+
+        // make sure no stepping happens while we're deleting stuff
+        m_initialized = false;
+
+        TerrainManager.ReleaseGroundPlaneAndTerrain();
+
+        foreach (KeyValuePair<uint, BSPhysObject> kvp in PhysObjects)
+        {
+            kvp.Value.Destroy();
+        }
+        PhysObjects.Clear();
+
+        // Now that the prims are all cleaned up, there should be no constraints left
+        if (Constraints != null)
+        {
+            Constraints.Dispose();
+            Constraints = null;
+        }
+
+        // Anything left in the unmanaged code should be cleaned out
+        BulletSimAPI.Shutdown(WorldID);
+
+        // Not logging any more
+        PhysicsLogging.Close();
+    }
+    #endregion // Construction and Initialization
+
+    #region Prim and Avatar addition and removal
+
     public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
     {
         m_log.ErrorFormat("{0}: CALL TO AddAvatar in BSScene. NOT IMPLEMENTED", LogHeader);
@@ -329,7 +374,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (!m_initialized) return null;
 
         BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying);
-        lock (m_avatars) m_avatars.Add(localID, actor);
+        lock (PhysObjects) PhysObjects.Add(localID, actor);
+
+        // TODO: Remove kludge someday.
+        // We must generate a collision for avatars whether they collide or not.
+        // This is required by OpenSim to update avatar animations, etc.
+        lock (m_avatarsWithCollisions) m_avatarsWithCollisions.Add(actor);
+
         return actor;
     }
 
@@ -344,7 +395,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         {
             try
             {
-                lock (m_avatars) m_avatars.Remove(actor.LocalID);
+                lock (PhysObjects) PhysObjects.Remove(actor.LocalID);
+                // Remove kludge someday
+                lock (m_avatarsWithCollisions) m_avatarsWithCollisions.Remove(bsactor);
             }
             catch (Exception e)
             {
@@ -362,11 +415,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPrim bsprim = prim as BSPrim;
         if (bsprim != null)
         {
-            // DetailLog("{0},RemovePrim,call", bsprim.LocalID);
+            DetailLog("{0},RemovePrim,call", bsprim.LocalID);
             // m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
             try
             {
-                lock (m_prims) m_prims.Remove(bsprim.LocalID);
+                lock (PhysObjects) PhysObjects.Remove(bsprim.LocalID);
             }
             catch (Exception e)
             {
@@ -388,10 +441,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         if (!m_initialized) return null;
 
-        // DetailLog("{0},AddPrimShape,call", localID);
+        DetailLog("{0},AddPrimShape,call", localID);
 
         BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
-        lock (m_prims) m_prims.Add(localID, prim);
+        lock (PhysObjects) PhysObjects.Add(localID, prim);
         return prim;
     }
 
@@ -400,6 +453,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // information call is not needed.
     public override void AddPhysicsActorTaint(PhysicsActor prim) { }
 
+    #endregion // Prim and Avatar addition and removal
+
+    #region Simulation
     // Simulate one timestep
     public override float Simulate(float timeStep)
     {
@@ -408,18 +464,16 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         int collidersCount = 0;
         IntPtr collidersPtr;
 
-        LastSimulatedTimestep = timeStep;
-
         // prevent simulation until we've been initialized
-        if (!m_initialized) return 10.0f;
-
-        int simulateStartTime = Util.EnvironmentTickCount();
+        if (!m_initialized) return 5.0f;
 
         // update the prim states while we know the physics engine is not busy
+        int numTaints = _taintedObjects.Count;
         ProcessTaints();
 
         // Some of the prims operate with special vehicle properties
         ProcessVehicles(timeStep);
+        numTaints += _taintedObjects.Count;
         ProcessTaints();    // the vehicles might have added taints
 
         // step the physical world one interval
@@ -427,15 +481,18 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         int numSubSteps = 0;
         try
         {
-            numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep,
+            numSubSteps = BulletSimAPI.PhysicsStep(WorldID, timeStep, m_maxSubSteps, m_fixedTimeStep,
                         out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
-            // DetailLog("{0},Simulate,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount); 
+            DetailLog("{0},Simulate,call, nTaints= {1}, substeps={2}, updates={3}, colliders={4}", 
+                        DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount); 
         }
         catch (Exception e)
         {
-            m_log.WarnFormat("{0},PhysicsStep Exception: substeps={1}, updates={2}, colliders={3}, e={4}", LogHeader, numSubSteps, updatedEntityCount, collidersCount, e);
-            // DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount);
-            // updatedEntityCount = 0;
+            m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}", 
+                        LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e);
+            DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}", 
+                        DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount);
+            updatedEntityCount = 0;
             collidersCount = 0;
         }
 
@@ -443,7 +500,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // 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
-        m_simulationNowTime = Util.EnvironmentTickCount();
+        SimulationNowTime = Util.EnvironmentTickCount();
+
+        // This is a kludge to get avatar movement updates. 
+        //   ODE sends collisions for avatars even if there are have been no collisions. This updates
+        //   avatar animations and stuff.
+        // If you fix avatar animation updates, remove this overhead and let normal collision processing happen.
+        m_objectsWithCollisions = new HashSet<BSPhysObject>(m_avatarsWithCollisions);
 
         // If there were collisions, process them by sending the event to the prim.
         // Collisions must be processed before updates.
@@ -462,19 +525,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // The above SendCollision's batch up the collisions on the objects.
         //      Now push the collisions into the simulator.
-        foreach (BSPrim bsp in m_primsWithCollisions)
+        foreach (BSPhysObject bsp in m_objectsWithCollisions)
             bsp.SendCollisions();
-        m_primsWithCollisions.Clear();
-
-        // This is a kludge to get avatar movement updated. 
-        //   Don't send collisions only if there were collisions -- send everytime.
-        //   ODE sends collisions even if there are none and this is used to update
-        //   avatar animations and stuff.
-        // foreach (BSCharacter bsc in m_avatarsWithCollisions)
-        //     bsc.SendCollisions();
-        foreach (KeyValuePair<uint, BSCharacter> kvp in m_avatars)
-            kvp.Value.SendCollisions();
-        m_avatarsWithCollisions.Clear();
+        m_objectsWithCollisions.Clear();
 
         // If any of the objects had updated properties, tell the object it has been changed by the physics engine
         if (updatedEntityCount > 0)
@@ -482,17 +535,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             for (int ii = 0; ii < updatedEntityCount; ii++)
             {
                 EntityProperties entprop = m_updateArray[ii];
-                BSPrim prim;
-                if (m_prims.TryGetValue(entprop.ID, out prim))
-                {
-                    prim.UpdateProperties(entprop);
-                    continue;
-                }
-                BSCharacter actor;
-                if (m_avatars.TryGetValue(entprop.ID, out actor))
+                BSPhysObject pobj;
+                if (PhysObjects.TryGetValue(entprop.ID, out pobj))
                 {
-                    actor.UpdateProperties(entprop);
-                    continue;
+                    pobj.UpdateProperties(entprop);
                 }
             }
         }
@@ -506,79 +552,59 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        // this is a waste since the outside routine also calcuates the physics simulation
-        //   period. TODO: There should be a way of computing physics frames from simulator computation.
-        // long simulateTotalTime = Util.EnvironmentTickCountSubtract(simulateStartTime);
-        // return (timeStep * (float)simulateTotalTime);
-
-        // TODO: FIX THIS: fps calculation possibly wrong.
-        // This calculation says 1/timeStep is the ideal frame rate. Any time added to
-        //    that by the physics simulation gives a slower frame rate.
-        long totalSimulationTime = Util.EnvironmentTickCountSubtract(simulateStartTime);
-        if (totalSimulationTime >= timeStep)
-            return 0;
-        return 1f / (timeStep + totalSimulationTime);
+        // 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.
+        // Since Bullet normally does 5 or 6 substeps, this will normally sum to about 60 FPS.
+        return numSubSteps * m_fixedTimeStep;
     }
 
     // Something has collided
-    private void SendCollision(uint localID, uint collidingWith, Vector3 collidePoint, Vector3 collideNormal, float penitration)
+    private void SendCollision(uint localID, uint collidingWith, Vector3 collidePoint, Vector3 collideNormal, float penetration)
     {
-        if (localID == TERRAIN_ID || localID == GROUNDPLANE_ID)
+        if (localID <= TerrainManager.HighestTerrainID)
         {
             return;         // don't send collisions to the terrain
         }
 
-        ActorTypes type = ActorTypes.Prim;
-        if (collidingWith == TERRAIN_ID || collidingWith == GROUNDPLANE_ID)
-            type = ActorTypes.Ground;
-        else if (m_avatars.ContainsKey(collidingWith))
-            type = ActorTypes.Agent;
-
-        BSPrim prim;
-        if (m_prims.TryGetValue(localID, out prim)) {
-            prim.Collide(collidingWith, type, collidePoint, collideNormal, penitration);
-            m_primsWithCollisions.Add(prim);
+        BSPhysObject collider;
+        if (!PhysObjects.TryGetValue(localID, out collider))
+        {
+            // If the object that is colliding cannot be found, just ignore the collision.
             return;
         }
-        BSCharacter actor;
-        if (m_avatars.TryGetValue(localID, out actor)) {
-            actor.Collide(collidingWith, type, collidePoint, collideNormal, penitration);
-            m_avatarsWithCollisions.Add(actor);
-            return;
+
+        // The terrain is not in the physical object list so 'collidee'
+        //    can be null when Collide() is called.
+        BSPhysObject collidee = null;
+        PhysObjects.TryGetValue(collidingWith, out collidee);
+
+        // DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
+
+        if (collider.Collide(collidingWith, collidee, collidePoint, collideNormal, penetration))
+        {
+            // If a collision was posted, remember to send it to the simulator
+            m_objectsWithCollisions.Add(collider);
         }
+
         return;
     }
 
-    public override void GetResults() { }
+    #endregion // Simulation
 
-    public override void SetTerrain(float[] heightMap) {
-        m_heightMap = heightMap;
-        this.TaintedObject("BSScene.SetTerrain", delegate()
-        {
-            BulletSimAPI.SetHeightmap(m_worldID, m_heightMap);
-        });
-    }
+    public override void GetResults() { }
 
-    // Someday we will have complex terrain with caves and tunnels
-    // For the moment, it's flat and convex
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
-    {
-        return GetTerrainHeightAtXY(loc.X, loc.Y);
-    }
+    #region Terrain
 
-    public float GetTerrainHeightAtXY(float tX, float tY)
-    {
-        if (tX < 0 || tX >= Constants.RegionSize || tY < 0 || tY >= Constants.RegionSize)
-            return 30;
-        return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)];
+    public override void SetTerrain(float[] heightMap) {
+        TerrainManager.SetTerrain(heightMap);
     }
 
     public override void SetWaterLevel(float baseheight) 
     {
         m_waterLevel = baseheight;
-        // TODO: pass to physics engine so things will float?
     }
-    public float GetWaterLevel()
+    // Someday....
+    public float GetWaterLevelAtXYZ(Vector3 loc)
     {
         return m_waterLevel;
     }
@@ -588,39 +614,27 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // m_log.DebugFormat("{0}: DeleteTerrain()", LogHeader);
     }
 
-    public override void Dispose()
+    // Although no one seems to check this, I do support combining.
+    public override bool SupportsCombining()
     {
-        // m_log.DebugFormat("{0}: Dispose()", LogHeader);
-
-        // make sure no stepping happens while we're deleting stuff
-        m_initialized = false;
-
-        foreach (KeyValuePair<uint, BSCharacter> kvp in m_avatars)
-        {
-            kvp.Value.Destroy();
-        }
-        m_avatars.Clear();
-
-        foreach (KeyValuePair<uint, BSPrim> kvp in m_prims)
-        {
-            kvp.Value.Destroy();
-        }
-        m_prims.Clear();
-
-        // Now that the prims are all cleaned up, there should be no constraints left
-        if (m_constraintCollection != null)
-        {
-            m_constraintCollection.Dispose();
-            m_constraintCollection = null;
-        }
-
-        // Anything left in the unmanaged code should be cleaned out
-        BulletSimAPI.Shutdown(WorldID);
+        return TerrainManager.SupportsCombining();
+    }
+    // This call says I am a child to region zero in a mega-region. 'pScene' is that
+    //    of region zero, 'offset' is my offset from regions zero's origin, and
+    //    'extents' is the largest XY that is handled in my region.
+    public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+    {
+        TerrainManager.Combine(pScene, offset, extents);
+    }
 
-        // Not logging any more
-        PhysicsLogging.Close();
+    // Unhook all the combining that I know about.
+    public override void UnCombine(PhysicsScene pScene)
+    {
+        TerrainManager.UnCombine(pScene);
     }
 
+    #endregion // Terrain
+
     public override Dictionary<uint, float> GetTopColliders()
     {
         return new Dictionary<uint, float>();
@@ -628,121 +642,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public override bool IsThreaded { get { return false;  } }
 
-    /// <summary>
-    /// Routine to figure out if we need to mesh this prim with our mesher
-    /// </summary>
-    /// <param name="pbs"></param>
-    /// <returns>true if the prim needs meshing</returns>
-    public bool NeedsMeshing(PrimitiveBaseShape pbs)
-    {
-        // most of this is redundant now as the mesher will return null if it cant mesh a prim
-        // but we still need to check for sculptie meshing being enabled so this is the most
-        // convenient place to do it for now...
-
-        // int iPropertiesNotSupportedDefault = 0;
-
-        if (pbs.SculptEntry && !_meshSculptedPrim)
-        {
-            // Render sculpties as boxes
-            return false;
-        }
-
-        // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since Bullet 
-        // can use an internal representation for the prim
-        if (!_forceSimplePrimMeshing)
-        {
-            if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
-                || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
-                        && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
-            {
-
-                if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
-                    && pbs.ProfileHollow == 0
-                    && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
-                    && pbs.PathBegin == 0 && pbs.PathEnd == 0
-                    && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
-                    && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
-                    && pbs.PathShearX == 0 && pbs.PathShearY == 0)
-                {
-                    return false;
-                }
-            }
-        }
-
-        /*  TODO: verify that the mesher will now do all these shapes
-        if (pbs.ProfileHollow != 0)
-            iPropertiesNotSupportedDefault++;
-
-        if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
-            iPropertiesNotSupportedDefault++;
-
-        if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
-            iPropertiesNotSupportedDefault++; 
-
-        if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
-            iPropertiesNotSupportedDefault++;
-
-        if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
-            iPropertiesNotSupportedDefault++;
-
-        if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
-            iPropertiesNotSupportedDefault++;
-
-        if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
-            iPropertiesNotSupportedDefault++;
-
-        if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
-            iPropertiesNotSupportedDefault++;
-
-        if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
-            iPropertiesNotSupportedDefault++;
-
-        // test for torus
-        if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
-        {
-            if (pbs.PathCurve == (byte)Extrusion.Curve1)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-        }
-        else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
-        {
-            if (pbs.PathCurve == (byte)Extrusion.Straight)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-            // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
-            else if (pbs.PathCurve == (byte)Extrusion.Curve1)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-        }
-        else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
-        {
-            if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-        }
-        else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
-        {
-            if (pbs.PathCurve == (byte)Extrusion.Straight)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-            else if (pbs.PathCurve == (byte)Extrusion.Curve1)
-            {
-                iPropertiesNotSupportedDefault++;
-            }
-        }
-        if (iPropertiesNotSupportedDefault == 0)
-        {
-            return false;
-        }
-         */
-        return true; 
-    }
-
     // Calls to the PhysicsActors can't directly call into the physics engine
     // 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.
@@ -751,7 +650,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (!m_initialized) return;
 
         lock (_taintLock)
+        {
             _taintedObjects.Add(new TaintCallbackEntry(ident, callback));
+        }
+
         return;
     }
 
@@ -830,14 +732,14 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // no locking because only called when physics engine is not busy
     private void ProcessVehicles(float timeStep)
     {
-        foreach (BSPrim prim in m_vehicles)
+        foreach (BSPhysObject pobj in m_vehicles)
         {
-            prim.StepVehicle(timeStep);
+            pobj.StepVehicle(timeStep);
         }
     }
     #endregion Vehicles
 
-    #region Parameters
+    #region INI and command line parameter processing
 
     delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
     delegate float ParamGet(BSScene scene);
@@ -888,25 +790,35 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
             ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s._meshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s._meshSculptedPrim); },
-            (s,p,l,v) => { s._meshSculptedPrim = s.BoolNumeric(v); } ),
+            (s,cf,p,v) => { s.ShouldMeshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldMeshSculptedPrim); },
+            (s,p,l,v) => { s.ShouldMeshSculptedPrim = s.BoolNumeric(v); } ),
         new ParameterDefn("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
             ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s._forceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s._forceSimplePrimMeshing); },
-            (s,p,l,v) => { s._forceSimplePrimMeshing = s.BoolNumeric(v); } ),
+            (s,cf,p,v) => { s.ShouldForceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldForceSimplePrimMeshing); },
+            (s,p,l,v) => { s.ShouldForceSimplePrimMeshing = s.BoolNumeric(v); } ),
 
-        new ParameterDefn("MeshLOD", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
+        new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
             8f,
-            (s,cf,p,v) => { s.m_meshLOD = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_meshLOD; },
-            (s,p,l,v) => { s.m_meshLOD = (int)v; } ),
-        new ParameterDefn("SculptLOD", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
+            (s,cf,p,v) => { s.MeshLOD = (float)cf.GetInt(p, (int)v); },
+            (s) => { return s.MeshLOD; },
+            (s,p,l,v) => { s.MeshLOD = v; } ),
+        new ParameterDefn("MeshLevelOfDetailMegaPrim", "Level of detail to render meshes larger than threshold meters",
+            16f,
+            (s,cf,p,v) => { s.MeshMegaPrimLOD = (float)cf.GetInt(p, (int)v); },
+            (s) => { return s.MeshMegaPrimLOD; },
+            (s,p,l,v) => { s.MeshMegaPrimLOD = v; } ),
+        new ParameterDefn("MeshLevelOfDetailMegaPrimThreshold", "Size (in meters) of a mesh before using MeshMegaPrimLOD",
+            10f,
+            (s,cf,p,v) => { s.MeshMegaPrimThreshold = (float)cf.GetInt(p, (int)v); },
+            (s) => { return s.MeshMegaPrimThreshold; },
+            (s,p,l,v) => { s.MeshMegaPrimThreshold = v; } ),
+        new ParameterDefn("SculptLevelOfDetail", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
             32f,
-            (s,cf,p,v) => { s.m_sculptLOD = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_sculptLOD; },
-            (s,p,l,v) => { s.m_sculptLOD = (int)v; } ),
+            (s,cf,p,v) => { s.SculptLOD = (float)cf.GetInt(p, (int)v); },
+            (s) => { return s.SculptLOD; },
+            (s,p,l,v) => { s.SculptLOD = v; } ),
 
         new ParameterDefn("MaxSubStep", "In simulation step, maximum number of substeps",
             10f,
@@ -930,9 +842,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
         new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)",
             10000.01f,
-            (s,cf,p,v) => { s.m_maximumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)s.m_maximumObjectMass; },
-            (s,p,l,v) => { s.m_maximumObjectMass = v; } ),
+            (s,cf,p,v) => { s.MaximumObjectMass = cf.GetFloat(p, v); },
+            (s) => { return (float)s.MaximumObjectMass; },
+            (s,p,l,v) => { s.MaximumObjectMass = v; } ),
 
         new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
             2200f,
@@ -976,42 +888,42 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             0f,
             (s,cf,p,v) => { s.m_params[0].linearDamping = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linearDamping; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].linearDamping, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].linearDamping, p, l, v); } ),
         new ParameterDefn("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
             0f,
             (s,cf,p,v) => { s.m_params[0].angularDamping = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].angularDamping; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].angularDamping, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].angularDamping, p, l, v); } ),
         new ParameterDefn("DeactivationTime", "Seconds before considering an object potentially static",
             0.2f,
             (s,cf,p,v) => { s.m_params[0].deactivationTime = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].deactivationTime; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].deactivationTime, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].deactivationTime, p, l, v); } ),
         new ParameterDefn("LinearSleepingThreshold", "Seconds to measure linear movement before considering static",
             0.8f,
             (s,cf,p,v) => { s.m_params[0].linearSleepingThreshold = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linearSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].linearSleepingThreshold, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].linearSleepingThreshold, p, l, v); } ),
         new ParameterDefn("AngularSleepingThreshold", "Seconds to measure angular movement before considering static",
             1.0f,
             (s,cf,p,v) => { s.m_params[0].angularSleepingThreshold = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].angularSleepingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].angularSleepingThreshold, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].angularSleepingThreshold, p, l, v); } ),
         new ParameterDefn("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ,
             0f,     // set to zero to disable
             (s,cf,p,v) => { s.m_params[0].ccdMotionThreshold = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].ccdMotionThreshold; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].ccdMotionThreshold, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].ccdMotionThreshold, p, l, v); } ),
         new ParameterDefn("CcdSweptSphereRadius", "Continuious collision detection test radius" ,
             0f,
             (s,cf,p,v) => { s.m_params[0].ccdSweptSphereRadius = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].ccdSweptSphereRadius; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].ccdSweptSphereRadius, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].ccdSweptSphereRadius, p, l, v); } ),
         new ParameterDefn("ContactProcessingThreshold", "Distance between contacts before doing collision check" ,
             0.1f,
             (s,cf,p,v) => { s.m_params[0].contactProcessingThreshold = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].contactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].contactProcessingThreshold, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].contactProcessingThreshold, p, l, v); } ),
 
         new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
             0.5f,
@@ -1029,35 +941,35 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].terrainRestitution; },
             (s,p,l,v) => { s.m_params[0].terrainRestitution = v; s.TaintedUpdateParameter(p,l,v); } ),
         new ParameterDefn("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
-            0.5f,
+            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.UpdateParameterAvatars(ref s.m_params[0].avatarFriction, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarFriction, p, l, v); } ),
         new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
             60f,
             (s,cf,p,v) => { s.m_params[0].avatarDensity = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].avatarDensity; },
-            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarDensity, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarDensity, p, l, v); } ),
         new ParameterDefn("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
             0f,
             (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.UpdateParameterAvatars(ref s.m_params[0].avatarRestitution, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarRestitution, p, l, v); } ),
         new ParameterDefn("AvatarCapsuleRadius", "Radius of space around an avatar",
             0.37f,
             (s,cf,p,v) => { s.m_params[0].avatarCapsuleRadius = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].avatarCapsuleRadius; },
-            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarCapsuleRadius, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleRadius, 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); },
             (s) => { return s.m_params[0].avatarCapsuleHeight; },
-            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarCapsuleHeight, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleHeight, p, l, v); } ),
             new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
             0.1f,
             (s,cf,p,v) => { s.m_params[0].avatarContactProcessingThreshold = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].avatarContactProcessingThreshold; },
-            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
 
 
             new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
@@ -1121,8 +1033,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
             (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
-            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=none, 1=all. Default=0",
-            0.0f,
+            new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
+            0.1f,
             (s,cf,p,v) => { s.m_params[0].linkConstraintCFM = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintCFM; },
             (s,p,l,v) => { s.m_params[0].linkConstraintCFM = v; } ),
@@ -1131,18 +1043,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linkConstraintERP = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintERP; },
             (s,p,l,v) => { s.m_params[0].linkConstraintERP = v; } ),
+            new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)",
+            40,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintSolverIterations = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintSolverIterations; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintSolverIterations = v; } ),
 
         new ParameterDefn("DetailedStats", "Frames between outputting detailed phys stats. (0 is off)",
             0f,
             (s,cf,p,v) => { s.m_detailedStatsStep = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_detailedStatsStep; },
             (s,p,l,v) => { s.m_detailedStatsStep = (int)v; } ),
-        new ParameterDefn("ShouldDebugLog", "Enables detailed DEBUG log statements",
-            ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.ShouldDebugLog = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s.ShouldDebugLog); },
-            (s,p,l,v) => { s.ShouldDebugLog = s.BoolNumeric(v); } ),
-
     };
 
     // Convert a boolean to our numeric true and false values
@@ -1200,6 +1111,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     private PhysParameterEntry[] SettableParameters = new PhysParameterEntry[1];
 
+    // This creates an array in the correct format for returning the list of
+    //    parameters. This is used by the 'list' option of the 'physics' command.
     private void BuildParameterTable()
     {
         if (SettableParameters.Length < ParameterDefinitions.Length)
@@ -1250,18 +1163,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     // check to see if we are updating a parameter for a particular or all of the prims
-    protected void UpdateParameterPrims(ref float loc, string parm, uint localID, float val)
-    {
-        List<uint> operateOn;
-        lock (m_prims) operateOn = new List<uint>(m_prims.Keys);
-        UpdateParameterSet(operateOn, ref loc, parm, localID, val);
-    }
-
-    // check to see if we are updating a parameter for a particular or all of the avatars
-    protected void UpdateParameterAvatars(ref float loc, string parm, uint localID, float val)
+    protected void UpdateParameterObject(ref float loc, string parm, uint localID, float val)
     {
         List<uint> operateOn;
-        lock (m_avatars) operateOn = new List<uint>(m_avatars.Keys);
+        lock (PhysObjects) operateOn = new List<uint>(PhysObjects.Keys);
         UpdateParameterSet(operateOn, ref loc, parm, localID, val);
     }
 
@@ -1284,7 +1189,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 TaintedObject("BSScene.UpdateParameterSet", delegate() {
                     foreach (uint lID in objectIDs)
                     {
-                        BulletSimAPI.UpdateParameter(m_worldID, lID, xparm, xval);
+                        BulletSimAPI.UpdateParameter(WorldID, lID, xparm, xval);
                     }
                 });
                 break;
@@ -1302,7 +1207,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         string xparm = parm.ToLower();
         float xval = val;
         TaintedObject("BSScene.TaintedUpdateParameter", delegate() {
-            BulletSimAPI.UpdateParameter(m_worldID, xlocalID, xparm, xval);
+            BulletSimAPI.UpdateParameter(WorldID, xlocalID, xparm, xval);
         });
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
new file mode 100755
index 0000000..d48462e
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -0,0 +1,480 @@
+/*
+ * 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 class BSTerrainManager
+{
+    static string LogHeader = "[BULLETSIM TERRAIN MANAGER]";
+
+    // These height values are fractional so the odd values will be
+    //     noticable when debugging.
+    public const float HEIGHT_INITIALIZATION = 24.987f;
+    public const float HEIGHT_INITIAL_LASTHEIGHT = 24.876f;
+    public const float HEIGHT_GETHEIGHT_RET = 24.765f;
+
+    // If the min and max height are equal, we reduce the min by this
+    //    amount to make sure that a bounding box is built for the terrain.
+    public const float HEIGHT_EQUAL_FUDGE = 0.2f;
+
+    public const float TERRAIN_COLLISION_MARGIN = 0.0f;
+
+    // Until the whole simulator is changed to pass us the region size, we rely on constants.
+    public Vector3 DefaultRegionSize = new Vector3(Constants.RegionSize, Constants.RegionSize, 0f);
+
+    // The scene that I am part of
+    private BSScene m_physicsScene;
+
+    // The ground plane created to keep thing from falling to infinity.
+    private BulletBody m_groundPlane;
+    
+    // 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;
+
+    // True of the terrain has been modified.
+    // Used to force recalculation of terrain height after terrain has been modified
+    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.
+    private uint m_terrainCount = BSScene.CHILDTERRAIN_ID - 1;
+    public uint HighestTerrainID { get {return m_terrainCount; } }
+
+    // If doing mega-regions, this holds our offset from region zero of
+    //     the mega-regions. "parentScene" points to the PhysicsScene of region zero.
+    private Vector3 m_worldOffset;
+    // If the parent region (region 0), this is the extent of the combined regions
+    //     relative to the origin of region zero
+    private Vector3 m_worldMax;
+    private PhysicsScene m_parentScene;
+
+    public BSTerrainManager(BSScene physicsScene)
+    {
+        m_physicsScene = physicsScene;
+        m_heightMaps = new Dictionary<Vector2,BulletHeightMapInfo>();
+        m_terrainModified = false;
+
+        // Assume one region of default size
+        m_worldOffset = Vector3.Zero;
+        m_worldMax = new Vector3(DefaultRegionSize.X, DefaultRegionSize.Y, 4096f);
+        m_parentScene = null;
+    }
+
+    // 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));
+        m_groundPlane = new BulletBody(BSScene.GROUNDPLANE_ID, 
+                        BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.Ptr, Vector3.Zero, Quaternion.Identity));
+        BulletSimAPI.AddObjectToWorld2(m_physicsScene.World.Ptr, m_groundPlane.Ptr);
+
+        Vector3 minTerrainCoords = new Vector3(0f, 0f, HEIGHT_INITIALIZATION - HEIGHT_EQUAL_FUDGE);
+        Vector3 maxTerrainCoords = new Vector3(DefaultRegionSize.X, DefaultRegionSize.Y, HEIGHT_INITIALIZATION);
+        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
+        float[] initialMap = new float[totalHeights];
+        for (int ii = 0; ii < totalHeights; ii++)
+        {
+            initialMap[ii] = HEIGHT_INITIALIZATION;
+        }
+        UpdateOrCreateTerrain(BSScene.TERRAIN_ID, initialMap, minTerrainCoords, maxTerrainCoords, true);
+    }
+
+    // Release all the terrain structures we might have allocated
+    public void ReleaseGroundPlaneAndTerrain()
+    {
+        if (m_groundPlane.Ptr != IntPtr.Zero)
+        {
+            if (BulletSimAPI.RemoveObjectFromWorld2(m_physicsScene.World.Ptr, m_groundPlane.Ptr))
+            {
+                BulletSimAPI.DestroyObject2(m_physicsScene.World.Ptr, m_groundPlane.Ptr);
+            }
+            m_groundPlane.Ptr = IntPtr.Zero;
+        }
+
+        ReleaseTerrain();
+    }
+
+    // Release all the terrain we have allocated
+    public void ReleaseTerrain()
+    {
+        foreach (KeyValuePair<Vector2, BulletHeightMapInfo> kvp in m_heightMaps)
+        {
+            if (BulletSimAPI.RemoveObjectFromWorld2(m_physicsScene.World.Ptr, kvp.Value.terrainBody.Ptr))
+            {
+                BulletSimAPI.DestroyObject2(m_physicsScene.World.Ptr, kvp.Value.terrainBody.Ptr);
+                BulletSimAPI.ReleaseHeightMapInfo2(kvp.Value.Ptr);
+            }
+        }
+        m_heightMaps.Clear();
+    }
+
+    // The simulator wants to set a new heightmap for the terrain.
+    public void SetTerrain(float[] heightMap) {
+        float[] localHeightMap = heightMap;
+        m_physicsScene.TaintedObject("TerrainManager.SetTerrain", delegate()
+        {
+            if (m_worldOffset != Vector3.Zero && m_parentScene != null)
+            {
+                // If a child of a mega-region, we shouldn't have any terrain allocated for us
+                ReleaseGroundPlaneAndTerrain();
+                // If doing the mega-prim stuff and we are the child of the zero region,
+                //    the terrain is added to our parent
+                if (m_parentScene is BSScene)
+                {
+                    DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}",
+                                    BSScene.DetailLogZero, m_worldOffset, m_worldMax);
+                    ((BSScene)m_parentScene).TerrainManager.UpdateOrCreateTerrain(BSScene.CHILDTERRAIN_ID,
+                                    localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                }
+            }
+            else
+            {
+                // If not doing the mega-prim thing, just change the terrain
+                DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
+
+                UpdateOrCreateTerrain(BSScene.TERRAIN_ID, localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+            }
+        });
+    }
+
+    // If called with no mapInfo for the terrain, this will create a new mapInfo and terrain
+    //     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 'doNow' boolean says whether to do all the unmanaged activities right now (like when
+    //     calling this routine from initialization or taint-time routines) or whether to delay
+    //     all the unmanaged activities to taint-time.
+    private void UpdateOrCreateTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords, bool doNow)
+    {
+        DetailLog("{0},BSTerrainManager.UpdateOrCreateTerrain,call,minC={1},maxC={2},doNow={3}",
+                            BSScene.DetailLogZero, minCoords, maxCoords, doNow);
+
+        float minZ = float.MaxValue;
+        float maxZ = float.MinValue;
+        Vector2 terrainRegionBase = new Vector2(minCoords.X, minCoords.Y);
+
+        int heightMapSize = heightMap.Length;
+        for (int ii = 0; ii < heightMapSize; ii++)
+        {
+            float height = heightMap[ii];
+            if (height < minZ) minZ = height;
+            if (height > maxZ) maxZ = height;
+        }
+
+        // The shape of the terrain is from its base to its extents.
+        minCoords.Z = minZ;
+        maxCoords.Z = maxZ;
+
+        BulletHeightMapInfo mapInfo;
+        if (m_heightMaps.TryGetValue(terrainRegionBase, out mapInfo))
+        {
+            // If this is terrain we know about, it's easy to update
+
+            mapInfo.heightMap = heightMap;
+            mapInfo.minCoords = minCoords;
+            mapInfo.maxCoords = maxCoords;
+            mapInfo.minZ = minZ;
+            mapInfo.maxZ = maxZ;
+            mapInfo.sizeX = maxCoords.X - minCoords.X;
+            mapInfo.sizeY = maxCoords.Y - minCoords.Y;
+            DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,call,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}", 
+                        BSScene.DetailLogZero, terrainRegionBase, mapInfo.minCoords, mapInfo.maxCoords, mapInfo.sizeX, mapInfo.sizeY);
+
+            BSScene.TaintCallback rebuildOperation = delegate()
+            {
+                if (m_parentScene != null)
+                {
+                    // 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);
+
+                    // Get rid of any terrain that may have been allocated for us.
+                    ReleaseGroundPlaneAndTerrain();
+
+                    // I hate doing this, but just bail
+                    return;
+                }
+
+                if (mapInfo.terrainBody.Ptr != IntPtr.Zero)
+                {
+                    // Updating an existing terrain.
+                    DetailLog("{0},UpdateOrCreateTerrain:UpdateExisting,taint,terrainBase={1},minC={2}, maxC={3}, szX={4}, szY={5}", 
+                                    BSScene.DetailLogZero, terrainRegionBase, mapInfo.minCoords, mapInfo.maxCoords, mapInfo.sizeX, mapInfo.sizeY);
+
+                    // Remove from the dynamics world because we're going to mangle this object
+                    BulletSimAPI.RemoveObjectFromWorld2(m_physicsScene.World.Ptr, mapInfo.terrainBody.Ptr);
+
+                    // Get rid of the old terrain
+                    BulletSimAPI.DestroyObject2(m_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(m_physicsScene.World.Ptr, mapInfo.ID,
+                        mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
+
+                    // 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);
+
+                    // Create the terrain shape from the mapInfo
+                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr));
+
+                    mapInfo.terrainBody = new BulletBody(mapInfo.ID, 
+                            BulletSimAPI.CreateBodyWithDefaultMotionState2(mapInfo.terrainShape.Ptr, 
+                                            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, m_physicsScene.Params.terrainFriction);
+                BulletSimAPI.SetHitFraction2(mapInfo.terrainBody.Ptr, m_physicsScene.Params.terrainHitFraction);
+                BulletSimAPI.SetRestitution2(mapInfo.terrainBody.Ptr, m_physicsScene.Params.terrainRestitution);
+                BulletSimAPI.SetCollisionFlags2(mapInfo.terrainBody.Ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+                BulletSimAPI.SetMassProps2(mapInfo.terrainBody.Ptr, 0f, Vector3.Zero);
+                BulletSimAPI.UpdateInertiaTensor2(mapInfo.terrainBody.Ptr);
+
+                // Return the new terrain to the world of physical objects
+                BulletSimAPI.AddObjectToWorld2(m_physicsScene.World.Ptr, mapInfo.terrainBody.Ptr);
+
+                // redo its bounding box now that it is in the world
+                BulletSimAPI.UpdateSingleAabb2(m_physicsScene.World.Ptr, mapInfo.terrainBody.Ptr);
+
+                // Make sure the new shape is processed.
+                BulletSimAPI.Activate2(mapInfo.terrainBody.Ptr, true);
+
+                m_terrainModified = true;
+            };
+
+            // There is the option to do the changes now (we're already in 'taint time'), or
+            //     to do the Bullet operations later.
+            if (doNow)
+                rebuildOperation();
+            else
+                m_physicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:UpdateExisting", rebuildOperation);
+        }
+        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
+            BSScene.TaintCallback createOperation = delegate()
+            {
+                DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoords.X, minCoords.Y);
+                // Create a new mapInfo that will be filled with the new info
+                mapInfo = new BulletHeightMapInfo(id, heightMapX,
+                        BulletSimAPI.CreateHeightMapInfo2(m_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;
+            };
+
+            // If already in taint-time, just call Bullet. Otherwise queue the operations for the safe time.
+            if (doNow)
+                createOperation();
+            else
+                m_physicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:NewTerrain", createOperation);
+        }
+    }
+
+    // Someday we will have complex terrain with caves and tunnels
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    {
+        // For the moment, it's flat and convex
+        return GetTerrainHeightAtXY(loc.X, loc.Y);
+    }
+
+    // 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
+    //    the same size and that is the default.
+    // Once the heightMapInfo is found, we have all the information to
+    //    compute the offset into the array.
+    private float lastHeightTX = 999999f;
+    private float lastHeightTY = 999999f;
+    private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
+    private float GetTerrainHeightAtXY(float tX, float tY)
+    {
+        // 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;
+
+        lastHeightTX = tX;
+        lastHeightTY = tY;
+        float ret = HEIGHT_GETHEIGHT_RET;
+
+        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);
+
+        BulletHeightMapInfo mapInfo;
+        if (m_heightMaps.TryGetValue(terrainBaseXY, out mapInfo))
+        {
+            float regionX = tX - offsetX;
+            float regionY = tY - offsetY;
+            int mapIndex = (int)regionY * (int)mapInfo.sizeY + (int)regionX;
+            try
+            {
+                ret = mapInfo.heightMap[mapIndex];
+            }
+            catch
+            {
+                // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+                m_physicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, x={2}, y={3}",
+                                    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
+        {
+            m_physicsScene.Logger.ErrorFormat("{0} GetTerrainHeightAtXY: terrain not found: region={1}, x={2}, y={3}",
+                    LogHeader, m_physicsScene.RegionName, tX, tY);
+        }
+        m_terrainModified = false;
+        lastHeight = ret;
+        return ret;
+    }
+
+    // Although no one seems to check this, I do support combining.
+    public bool SupportsCombining()
+    {
+        return true;
+    }
+    
+    // This routine is called two ways:
+    //    One with 'offset' and 'pScene' zero and null but 'extents' giving the maximum
+    //        extent of the combined regions. This is to inform the parent of the size
+    //        of the combined regions.
+    //    and one with 'offset' as the offset of the child region to the base region,
+    //        'pScene' pointing to the parent and 'extents' of zero. This informs the
+    //        child of its relative base and new parent.
+    public void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
+    {
+        m_worldOffset = offset;
+        m_worldMax = extents;
+        m_parentScene = pScene;
+        if (pScene != null)
+        {
+            // We are a child. 
+            // We want m_worldMax to be the highest coordinate of our piece of terrain.
+            m_worldMax = offset + DefaultRegionSize;
+        }
+        DetailLog("{0},BSTerrainManager.Combine,offset={1},extents={2},wOffset={3},wMax={4}", 
+                        BSScene.DetailLogZero, offset, extents, m_worldOffset, m_worldMax);
+    }
+
+    // Unhook all the combining that I know about.
+    public void UnCombine(PhysicsScene pScene)
+    {
+        // Just like ODE, for the moment a NOP
+        DetailLog("{0},BSTerrainManager.UnCombine", BSScene.DetailLogZero);
+    }
+
+
+    private void DetailLog(string msg, params Object[] args)
+    {
+        m_physicsScene.PhysicsLogging.Write(msg, args);
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 504bd3c..9221cdb 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -33,15 +33,25 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin {
 
 // Classes to allow some type checking for the API
+// These hold pointers to allocated objects in the unmanaged space.
+
+// The physics engine controller class created at initialization
 public struct BulletSim
 {
-    public BulletSim(uint id, BSScene bss, IntPtr xx) { ID = id; scene = bss;  Ptr = xx; }
-    public uint ID;
+    public BulletSim(uint worldId, BSScene bss, IntPtr xx) { worldID = worldId; scene = bss;  Ptr = xx; }
+    public uint worldID;
     // The scene is only in here so very low level routines have a handle to print debug/error messages
     public BSScene scene;
     public IntPtr Ptr;
 }
 
+public struct BulletShape
+{
+    public BulletShape(IntPtr xx) { Ptr = xx; }
+    public IntPtr Ptr;
+}
+
+// An allocated Bullet btRigidBody
 public struct BulletBody
 {
     public BulletBody(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
@@ -49,12 +59,41 @@ public struct BulletBody
     public uint ID;
 }
 
+// An allocated Bullet btConstraint
 public struct BulletConstraint
 {
     public BulletConstraint(IntPtr xx) { Ptr = xx; }
     public IntPtr Ptr;
 }
 
+// An allocated HeightMapThing which hold various heightmap info
+// Made a class rather than a struct so there would be only one
+//      instance of this and C# will pass around pointers rather
+//      than making copies.
+public class BulletHeightMapInfo
+{
+    public BulletHeightMapInfo(uint id, float[] hm, IntPtr xx) {
+        ID = id;
+        Ptr = xx;
+        heightMap = hm;
+        terrainRegionBase = new Vector2(0f, 0f);
+        minCoords = new Vector3(100f, 100f, 25f);
+        maxCoords = new Vector3(101f, 101f, 26f);
+        minZ = maxZ = 0f;
+        sizeX = sizeY = 256f;
+    }
+    public uint ID;
+    public IntPtr Ptr;
+    public float[] heightMap;
+    public Vector2 terrainRegionBase;
+    public Vector3 minCoords;
+    public Vector3 maxCoords;
+    public float sizeX, sizeY;
+    public float minZ, maxZ;
+    public BulletShape terrainShape;
+    public BulletBody terrainBody;
+}
+
 // ===============================================================================
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull 
@@ -174,12 +213,26 @@ public struct ConfigurationParameters
     public float linkConstraintTransMotorMaxForce;
     public float linkConstraintERP;
     public float linkConstraintCFM;
+    public float linkConstraintSolverIterations;
 
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
 }
 
-// Values used by Bullet and BulletSim to control collisions
+
+// The states a bullet collision object can have
+public enum ActivationState : uint
+{
+    ACTIVE_TAG = 1,
+    ISLAND_SLEEPING,
+    WANTS_DEACTIVATION,
+    DISABLE_DEACTIVATION,
+    DISABLE_SIMULATION
+}
+
+// Values used by Bullet and BulletSim to control object properties.
+// Bullet's "CollisionFlags" has more to do with operations on the
+//    object (if collisions happen, if gravity effects it, ...).
 public enum CollisionFlags : uint
 {
     CF_STATIC_OBJECT                 = 1 << 0,
@@ -194,8 +247,75 @@ public enum CollisionFlags : uint
     BS_VOLUME_DETECT_OBJECT          = 1 << 11,
     BS_PHANTOM_OBJECT                = 1 << 12,
     BS_PHYSICAL_OBJECT               = 1 << 13,
+    BS_TERRAIN_OBJECT                = 1 << 14,
+    BS_NONE                          = 0,
+    BS_ALL                           = 0xFFFFFFFF
+};
+
+// Values for collisions groups and masks
+public enum CollisionFilterGroups : uint
+{
+    NoneFilter              = 0,
+    DefaultFilter           = 1 << 0,
+    StaticFilter            = 1 << 1,
+    KinematicFilter         = 1 << 2,
+    DebrisFilter            = 1 << 3,
+    SensorTrigger           = 1 << 4,
+    CharacterFilter         = 1 << 5,
+    AllFilter               = 0xFFFFFFFF,
+    // Filter groups defined by BulletSim
+    GroundPlaneFilter       = 1 << 10,
+    TerrainFilter           = 1 << 11,
+    RaycastFilter           = 1 << 12,
+    SolidFilter             = 1 << 13,
 };
 
+    // For each type, we first clear and then set the collision flags
+public enum ClearCollisionFlag : uint
+{
+    Terrain = CollisionFlags.BS_ALL,
+    Phantom = CollisionFlags.BS_ALL,
+    VolumeDetect = CollisionFlags.BS_ALL,
+    PhysicalObject = CollisionFlags.BS_ALL,
+    StaticObject = CollisionFlags.BS_ALL
+}
+
+public enum SetCollisionFlag : uint
+{
+    Terrain = CollisionFlags.CF_STATIC_OBJECT
+        | CollisionFlags.BS_TERRAIN_OBJECT,
+    Phantom = CollisionFlags.CF_STATIC_OBJECT
+        | CollisionFlags.BS_PHANTOM_OBJECT
+        | CollisionFlags.CF_NO_CONTACT_RESPONSE,
+    VolumeDetect = CollisionFlags.CF_STATIC_OBJECT
+        | CollisionFlags.BS_VOLUME_DETECT_OBJECT
+        | CollisionFlags.CF_NO_CONTACT_RESPONSE,
+    PhysicalObject = CollisionFlags.BS_PHYSICAL_OBJECT,
+    StaticObject = CollisionFlags.CF_STATIC_OBJECT,
+}
+
+// Collision filters used for different types of objects
+public enum SetCollisionFilter : uint
+{
+    Terrain = CollisionFilterGroups.AllFilter,
+    Phantom = CollisionFilterGroups.GroundPlaneFilter
+        | CollisionFilterGroups.TerrainFilter,
+    VolumeDetect = CollisionFilterGroups.AllFilter,
+    PhysicalObject = CollisionFilterGroups.AllFilter,
+    StaticObject = CollisionFilterGroups.AllFilter,
+}
+
+// Collision masks used for different types of objects
+public enum SetCollisionMask : uint
+{
+    Terrain = CollisionFilterGroups.AllFilter,
+    Phantom = CollisionFilterGroups.GroundPlaneFilter
+        | CollisionFilterGroups.TerrainFilter,
+    VolumeDetect = CollisionFilterGroups.AllFilter,
+    PhysicalObject = CollisionFilterGroups.AllFilter,
+    StaticObject = CollisionFilterGroups.AllFilter
+}
+
 // CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
 // ERP controls amount of correction per tick. Usable range=0.1..0.8. Default=0.2.
 public enum ConstraintParams : int
@@ -221,6 +341,10 @@ public enum ConstraintParamAxis : int
 // ===============================================================================
 static class BulletSimAPI {
 
+// Link back to the managed code for outputting log messages
+[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
+
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 [return: MarshalAs(UnmanagedType.LPStr)]
 public static extern string GetVersion();
@@ -228,7 +352,11 @@ public static extern string GetVersion();
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern uint Initialize(Vector3 maxPosition, IntPtr parms, 
                         int maxCollisions, IntPtr collisionArray, 
-                        int maxUpdates, IntPtr updateArray);
+                        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);
@@ -268,23 +396,6 @@ 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);
 
-/*  Remove old functionality
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void AddConstraint(uint worldID, uint id1, uint id2, 
-                        Vector3 frame1, Quaternion frame1rot,
-                        Vector3 frame2, Quaternion frame2rot,
-                        Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveConstraintByID(uint worldID, uint id1);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
- */
-
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
 
@@ -300,6 +411,7 @@ public static extern bool SetObjectVelocity(uint worldID, uint id, Vector3 veloc
 [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);
 
@@ -342,8 +454,6 @@ public static extern Vector3 RecoverFromPenetration(uint worldID, uint id);
 public static extern void DumpBulletStatistics();
 
 // Log a debug message
-[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
-public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetDebugLogCallback(DebugLogCallback callback);
 
@@ -358,6 +468,7 @@ public static extern void SetDebugLogCallback(DebugLogCallback callback);
 // 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);
 
@@ -368,6 +479,7 @@ public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
 public static extern IntPtr GetBodyHandle2(IntPtr world, uint id);
 
 // ===============================================================================
+// Initialization and simulation
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
                                                                                         int maxCollisions,  IntPtr collisionArray,
@@ -377,7 +489,7 @@ public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
 public static extern bool UpdateParameter2(IntPtr world, uint localID, String parm, float value);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightmap2(IntPtr world, float[] heightmap);
+public static extern void SetHeightMap2(IntPtr world, float[] heightmap);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void Shutdown2(IntPtr sim);
@@ -392,23 +504,78 @@ public static extern int PhysicsStep2(IntPtr world, float timeStep, int maxSubSt
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool PushUpdate2(IntPtr obj);
 
-/*
+// =====================================================================================
+// Mesh, hull, shape and body creation helper routines
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateMeshShape2(IntPtr world, 
+                int indicesCount, [MarshalAs(UnmanagedType.LPArray)] int[] indices, 
+                int verticesCount, [MarshalAs(UnmanagedType.LPArray)] float[] vertices );
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateHullShape2(IntPtr world,
+                int hullCount, [MarshalAs(UnmanagedType.LPArray)] float[] hulls);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr BuildHullShape2(IntPtr world, IntPtr meshShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr BuildNativeShape2(IntPtr world,
+                float shapeType, float collisionMargin, Vector3 scale);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsNativeShape2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateCompoundShape2(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddChildToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RemoveChildFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateBodyFromShapeAndInfo2(IntPtr sim, IntPtr shape, IntPtr constructionInfo);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DeleteCollisionShape2(IntPtr world, IntPtr shape);
+
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateMesh2(IntPtr world, int indicesCount, int* indices, int verticesCount, float* vertices );
+public static extern IntPtr CreateBodyFromShape2(IntPtr sim, IntPtr shape, Vector3 pos, Quaternion rot);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool BuildHull2(IntPtr world, IntPtr mesh);
+public static extern IntPtr CreateBodyWithDefaultMotionState2(IntPtr shape, Vector3 pos, Quaternion rot);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool ReleaseHull2(IntPtr world, IntPtr mesh);
+public static extern IntPtr AllocateBodyInfo2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyMesh2(IntPtr world, IntPtr mesh);
+public static extern void ReleaseBodyInfo2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateObject2(IntPtr world, ShapeData shapeData);
-*/
+public static extern void DestroyObject2(IntPtr sim, IntPtr obj);
 
+// =====================================================================================
+// Terrain creation and helper routines
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateHeightMapInfo2(IntPtr sim, uint id, Vector3 minCoords, Vector3 maxCoords, 
+        [MarshalAs(UnmanagedType.LPArray)] float[] heightMap, float collisionMargin);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr FillHeightMapInfo2(IntPtr sim, IntPtr mapInfo, uint id, Vector3 minCoords, Vector3 maxCoords, 
+        [MarshalAs(UnmanagedType.LPArray)] float[] heightMap, float collisionMargin);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool ReleaseHeightMapInfo2(IntPtr heightMapInfo);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateGroundPlaneShape2(uint id, float height, float collisionMargin);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateTerrainShape2(IntPtr mapInfo);
+
+// =====================================================================================
+// Constraint creation and helper routines
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr Create6DofConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
                     Vector3 frame1loc, Quaternion frame1rot,
@@ -460,11 +627,108 @@ public static extern bool SetConstraintParam2(IntPtr constrain, ConstraintParams
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyConstraint2(IntPtr world, IntPtr constrain);
 
+// =====================================================================================
+// btCollisionWorld entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateSingleAabb2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateAabbs2(IntPtr world);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool GetForceUpdateAllAabbs2(IntPtr world);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetForceUpdateAllAabbs2(IntPtr world, bool force);
+
+// =====================================================================================
+// btDynamicsWorld entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddConstraintToWorld2(IntPtr world, IntPtr constrain, bool disableCollisionsBetweenLinkedObjects);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveConstraintFromWorld2(IntPtr world, IntPtr constrain);
+// =====================================================================================
+// btCollisionObject entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAnisotripicFriction2(IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 SetAnisotripicFriction2(IntPtr constrain, Vector3 frict);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool HasAnisotripicFriction2(IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetContactProcessingThreshold2(IntPtr obj, float val);
+
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 AddObjectToWorld2(IntPtr world, IntPtr obj);
+public static extern float GetContactProcessingThreshold2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
+public static extern bool IsStaticObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsKinematicObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsStaticOrKinematicObject2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool HasContactResponse2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCollisionShape2(IntPtr sim, IntPtr obj, IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetCollisionShape2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetActivationState2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetActivationState2(IntPtr obj, int state);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetDeactivationTime2(IntPtr obj, float dtime);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetDeactivationTime2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ForceActivationState2(IntPtr obj, ActivationState state);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Activate2(IntPtr obj, bool forceActivation);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsActive2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetRestitution2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetRestitution2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetFriction2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetFriction2(IntPtr obj);
+
+    /* Haven't defined the type 'Transform'
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Transform GetWorldTransform2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void setWorldTransform2(IntPtr obj, Transform trans);
+     */
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetPosition2(IntPtr obj);
@@ -473,85 +737,293 @@ public static extern Vector3 GetPosition2(IntPtr obj);
 public static extern Quaternion GetOrientation2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetTranslation2(IntPtr obj, Vector3 position, Quaternion rotation);
+public static extern void SetTranslation2(IntPtr obj, Vector3 position, Quaternion rotation);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetVelocity2(IntPtr obj, Vector3 velocity);
+public static extern IntPtr GetBroadphaseHandle2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetAngularVelocity2(IntPtr obj, Vector3 angularVelocity);
+public static extern void SetBroadphaseHandle2(IntPtr obj, IntPtr handle);
 
+    /*
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectForce2(IntPtr obj, Vector3 force);
+public static extern Transform GetInterpolationWorldTransform2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddObjectForce2(IntPtr obj, Vector3 force);
+public static extern void SetInterpolationWorldTransform2(IntPtr obj, Transform trans);
+     */
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetCcdMotionThreshold2(IntPtr obj, float val);
+public static extern void SetInterpolationLinearVelocity2(IntPtr obj, Vector3 vel);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetCcdSweepSphereRadius2(IntPtr obj, float val);
+public static extern void SetInterpolationAngularVelocity2(IntPtr obj, Vector3 vel);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
+public static extern void SetInterpolationVelocity2(IntPtr obj, Vector3 linearVel, Vector3 angularVel);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetDeactivationTime2(IntPtr obj, float val);
+public static extern float GetHitFraction2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetSleepingThresholds2(IntPtr obj, float lin_threshold, float ang_threshold);
+public static extern void SetHitFraction2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetContactProcessingThreshold2(IntPtr obj, float val);
+public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetFriction2(IntPtr obj, float val);
+public static extern CollisionFlags SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetRestitution2(IntPtr obj, float val);
+public static extern CollisionFlags AddToCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetLinearVelocity2(IntPtr obj, Vector3 val);
+public static extern CollisionFlags RemoveFromCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetInterpolation2(IntPtr obj, Vector3 lin, Vector3 ang);
+public static extern float GetCcdMotionThreshold2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
+public static extern void SetCcdMotionThreshold2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetCcdSweepSphereRadius2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCcdSweepSphereRadius2(IntPtr obj, float val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetUserPointer2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetUserPointer2(IntPtr obj, IntPtr val);
+
+// =====================================================================================
+// btRigidBody entries
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyGravity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetGravity2(IntPtr obj, Vector3 val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetGravity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetLinearDamping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularDamping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetLinearSleepingThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularSleepingThreshold2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyDamping2(IntPtr obj, float timeStep);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetLinearFactor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearFactor2(IntPtr obj, Vector3 factor);
+
+    /*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCenterOfMassTransform2(IntPtr obj, Transform trans);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetCenterOfMassByPosRot2(IntPtr obj, Vector3 pos, Quaternion rot);
+
+// Add a force to the object as if its mass is one.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyCentralForce2(IntPtr obj, Vector3 force);
+
+// Set the force being applied to the object as if its mass is one.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetObjectForce2(IntPtr obj, Vector3 force);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetTotalForce2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetTotalTorque2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetInvInertiaDiagLocal2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetInvInertiaDiagLocal2(IntPtr obj, Vector3 inert);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetSleepingThresholds2(IntPtr obj, float lin_threshold, float ang_threshold);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyTorque2(IntPtr obj, Vector3 torque);
+
+// Apply force at the given point. Will add torque to the object.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyForce2(IntPtr obj, Vector3 force, Vector3 pos);
+
+// Apply impulse to the object. Same as "ApplycentralForce" but force scaled by object's mass.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyCentralImpulse2(IntPtr obj, Vector3 imp);
+
+// Apply impulse to the object's torque. Force is scaled by object's mass.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyTorqueImpulse2(IntPtr obj, Vector3 imp);
+
+// Apply impulse at the point given. For is scaled by object's mass and effects both linear and angular forces.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ApplyImpulse2(IntPtr obj, Vector3 imp, Vector3 pos);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ClearForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void ClearAllForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateInertiaTensor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetCenterOfMassPosition2(IntPtr obj);
+
+    /*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Transform GetCenterOfMassTransform2(IntPtr obj);
+     */
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetLinearVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAngularVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearVelocity2(IntPtr obj, Vector3 val);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularVelocity2(IntPtr obj, Vector3 angularVelocity);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetVelocityInLocalPoint2(IntPtr obj, Vector3 pos);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Translate2(IntPtr obj, Vector3 trans);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateDeactivation2(IntPtr obj, float timeStep);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool WantsSleeping2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularFactor2(IntPtr obj, float factor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularFactorV2(IntPtr obj, Vector3 factor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetAngularFactor2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsInWorld2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void AddConstraintRef2(IntPtr obj, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RemoveConstraintRef2(IntPtr obj, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetConstraintRef2(IntPtr obj, int index);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int GetNumConstraintRefs2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetDeltaLinearVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetDeltaAngularVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetPushVelocity2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetTurnVelocity2(IntPtr obj);
+
+// =====================================================================================
+// btCollisionShape entries
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetAngularMotionDisc2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern float GetContactBreakingThreshold2(IntPtr shape, float defaultFactor);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsPolyhedral2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConvex2d2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConvex2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsNonMoving2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsConcave2(IntPtr shape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool IsCompound2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
+public static extern bool IsSoftBody2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr AddToCollisionFlags2(IntPtr obj, CollisionFlags flags);
+public static extern bool IsInfinite2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, CollisionFlags flags);
+public static extern void SetLocalScaling2(IntPtr shape, Vector3 scale);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
+public static extern Vector3 GetLocalScaling2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateInertiaTensor2(IntPtr obj);
+public static extern Vector3 CalculateLocalInertia2(IntPtr shape, float mass);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetGravity2(IntPtr obj, Vector3 val);
+public static extern int GetShapeType2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr ClearForces2(IntPtr obj);
+public static extern void SetMargin2(IntPtr shape, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr ClearAllForces2(IntPtr obj);
+public static extern float GetMargin2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetMargin2(IntPtr obj, float val);
+public static extern void SetCollisionFilterMask(IntPtr shape, uint filter, uint mask);
 
+// =====================================================================================
+// Debugging
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
+public static extern void DumpRigidBody2(IntPtr sim, IntPtr collisionObject);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyObject2(IntPtr world, uint id);
+public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpPhysicsStatistics2(IntPtr sim);