20 files changed, 3146 insertions, 1481 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 623ac8f..4c195e1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -34,7 +34,7 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSCharacter : BSPhysObject
+public sealed class BSCharacter : BSPhysObject
 {
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS CHAR]";
@@ -78,11 +78,17 @@ public class BSCharacter : BSPhysObject
     private float _PIDHoverTao;
 
     public BSCharacter(uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, bool isFlying)
+            : base(parent_scene, localID, avName, "BSCharacter")
     {
-        base.BaseInitialize(parent_scene, localID, avName, "BSCharacter");
         _physicsActorType = (int)ActorTypes.Agent;
         _position = pos;
+
+        // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
+        //     replace with the default values.
         _size = size;
+        if (_size.X == 0f) _size.X = PhysicsScene.Params.avatarCapsuleDepth;
+        if (_size.Y == 0f) _size.Y = PhysicsScene.Params.avatarCapsuleWidth;
+
         _flying = isFlying;
         _orientation = OMV.Quaternion.Identity;
         _velocity = OMV.Vector3.Zero;
@@ -97,28 +103,14 @@ public class BSCharacter : BSPhysObject
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
         DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5}",
-                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, MassRaw);
-
-        ShapeData shapeData = new ShapeData();
-        shapeData.ID = LocalID;
-        shapeData.Type = ShapeData.PhysicsShapeType.SHAPE_AVATAR;
-        shapeData.Position = _position;
-        shapeData.Rotation = _orientation;
-        shapeData.Velocity = _velocity;
-        shapeData.Size = Scale;
-        shapeData.Scale = Scale;
-        shapeData.Mass = _mass;
-        shapeData.Buoyancy = _buoyancy;
-        shapeData.Static = ShapeData.numericFalse;
-        shapeData.Friction = PhysicsScene.Params.avatarStandingFriction;
-        shapeData.Restitution = PhysicsScene.Params.avatarRestitution;
+                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
 
         // do actual create at taint time
         PhysicsScene.TaintedObject("BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
-            // New body and shape into BSBody and BSShape
-            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, shapeData, null, null, null);
+            // New body and shape into PhysBody and PhysShape
+            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, null, null);
 
             SetPhysicalProperties();
         });
@@ -131,41 +123,50 @@ public class BSCharacter : BSPhysObject
         DetailLog("{0},BSCharacter.Destroy", LocalID);
         PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
         {
-            PhysicsScene.Shapes.DereferenceBody(BSBody, true, null);
-            PhysicsScene.Shapes.DereferenceShape(BSShape, true, null);
+            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
         });
     }
 
     private void SetPhysicalProperties()
     {
-        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, BSBody.ptr);
+        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
 
-        ZeroMotion();
+        ZeroMotion(true);
         ForcePosition = _position;
         // Set the velocity and compute the proper friction
         ForceVelocity = _velocity;
-        BulletSimAPI.SetRestitution2(BSBody.ptr, PhysicsScene.Params.avatarRestitution);
-        BulletSimAPI.SetLocalScaling2(BSShape.ptr, Scale);
-        BulletSimAPI.SetContactProcessingThreshold2(BSBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
+
+        // This will enable or disable the flying buoyancy of the avatar.
+        // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
+        Flying = _flying;
+
+        BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.avatarRestitution);
+        BulletSimAPI.SetMargin2(PhysShape.ptr, PhysicsScene.Params.collisionMargin);
+        BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
+        BulletSimAPI.SetContactProcessingThreshold2(PhysBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
         if (PhysicsScene.Params.ccdMotionThreshold > 0f)
         {
-            BulletSimAPI.SetCcdMotionThreshold2(BSBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-            BulletSimAPI.SetCcdSweepSphereRadius2(BSBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+            BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
+            BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
         }
 
-        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
-        BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
+        UpdatePhysicalMassProperties(RawMass);
+
+        // Make so capsule does not fall over
+        BulletSimAPI.SetAngularFactorV2(PhysBody.ptr, OMV.Vector3.Zero);
 
-        BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.CF_CHARACTER_OBJECT);
+        BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_CHARACTER_OBJECT);
 
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, BSBody.ptr);
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
 
-        BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, BSBody.ptr);
+        // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
+        BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_DEACTIVATION);
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Do this after the object has been added to the world
-        BulletSimAPI.SetCollisionFilterMask2(BSBody.ptr,
-                        (uint)CollisionFilterGroups.AvatarFilter, 
+        BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr,
+                        (uint)CollisionFilterGroups.AvatarFilter,
                         (uint)CollisionFilterGroups.AvatarMask);
     }
 
@@ -175,6 +176,7 @@ public class BSCharacter : BSPhysObject
     }
     // No one calls this method so I don't know what it could possibly mean
     public override bool Stopped { get { return false; } }
+
     public override OMV.Vector3 Size {
         get
         {
@@ -187,23 +189,29 @@ public class BSCharacter : BSPhysObject
             _size = value;
             ComputeAvatarScale(_size);
             ComputeAvatarVolumeAndMass();
-            DetailLog("{0},BSCharacter.setSize,call,scale={1},density={2},volume={3},mass={4}",
-                            LocalID, Scale, _avatarDensity, _avatarVolume, MassRaw);
+            DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
+                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, RawMass);
 
             PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
             {
-                BulletSimAPI.SetLocalScaling2(BSShape.ptr, Scale);
-                OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
-                BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
+                BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
+                UpdatePhysicalMassProperties(RawMass);
             });
 
         }
     }
+
     public override OMV.Vector3 Scale { get; set; }
+
     public override PrimitiveBaseShape Shape
     {
         set { BaseShape = value; }
     }
+    // I want the physics engine to make an avatar capsule
+    public override BSPhysicsShapeType PreferredPhysicalShape
+    {
+        get {return BSPhysicsShapeType.SHAPE_CAPSULE; }
+    }
 
     public override bool Grabbed {
         set { _grabbed = value; }
@@ -219,23 +227,42 @@ public class BSCharacter : BSPhysObject
     // 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 override void ZeroMotion()
+    public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        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);
+        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        {
+            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+        });
     }
+    public override void ZeroAngularMotion(bool inTaintTime)
+    {
+        _rotationalVelocity = OMV.Vector3.Zero;
+
+        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        {
+            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            // The next also get rid of applied linear force but the linear velocity is untouched.
+            BulletSimAPI.ClearForces2(PhysBody.ptr);
+        });
+    }
+
 
     public override void LockAngularMotion(OMV.Vector3 axis) { return; }
 
+    public override OMV.Vector3 RawPosition
+    {
+        get { return _position; }
+        set { _position = value; }
+    }
     public override OMV.Vector3 Position {
         get {
+            // Don't refetch the position because this function is called a zillion times
             // _position = BulletSimAPI.GetObjectPosition2(Scene.World.ptr, LocalID);
             return _position;
         }
@@ -246,30 +273,30 @@ public class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+            _position = BulletSimAPI.GetPosition2(PhysBody.ptr);
             return _position;
         }
         set {
             _position = value;
             PositionSanityCheck();
-            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
         }
     }
 
 
     // Check that the current position is sane and, if not, modify the position to make it so.
-    // Check for being below terrain and being out of bounds.
+    // Check for being below terrain or on water.
     // Returns 'true' of the position was made sane by some action.
     private bool PositionSanityCheck()
     {
         bool ret = false;
-      
+
         // If below the ground, move the avatar up
         float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
         if (Position.Z < terrainHeight)
@@ -302,15 +329,11 @@ public class BSCharacter : BSPhysObject
         {
             // The new position value must be pushed into the physics engine but we can't
             //    just assign to "Position" because of potential call loops.
-            BSScene.TaintCallback sanityOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
-            };
-            if (inTaintTime)
-                sanityOperation();
-            else
-                PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", sanityOperation);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            });
             ret = true;
         }
         return ret;
@@ -319,7 +342,14 @@ public class BSCharacter : BSPhysObject
     public override float Mass { get { 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 float RawMass { 
+        get {return _mass; }
+    }
+    public override void UpdatePhysicalMassProperties(float physMass)
+    {
+        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
+        BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, localInertia);
+    }
 
     public override OMV.Vector3 Force {
         get { return _force; }
@@ -329,13 +359,13 @@ public class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject("BSCharacter.SetForce", delegate()
             {
                 DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(BSBody.ptr, _force);
+                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
             });
         }
     }
 
     // Avatars don't do vehicles
-    public override int VehicleType { get { return 0; } set { return; } }
+    public override int VehicleType { get { return (int)Vehicle.TYPE_NONE; } set { return; } }
     public override void VehicleFloatParam(int param, float value) { }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) {}
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) { }
@@ -368,7 +398,7 @@ public class BSCharacter : BSPhysObject
                 if (_currentFriction != PhysicsScene.Params.avatarStandingFriction)
                 {
                     _currentFriction = PhysicsScene.Params.avatarStandingFriction;
-                    BulletSimAPI.SetFriction2(BSBody.ptr, _currentFriction);
+                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
                 }
             }
             else
@@ -376,15 +406,15 @@ public class BSCharacter : BSPhysObject
                 if (_currentFriction != PhysicsScene.Params.avatarFriction)
                 {
                     _currentFriction = PhysicsScene.Params.avatarFriction;
-                    BulletSimAPI.SetFriction2(BSBody.ptr, _currentFriction);
+                    BulletSimAPI.SetFriction2(PhysBody.ptr, _currentFriction);
                 }
             }
             _velocity = value;
             // Remember the set velocity so we can suppress the reduction by friction, ...
             _appliedVelocity = value;
 
-            BulletSimAPI.SetLinearVelocity2(BSBody.ptr, _velocity);
-            BulletSimAPI.Activate2(BSBody.ptr, true);
+            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
+            BulletSimAPI.Activate2(PhysBody.ptr, true);
         }
     }
     public override OMV.Vector3 Torque {
@@ -401,6 +431,11 @@ public class BSCharacter : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
+    public override OMV.Quaternion RawOrientation
+    {
+        get { return _orientation; }
+        set { _orientation = value; }
+    }
     public override OMV.Quaternion Orientation {
         get { return _orientation; }
         set {
@@ -409,22 +444,22 @@ public class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject("BSCharacter.setOrientation", delegate()
             {
                 // _position = BulletSimAPI.GetPosition2(BSBody.ptr);
-                BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
             });
         }
     }
-    // Go directly to Bullet to get/set the value. 
+    // Go directly to Bullet to get/set the value.
     public override OMV.Quaternion ForceOrientation
     {
         get
         {
-            _orientation = BulletSimAPI.GetOrientation2(BSBody.ptr);
+            _orientation = BulletSimAPI.GetOrientation2(PhysBody.ptr);
             return _orientation;
         }
         set
         {
             _orientation = value;
-            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
         }
     }
     public override int PhysicsActorType {
@@ -478,14 +513,14 @@ public class BSCharacter : BSPhysObject
         set { _collidingObj = value; }
     }
     public override bool FloatOnWater {
-        set { 
+        set {
             _floatOnWater = value;
             PhysicsScene.TaintedObject("BSCharacter.setFloatOnWater", delegate()
             {
                 if (_floatOnWater)
-                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
                 else
-                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
             });
         }
     }
@@ -518,7 +553,7 @@ public class BSCharacter : BSPhysObject
             DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
             float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
-            BulletSimAPI.SetGravity2(BSBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
         }
     }
 
@@ -564,7 +599,7 @@ public class BSCharacter : BSPhysObject
             PhysicsScene.TaintedObject("BSCharacter.AddForce", delegate()
             {
                 DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(BSBody.ptr, _force);
+                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
             });
         }
         else
@@ -584,14 +619,19 @@ public class BSCharacter : BSPhysObject
         // The 'size' given by the simulator is the mid-point of the avatar
         //    and X and Y are unspecified.
 
-        OMV.Vector3 newScale = OMV.Vector3.Zero;
-        newScale.X = PhysicsScene.Params.avatarCapsuleRadius;
-        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius;
+        OMV.Vector3 newScale = size;
+        // newScale.X = PhysicsScene.Params.avatarCapsuleWidth;
+        // newScale.Y = PhysicsScene.Params.avatarCapsuleDepth;
+
+        // From the total height, remove the capsule half spheres that are at each end
+        // The 1.15f came from ODE. Not sure what this factors in.
+        // newScale.Z = (size.Z * 1.15f) - (newScale.X + newScale.Y);
+
+        // The total scale height is the central cylindar plus the caps on the two ends.
+        newScale.Z = size.Z + (Math.Min(size.X, size.Y) * 2f);
 
-        // From the total height, remote the capsule half spheres that are at each end 
-        newScale.Z = (size.Z * 2f) - Math.Min(newScale.X, newScale.Y);
-        // newScale.Z = (size.Z * 2f);
-        Scale = newScale;
+        // Convert diameters to radii and height to half height -- the way Bullet expects it.
+        Scale = newScale / 2f;
     }
 
     // set _avatarVolume and _mass based on capsule size, _density and Scale
@@ -633,10 +673,10 @@ public class BSCharacter : BSPhysObject
             // That's just the way they are defined.
             OMV.Vector3 avVel = new OMV.Vector3(_appliedVelocity.X, _appliedVelocity.Y, entprop.Velocity.Z);
             _velocity = avVel;
-            BulletSimAPI.SetLinearVelocity2(BSBody.ptr, avVel);
+            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, avVel);
         }
 
-        // Tell the linkset about this
+        // Tell the linkset about value changes
         Linkset.UpdateProperties(this);
 
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index a20be3a..65fac00 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -34,12 +34,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
 public abstract class BSConstraint : IDisposable
 {
+    private static string LogHeader = "[BULLETSIM CONSTRAINT]";
+
     protected BulletSim m_world;
     protected BulletBody m_body1;
     protected BulletBody m_body2;
     protected BulletConstraint m_constraint;
     protected bool m_enabled = false;
 
+    public BulletBody Body1 { get { return m_body1; } }
+    public BulletBody Body2 { get { return m_body2; } }
+    public BulletConstraint Constraint { get { return m_constraint; } }
+    public abstract ConstraintType Type { get; }
+    public bool IsEnabled { get { return m_enabled; } }
+
     public BSConstraint()
     {
     }
@@ -53,7 +61,7 @@ public abstract class BSConstraint : IDisposable
             {
                 bool success = BulletSimAPI.DestroyConstraint2(m_world.ptr, m_constraint.ptr);
                 m_world.physicsScene.DetailLog("{0},BSConstraint.Dispose,taint,id1={1},body1={2},id2={3},body2={4},success={5}",
-                                    BSScene.DetailLogZero, 
+                                    BSScene.DetailLogZero,
                                     m_body1.ID, m_body1.ptr.ToString("X"),
                                     m_body2.ID, m_body2.ptr.ToString("X"),
                                     success);
@@ -62,12 +70,6 @@ public abstract class BSConstraint : IDisposable
         }
     }
 
-    public BulletBody Body1 { get { return m_body1; } }
-    public BulletBody Body2 { get { return m_body2; } }
-    public BulletConstraint Constraint { get { return m_constraint; } }
-    public abstract ConstraintType Type { get; }
-
-
     public virtual bool SetLinearLimits(Vector3 low, Vector3 high)
     {
         bool ret = false;
@@ -124,7 +126,7 @@ public abstract class BSConstraint : IDisposable
             }
             else
             {
-                m_world.physicsScene.Logger.ErrorFormat("[BULLETSIM CONSTRAINT] CalculateTransforms failed. A={0}, B={1}", Body1.ID, Body2.ID);
+                m_world.physicsScene.Logger.ErrorFormat("{0} CalculateTransforms failed. A={1}, B={2}", LogHeader, Body1.ID, Body2.ID);
             }
         }
         return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
index 3306a97..23ef052 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint6Dof.cs
@@ -32,14 +32,14 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-public class BS6DofConstraint : BSConstraint
+public sealed class BSConstraint6Dof : BSConstraint
 {
     private static string LogHeader = "[BULLETSIM 6DOF CONSTRAINT]";
 
     public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } }
 
     // Create a btGeneric6DofConstraint
-    public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
                     Vector3 frame1, Quaternion frame1rot,
                     Vector3 frame2, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
@@ -58,7 +58,7 @@ public class BS6DofConstraint : BSConstraint
                             obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
     }
 
-    public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
                     Vector3 joinPoint,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     {
@@ -71,8 +71,7 @@ public class BS6DofConstraint : BSConstraint
                             BSScene.DetailLogZero, world.worldID,
                             obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
             world.physicsScene.Logger.ErrorFormat("{0} Attempt to build 6DOF constraint with missing bodies: wID={1}, rID={2}, rBody={3}, cID={4}, cBody={5}",
-                            "[BULLETSIM 6DOF CONSTRAINT]", world.worldID,
-                            obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
+                            LogHeader, world.worldID, obj1.ID, obj1.ptr.ToString("X"), obj2.ID, obj2.ptr.ToString("X"));
             m_enabled = false;
         }
         else
@@ -135,7 +134,11 @@ public class BS6DofConstraint : BSConstraint
         bool ret = false;
         float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
         if (m_enabled)
+        {
             ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.ptr, onOff, targetVelocity, maxMotorForce);
+            m_world.physicsScene.DetailLog("{0},BS6DOFConstraint,TransLimitMotor,enable={1},vel={2},maxForce={3}",
+                            BSScene.DetailLogZero, enable, targetVelocity, maxMotorForce);
+        }
         return ret;
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index 22ea367..a9fd826 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -33,7 +33,7 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-public class BSConstraintCollection : IDisposable
+public sealed class BSConstraintCollection : IDisposable
 {
     // private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
     // private static readonly string LogHeader = "[CONSTRAINT COLLECTION]";
@@ -143,8 +143,6 @@ public class BSConstraintCollection : IDisposable
     // Return 'true' if any constraints were destroyed.
     public bool RemoveAndDestroyConstraint(BulletBody body1)
     {
-        // return BulletSimAPI.RemoveConstraintByID(m_world.ID, obj.ID);
-
         List<BSConstraint> toRemove = new List<BSConstraint>();
         uint lookingID = body1.ID;
         lock (m_constraints)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSHingeConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
index 7c8a215..ed3ffa7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSHingeConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintHinge.cs
@@ -32,11 +32,11 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-class BSHingeConstraint : BSConstraint
+public sealed class BSConstraintHinge : BSConstraint
 {
     public override ConstraintType Type { get { return ConstraintType.HINGE_CONSTRAINT_TYPE; } }
 
-    public BSHingeConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraintHinge(BulletSim world, BulletBody obj1, BulletBody obj2,
                     Vector3 pivotInA, Vector3 pivotInB,
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 56342b8..dbc9039 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -23,7 +23,7 @@
  * 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.
- */
+ *
 
 /* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to
  * call the BulletSim system.
@@ -52,12 +52,17 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-    public class BSDynamics
+    public sealed class BSDynamics
     {
+        private static string LogHeader = "[BULLETSIM VEHICLE]";
+
         private BSScene PhysicsScene { get; set; }
         // the prim this dynamic controller belongs to
         private BSPrim Prim { get; set; }
 
+        // mass of the vehicle fetched each time we're calles
+        private float m_vehicleMass;
+
         // Vehicle properties
         public Vehicle Type { get; set; }
 
@@ -72,8 +77,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                                                         // LIMIT_ROLL_ONLY
         private Vector3 m_BlockingEndPoint = Vector3.Zero;
         private Quaternion m_RollreferenceFrame = Quaternion.Identity;
+        private Quaternion m_referenceFrame = Quaternion.Identity;
+
         // Linear properties
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
+        private Vector3 m_linearMotorOffset = Vector3.Zero;             // the point of force can be offset from the center
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
         private Vector3 m_newVelocity = Vector3.Zero;                   // velocity computed to be applied to body
         private Vector3 m_linearFrictionTimescale = Vector3.Zero;
@@ -86,7 +94,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         //Angular properties
         private Vector3 m_angularMotorDirection = Vector3.Zero;         // angular velocity requested by LSL motor
-        private int m_angularMotorApply = 0;                            // application frame counter
+        // private int m_angularMotorApply = 0;                            // application frame counter
         private Vector3 m_angularMotorVelocity = Vector3.Zero;          // current angular motor velocity
         private float m_angularMotorTimescale = 0;                      // motor angular velocity ramp up rate
         private float m_angularMotorDecayTimescale = 0;                 // motor angular velocity decay rate
@@ -95,19 +103,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
-        // private float m_angularDeflectionEfficiency = 0;
-        // private float m_angularDeflectionTimescale = 0;
-        // private float m_linearDeflectionEfficiency = 0;
-        // private float m_linearDeflectionTimescale = 0;
+        private float m_angularDeflectionEfficiency = 0;
+        private float m_angularDeflectionTimescale = 0;
+        private float m_linearDeflectionEfficiency = 0;
+        private float m_linearDeflectionTimescale = 0;
 
         //Banking properties
-        // private float m_bankingEfficiency = 0;
-        // private float m_bankingMix = 0;
-        // private float m_bankingTimescale = 0;
+        private float m_bankingEfficiency = 0;
+        private float m_bankingMix = 0;
+        private float m_bankingTimescale = 0;
 
         //Hover and Buoyancy properties
         private float m_VhoverHeight = 0f;
-//        private float m_VhoverEfficiency = 0f;
+        private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
         private float m_VhoverTargetHeight = -1.0f;     // if <0 then no hover, else its the current target height
         private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
@@ -138,10 +146,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
-                    // m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
+                    m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
-                    // m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
+                    m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
                     m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f);
@@ -150,20 +158,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularMotorTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BANKING_EFFICIENCY:
-                    // m_bankingEfficiency = Math.Max(pValue, 0.01f);
+                    m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f));
                     break;
                 case Vehicle.BANKING_MIX:
-                    // m_bankingMix = Math.Max(pValue, 0.01f);
+                    m_bankingMix = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BANKING_TIMESCALE:
-                    // m_bankingTimescale = Math.Max(pValue, 0.01f);
+                    m_bankingTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.BUOYANCY:
                     m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f));
                     break;
-//                case Vehicle.HOVER_EFFICIENCY:
-//                    m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f));
-//                    break;
+                case Vehicle.HOVER_EFFICIENCY:
+                    m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f));
+                    break;
                 case Vehicle.HOVER_HEIGHT:
                     m_VhoverHeight = pValue;
                     break;
@@ -171,10 +179,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_VhoverTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
-                    // m_linearDeflectionEfficiency = Math.Max(pValue, 0.01f);
+                    m_linearDeflectionEfficiency = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
-                    // m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
+                    m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
                     m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f);
@@ -196,7 +204,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
                     m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
-                    m_angularMotorApply = 10;
+                    // m_angularMotorApply = 100;
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -206,7 +214,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
-                    // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
+                    m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
                     break;
 
             }
@@ -221,15 +229,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DIRECTION:
-                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     // Limit requested angular speed to 2 rps= 4 pi rads/sec
-                    if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
-                    if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
-                    if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
-                    if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
-                    if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
-                    if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
-                    m_angularMotorApply = 10;
+                    pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f));
+                    pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
+                    pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
+                    m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    // m_angularMotorApply = 100;
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -239,7 +244,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
-                    // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     break;
                 case Vehicle.BLOCK_EXIT:
                     m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -253,7 +258,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             switch (pParam)
             {
                 case Vehicle.REFERENCE_FRAME:
-                    // m_referenceFrame = pValue;
+                    m_referenceFrame = pValue;
                     break;
                 case Vehicle.ROLL_FRAME:
                     m_RollreferenceFrame = pValue;
@@ -265,21 +270,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", Prim.LocalID, pParam, remove);
             VehicleFlag parm = (VehicleFlag)pParam;
-            if (remove)
+            if (pParam == -1)
+                m_flags = (VehicleFlag)0;
+            else
             {
-                if (pParam == -1)
-                {
-                    m_flags = (VehicleFlag)0;
-                }
-                else
-                {
+                if (remove)
                     m_flags &= ~parm;
-                }
-            }
-            else {
-                m_flags |= parm;
+                else
+                    m_flags |= parm;
             }
-        }//end ProcessVehicleFlags
+        }
 
         internal void ProcessTypeChange(Vehicle pType)
         {
@@ -288,101 +288,144 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Type = pType;
             switch (pType)
             {
-                    case Vehicle.TYPE_NONE:
-                    m_linearFrictionTimescale = new Vector3(0, 0, 0);
-                    m_angularFrictionTimescale = new Vector3(0, 0, 0);
+                case Vehicle.TYPE_NONE:
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 0;
                     m_linearMotorDecayTimescale = 0;
+                    m_linearFrictionTimescale = new Vector3(0, 0, 0);
+
                     m_angularMotorDirection = Vector3.Zero;
-                    m_angularMotorTimescale = 0;
                     m_angularMotorDecayTimescale = 0;
+                    m_angularMotorTimescale = 0;
+                    m_angularFrictionTimescale = new Vector3(0, 0, 0);
+
                     m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 0;
                     m_VehicleBuoyancy = 0;
+                    
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 1000;
+
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 0;
+
+                    m_bankingEfficiency = 0;
+                    m_bankingTimescale = 1000;
+                    m_bankingMix = 1;
+
+                    m_referenceFrame = Quaternion.Identity;
                     m_flags = (VehicleFlag)0;
                     break;
 
                 case Vehicle.TYPE_SLED:
-                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
-                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 1000;
                     m_linearMotorDecayTimescale = 120;
+                    m_linearFrictionTimescale = new Vector3(30, 1, 1000);
+
                     m_angularMotorDirection = Vector3.Zero;
                     m_angularMotorTimescale = 1000;
                     m_angularMotorDecayTimescale = 120;
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 1;
+                    m_VhoverEfficiency = 10;    // TODO: this looks wrong!!
                     m_VhoverTimescale = 10;
                     m_VehicleBuoyancy = 0;
-                    // m_linearDeflectionEfficiency = 1;
-                    // m_linearDeflectionTimescale = 1;
-                    // m_angularDeflectionEfficiency = 1;
-                    // m_angularDeflectionTimescale = 1000;
-                    // m_bankingEfficiency = 0;
-                    // m_bankingMix = 1;
-                    // m_bankingTimescale = 10;
-                    // m_referenceFrame = Quaternion.Identity;
+
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 1;
+
+                    m_angularDeflectionEfficiency = 1;
+                    m_angularDeflectionTimescale = 1000;
+
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 0;
+
+                    m_bankingEfficiency = 0;
+                    m_bankingTimescale = 10;
+                    m_bankingMix = 1;
+
+                    m_referenceFrame = Quaternion.Identity;
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
                     m_flags &=
                          ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
                            VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
                     break;
                 case Vehicle.TYPE_CAR:
-                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
-                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 1;
                     m_linearMotorDecayTimescale = 60;
+                    m_linearFrictionTimescale = new Vector3(100, 2, 1000);
+
                     m_angularMotorDirection = Vector3.Zero;
                     m_angularMotorTimescale = 1;
                     m_angularMotorDecayTimescale = 0.8f;
+                    m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
+
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0;
+                    m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 1000;
                     m_VehicleBuoyancy = 0;
-                    // // m_linearDeflectionEfficiency = 1;
-                    // // m_linearDeflectionTimescale = 2;
-                    // // m_angularDeflectionEfficiency = 0;
-                    // m_angularDeflectionTimescale = 10;
+
+                    m_linearDeflectionEfficiency = 1;
+                    m_linearDeflectionTimescale = 2;
+
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 10;
+
                     m_verticalAttractionEfficiency = 1f;
                     m_verticalAttractionTimescale = 10f;
-                    // m_bankingEfficiency = -0.2f;
-                    // m_bankingMix = 1;
-                    // m_bankingTimescale = 1;
-                    // m_referenceFrame = Quaternion.Identity;
-                    m_flags |= (VehicleFlag.NO_DEFLECTION_UP 
+
+                    m_bankingEfficiency = -0.2f;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1;
+
+                    m_referenceFrame = Quaternion.Identity;
+                    m_flags &= ~(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_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
-                    m_flags |= (VehicleFlag.HOVER_UP_ONLY);
+                                | VehicleFlag.LIMIT_MOTOR_UP
+                                | VehicleFlag.HOVER_UP_ONLY);
                     break;
                 case Vehicle.TYPE_BOAT:
-                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
-                    m_angularFrictionTimescale = new Vector3(10,10,10);
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 5;
                     m_linearMotorDecayTimescale = 60;
+                    m_linearFrictionTimescale = new Vector3(10, 3, 2);
+
                     m_angularMotorDirection = Vector3.Zero;
                     m_angularMotorTimescale = 4;
                     m_angularMotorDecayTimescale = 4;
+                    m_angularFrictionTimescale = new Vector3(10,10,10);
+
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 2;
                     m_VehicleBuoyancy = 1;
-                    // m_linearDeflectionEfficiency = 0.5f;
-                    // m_linearDeflectionTimescale = 3;
-                    // m_angularDeflectionEfficiency = 0.5f;
-                    // m_angularDeflectionTimescale = 5;
+
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 3;
+
+                    m_angularDeflectionEfficiency = 0.5f;
+                    m_angularDeflectionTimescale = 5;
+
                     m_verticalAttractionEfficiency = 0.5f;
                     m_verticalAttractionTimescale = 5f;
-                    // m_bankingEfficiency = -0.3f;
-                    // m_bankingMix = 0.8f;
-                    // m_bankingTimescale = 1;
-                    // m_referenceFrame = Quaternion.Identity;
+
+                    m_bankingEfficiency = -0.3f;
+                    m_bankingMix = 0.8f;
+                    m_bankingTimescale = 1;
+
+                    m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
-                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT 
+                                    | VehicleFlag.HOVER_GLOBAL_HEIGHT
                                     | VehicleFlag.LIMIT_ROLL_ONLY
                                     | VehicleFlag.HOVER_UP_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP
@@ -390,28 +433,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                     | VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
-                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
-                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 2;
                     m_linearMotorDecayTimescale = 60;
+                    m_linearFrictionTimescale = new Vector3(200, 10, 5);
+
                     m_angularMotorDirection = Vector3.Zero;
                     m_angularMotorTimescale = 4;
                     m_angularMotorDecayTimescale = 4;
+                    m_angularFrictionTimescale = new Vector3(20, 20, 20);
+
                     m_VhoverHeight = 0;
-//                    m_VhoverEfficiency = 0.5f;
+                    m_VhoverEfficiency = 0.5f;
                     m_VhoverTimescale = 1000;
                     m_VehicleBuoyancy = 0;
-                    // m_linearDeflectionEfficiency = 0.5f;
-                    // m_linearDeflectionTimescale = 3;
-                    // m_angularDeflectionEfficiency = 1;
-                    // m_angularDeflectionTimescale = 2;
+
+                    m_linearDeflectionEfficiency = 0.5f;
+                    m_linearDeflectionTimescale = 3;
+
+                    m_angularDeflectionEfficiency = 1;
+                    m_angularDeflectionTimescale = 2;
+
                     m_verticalAttractionEfficiency = 0.9f;
                     m_verticalAttractionTimescale = 2f;
-                    // m_bankingEfficiency = 1;
-                    // m_bankingMix = 0.7f;
-                    // m_bankingTimescale = 2;
-                    // m_referenceFrame = Quaternion.Identity;
+
+                    m_bankingEfficiency = 1;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 2;
+
+                    m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
                                     | VehicleFlag.HOVER_TERRAIN_ONLY
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT
@@ -421,28 +471,36 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                     break;
                 case Vehicle.TYPE_BALLOON:
-                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
-                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
                     m_linearMotorDirection = Vector3.Zero;
                     m_linearMotorTimescale = 5;
+                    m_linearFrictionTimescale = new Vector3(5, 5, 5);
                     m_linearMotorDecayTimescale = 60;
+
                     m_angularMotorDirection = Vector3.Zero;
                     m_angularMotorTimescale = 6;
+                    m_angularFrictionTimescale = new Vector3(10, 10, 10);
                     m_angularMotorDecayTimescale = 10;
+
                     m_VhoverHeight = 5;
-//                    m_VhoverEfficiency = 0.8f;
+                    m_VhoverEfficiency = 0.8f;
                     m_VhoverTimescale = 10;
                     m_VehicleBuoyancy = 1;
-                    // m_linearDeflectionEfficiency = 0;
-                    // m_linearDeflectionTimescale = 5;
-                    // m_angularDeflectionEfficiency = 0;
-                    // m_angularDeflectionTimescale = 5;
+
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 5;
+
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 5;
+
                     m_verticalAttractionEfficiency = 1f;
                     m_verticalAttractionTimescale = 100f;
-                    // m_bankingEfficiency = 0;
-                    // m_bankingMix = 0.7f;
-                    // m_bankingTimescale = 5;
-                    // m_referenceFrame = Quaternion.Identity;
+
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 0.7f;
+                    m_bankingTimescale = 5;
+                    m_referenceFrame = Quaternion.Identity;
+
+                    m_referenceFrame = Quaternion.Identity;
                     m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
                                     | VehicleFlag.HOVER_TERRAIN_ONLY
                                     | VehicleFlag.HOVER_UP_ONLY
@@ -452,20 +510,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
-        }//end SetDefaultsForType
+        }
 
         // Some of the properties of this prim may have changed.
         // Do any updating needed for a vehicle
         public void Refresh()
         {
-            if (!IsActive) 
-                return;
+            if (IsActive)
+            {
+                // Friction effects are handled by this vehicle code
+                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
+                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
+
+                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
 
-            // Set the prim's inertia to zero. The vehicle code handles that and this
-            //    removes the motion and torque actions introduced by Bullet.
-            Vector3 inertia = Vector3.Zero;
-            BulletSimAPI.SetMassProps2(Prim.BSBody.ptr, Prim.MassRaw, inertia);
-            BulletSimAPI.UpdateInertiaTensor2(Prim.BSBody.ptr);
+                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+            }
+        }
+
+        public bool RemoveBodyDependencies(BSPhysObject prim)
+        {
+            // If active, we need to add our properties back when the body is rebuilt.
+            return IsActive;
+        }
+
+        public void RestoreBodyDependencies(BSPhysObject prim)
+        {
+            if (Prim.LocalID != prim.LocalID)
+            {
+                // The call should be on us by our prim. Error if not.
+                PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
+                                LogHeader, prim.LocalID, Prim.LocalID);
+                return;
+            }
+            Refresh();
         }
 
         // One step of the vehicle properties for the next 'pTimestep' seconds.
@@ -473,13 +551,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (!IsActive) return;
 
+            // DEBUG
+            // Because Bullet does apply forces to the vehicle, our last computed
+            //     linear and angular velocities are not what is happening now.
+            // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
+            // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
+            // m_lastAngularVelocity = Prim.ForceRotationalVelocity;   // DEBUG: account for what Bullet did last time
+            // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);        // DEBUG:
+            // END DEBUG
+
+            m_vehicleMass = Prim.Linkset.LinksetMass;
+
             MoveLinear(pTimestep);
+            // Commented out for debug
             MoveAngular(pTimestep);
+            // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false);    // DEBUG DEBUG
+            // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity;    // DEBUG DEBUG
+
             LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
             m_lastPositionVector = Prim.ForcePosition;
 
+            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
+                                    Prim.LocalID, 
+                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
+                                    BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
+                                    Prim.Inertia,
+                                    m_vehicleMass
+                                    );
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }// end Step
@@ -488,62 +588,52 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Also does hover and float.
         private void MoveLinear(float pTimestep)
         {
-            // m_linearMotorDirection is the direction we are moving relative to the vehicle coordinates
-            // m_lastLinearVelocityVector is the speed we are moving in that direction
+            // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates
+            // m_lastLinearVelocityVector is the current speed we are moving in that direction
             if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
-                Vector3 origDir = m_linearMotorDirection;
-                Vector3 origVel = m_lastLinearVelocityVector;
-
-                // add drive to body
-                // 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);
+                Vector3 origDir = m_linearMotorDirection;       // DEBUG
+                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
+                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
+                Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
+
+                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
                 m_lastLinearVelocityVector += addAmount;
 
-                // 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;
-                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
-                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
-                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);
-                // (RA: do not know where the 0.5f comes from)
-                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-                 */
-                float keepfraction = 1.0f - (1.0f / (m_linearMotorDecayTimescale / pTimestep));
-                m_linearMotorDirection *= keepfraction;
-
-                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},notDecay={4},dir={5},vel={6}",
-                    Prim.LocalID, origDir, origVel, addAmount, keepfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
+                float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
+                m_linearMotorDirection *= (1f - decayFactor);
+
+                // Rotate new object velocity from vehicle relative to world coordinates
+                m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
+
+                // Apply friction for next time
+                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
+                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
+
+                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
+                                Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
+                                m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
             }
             else
             {
                 // if what remains of direction is very small, zero it.
                 m_linearMotorDirection = Vector3.Zero;
                 m_lastLinearVelocityVector = Vector3.Zero;
+                m_newVelocity = Vector3.Zero;
+
                 VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
             }
 
-            // convert requested object velocity to object relative vector
-            Quaternion rotq = Prim.ForceOrientation;
-            m_newVelocity = m_lastLinearVelocityVector * rotq;
-
-            // Add the various forces into m_dir which will be our new direction vector (velocity)
+            // m_newVelocity is velocity computed from linear motor in world coordinates
 
-            // add Gravity and Buoyancy
+            // Gravity and Buoyancy
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Linkset.LinksetMass * (1f - m_VehicleBuoyancy));
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
 
             /*
-             * RA: Not sure why one would do this
+             * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
             // Preserve the current Z velocity
             Vector3 vel_now = m_prim.Velocity;
             m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
@@ -555,7 +645,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // If below the terrain, move us above the ground a little.
             float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
+            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
             // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
@@ -566,6 +656,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
             // Check if hovering
+            // m_VhoverEfficiency: 0=bouncy, 1=totally damped
+            // m_VhoverTimescale: time to achieve height
             if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
             {
                 // We should hover, get the target height
@@ -584,25 +676,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 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 body is already heigher, use its height as target height
+                    if (pos.Z > m_VhoverTargetHeight)
+                        m_VhoverTargetHeight = pos.Z;
                 }
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
-                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
+                    if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f)
                     {
+                        pos.Z = m_VhoverTargetHeight;
                         Prim.ForcePosition = pos;
                     }
                 }
                 else
                 {
-                    float herr0 = pos.Z - m_VhoverTargetHeight;
+                    float verticalError = pos.Z - m_VhoverTargetHeight;
+                    // RA: where does the 50 come from?
+                    float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
                     // Replace Vertical speed with correction figure if significant
-                    if (Math.Abs(herr0) > 0.01f)
+                    if (Math.Abs(verticalError) > 0.01f)
                     {
-                        m_newVelocity.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
+                        m_newVelocity.Z += verticalCorrectionVelocity;
                         //KF: m_VhoverEfficiency is not yet implemented
                     }
+                    else if (verticalError < -0.01)
+                    {
+                        m_newVelocity.Z -= verticalCorrectionVelocity;
+                    }
                     else
                     {
                         m_newVelocity.Z = 0f;
@@ -649,25 +749,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
             }
 
-            // Limit absolute vertical change
-            float Zchange = Math.Abs(posChange.Z);
+            #region downForce
+            Vector3 downForce = Vector3.Zero;
+
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                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 postemp = (pos.Z - terrainHeight);
-                if (postemp > 2.5f)
-                    grav.Z = (float)(grav.Z * 1.037125);
-                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
+                // If the vehicle is motoring into the sky, get it going back down.
+                // Is this an angular force or both linear and angular??
+                float distanceAboveGround = pos.Z - terrainHeight;
+                if (distanceAboveGround > 2f)
+                {
+                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
+                    // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
+                    downForce = new Vector3(0, 0, -distanceAboveGround);
+                }
+                // TODO: this calculation is all wrong. From the description at
+                //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
+                //     has a decay factor. This says this force should
+                //     be computed with a motor.
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 
+                                    Prim.LocalID, distanceAboveGround, downForce);
             }
+            #endregion // downForce
 
             // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -677,21 +780,32 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                 m_newVelocity.Z = 0;
 
-            // Apply velocity
+            // Clamp REALLY high or low velocities
+            if (m_newVelocity.LengthSquared() > 1e6f)
+            {
+                m_newVelocity /= m_newVelocity.Length();
+                m_newVelocity *= 1000f;
+            }
+            else if (m_newVelocity.LengthSquared() < 1e-6f)
+                m_newVelocity = Vector3.Zero;
+
+            // Stuff new linear velocity into the vehicle
             Prim.ForceVelocity = m_newVelocity;
-            // apply gravity force
-            // Why is this set here? The physics engine already does gravity.
-            Prim.AddForce(grav, false, true);
+            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
 
-            // Apply friction
-            Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
-            m_lastLinearVelocityVector *= keepFraction;
+            Vector3 totalDownForce = downForce + grav;
+            if (totalDownForce != Vector3.Zero)
+            {
+                Prim.AddForce(totalDownForce * m_vehicleMass, false);
+                // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
+            }
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4},1Mdecay={5}",
-                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav, keepFraction);
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
+                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
 
         } // end MoveLinear()
 
+        // =======================================================================
         // Apply the effect of the angular motor.
         private void MoveAngular(float pTimestep)
         {
@@ -703,95 +817,191 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_angularFrictionTimescale      // body angular velocity  decay rate
             // m_lastAngularVelocity           // what was last applied to body
 
-            // Get what the body is doing, this includes 'external' influences
-            Vector3 angularVelocity = Prim.ForceRotationalVelocity;
-
-            if (m_angularMotorApply > 0)
+            if (m_angularMotorDirection.LengthSquared() > 0.0001)
             {
-                // Rather than snapping the angular motor velocity from the old value to
-                //    a newly set velocity, this routine steps the value from the previous
-                //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
-                // There are m_angularMotorApply steps.
                 Vector3 origVel = m_angularMotorVelocity;
                 Vector3 origDir = m_angularMotorDirection;
 
-                // ramp up to new value
-                //       new 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);
-
-                VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
-                        Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
+                //       new velocity    +=                      error                       /  (  time to get there   / step interval)
+                //                           requested direction   - current vehicle direction
+                m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) /  (m_angularMotorTimescale / pTimestep);
+                // decay requested direction
+                m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
 
-                m_angularMotorApply--;
+                VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
+                        Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
             }
             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
+                m_angularMotorVelocity = Vector3.Zero;
+            }
+
+            #region Vertical attactor
+
             Vector3 vertattr = Vector3.Zero;
             Vector3 deflection = Vector3.Zero;
             Vector3 banking = Vector3.Zero;
 
+            // If vertical attaction timescale is reasonable and we applied an angular force last time...
             if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
             {
-                float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
-                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
+                float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
+                if (Prim.IsColliding)
+                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
 
-                // get present body rotation
-                Quaternion rotq = Prim.ForceOrientation;
-                // vector pointing up
-                Vector3 verterr = Vector3.Zero;
-                verterr.Z = 1.0f;
+                VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
 
-                // rotate it to Body Angle
-                verterr = verterr * rotq;
-                // verterr.X and .Y are the World error amounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
-                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
-                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
+                // Create a vector of the vehicle "up" in world coordinates
+                Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
+                // verticalError.X and .Y are the World error amounts. They are 0 when there is no
+                //      error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its
+                //      side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall
+                //      and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
+                //      modulated to prevent a stable inverted body.
 
                 // Error is 0 (no error) to +/- 2 (max error)
-                if (verterr.Z < 0.0f)
+                if (verticalError.Z < 0.0f)
                 {
-                    verterr.X = 2.0f - verterr.X;
-                    verterr.Y = 2.0f - verterr.Y;
+                    verticalError.X = 2.0f - verticalError.X;
+                    verticalError.Y = 2.0f - verticalError.Y;
                 }
-                // scale it by VAservo
-                verterr = verterr * VAservo;
-
-                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
-                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
-                vertattr.X =    verterr.Y;
-                vertattr.Y =  - verterr.X;
+                // scale it by VAservo (timestep and timescale)
+                verticalError = verticalError * VAservo;
+
+                // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
+                //     then .X increases, so change  Body angular velocity  X based on Y, and Y based on X.
+                //     Z is not changed.
+                vertattr.X =    verticalError.Y;
+                vertattr.Y =  - verticalError.X;
                 vertattr.Z = 0f;
 
                 // scaling appears better usingsquare-law
+                Vector3 angularVelocity = Prim.ForceRotationalVelocity;
                 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
-                VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
-                            Prim.LocalID, verterr, bounce, vertattr);
+                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
+                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
 
-            } // else vertical attractor is off
+            }
+            #endregion // Vertical attactor
 
-            m_lastVertAttractor = vertattr;
+            #region Deflection
+
+            if (m_angularDeflectionEfficiency != 0)
+            {
+                // Compute a scaled vector that points in the preferred axis (X direction)
+                Vector3 scaledDefaultDirection =
+                    new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
+                // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
+                // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
+                Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
+
+                // Scale by efficiency and timescale
+                deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+
+                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
+                                        Prim.LocalID, preferredAxisOfMotion, deflection);
+                // This deflection computation is not correct.
+                deflection = Vector3.Zero;
+            }
 
-            // Bank section tba
+            #endregion
 
-            // Deflection section tba
+            #region Banking
+
+            if (m_bankingEfficiency != 0)
+            {
+                Vector3 dir = Vector3.One * Prim.ForceOrientation;
+                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
+                    //Changes which way it banks in and out of turns
+
+                //Use the square of the efficiency, as it looks much more how SL banking works
+                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
+                if (m_bankingEfficiency < 0)
+                    effSquared *= -1; //Keep the negative!
+
+                float mix = Math.Abs(m_bankingMix);
+                if (m_angularMotorVelocity.X == 0)
+                {
+                    /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
+                    {
+                        Vector3 axisAngle;
+                        float angle;
+                        parent.Orientation.GetAxisAngle(out axisAngle, out angle);
+                        Vector3 rotatedVel = parent.Velocity * parent.Orientation;
+                        if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
+                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
+                        else
+                            m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
+                    }*/
+                }
+                else
+                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
+                if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
+                    //If they are colliding, we probably shouldn't shove the prim around... probably
+                {
+                    float angVelZ = m_angularMotorVelocity.X*-1;
+                    /*if(angVelZ > mix)
+                        angVelZ = mix;
+                    else if(angVelZ < -mix)
+                        angVelZ = -mix;*/
+                    //This controls how fast and how far the banking occurs
+                    Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
+                    if (bankingRot.X > 3)
+                        bankingRot.X = 3;
+                    else if (bankingRot.X < -3)
+                        bankingRot.X = -3;
+                    bankingRot *= Prim.ForceOrientation;
+                    banking += bankingRot;
+                }
+                m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
+                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 
+                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
+            }
+
+            #endregion
+
+            m_lastVertAttractor = vertattr;
 
             // Sum velocities
-            m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
-         
+            m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
+
+            #region Linear Motor Offset
+
+            //Offset section
+            if (m_linearMotorOffset != Vector3.Zero)
+            {
+                //Offset of linear velocity doesn't change the linear velocity,
+                //   but causes a torque to be applied, for example...
+                //
+                //      IIIII     >>>   IIIII
+                //      IIIII     >>>    IIIII
+                //      IIIII     >>>     IIIII
+                //          ^
+                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
+                //
+                //
+                // The torque created is the linear velocity crossed with the offset
+
+                // NOTE: this computation does should be in the linear section
+                //    because there we know the impulse being applied.
+                Vector3 torqueFromOffset = Vector3.Zero;
+                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
+                if (float.IsNaN(torqueFromOffset.X))
+                    torqueFromOffset.X = 0;
+                if (float.IsNaN(torqueFromOffset.Y))
+                    torqueFromOffset.Y = 0;
+                if (float.IsNaN(torqueFromOffset.Z))
+                    torqueFromOffset.Z = 0;
+                torqueFromOffset *= m_vehicleMass;
+                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
+                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+            }
+
+            #endregion
+
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 m_lastAngularVelocity.X = 0;
@@ -802,55 +1012,56 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                Prim.ZeroAngularMotion(true);
+                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+            }
+            else
+            {
+                // Apply to the body.
+                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
+                // Since we are stuffing the angular velocity directly into the object, the computed
+                //     velocity needs to be scaled by the timestep.
+                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
+                Prim.ForceRotationalVelocity = applyAngularForce;
+
+                // Decay the angular movement for next time
+                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
+                m_lastAngularVelocity *= Vector3.One - decayamount;
+
+                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
+                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
             }
-
-             // apply friction
-            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
-            m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
-
-            // Apply to the body
-            Prim.ForceRotationalVelocity = m_lastAngularVelocity;
-
-            VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)
         {
             Quaternion rotq = Prim.ForceOrientation;
             Quaternion m_rot = rotq;
-            bool changed = false;
             if (m_RollreferenceFrame != Quaternion.Identity)
             {
                 if (rotq.X >= m_RollreferenceFrame.X)
                 {
                     m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
-                    changed = true;
                 }
                 if (rotq.Y >= m_RollreferenceFrame.Y)
                 {
                     m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
-                    changed = true;
                 }
                 if (rotq.X <= -m_RollreferenceFrame.X)
                 {
                     m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
-                    changed = true;
                 }
                 if (rotq.Y <= -m_RollreferenceFrame.Y)
                 {
                     m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
-                    changed = true;
                 }
-                changed = true;
             }
             if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
             {
                 m_rot.X = 0;
                 m_rot.Y = 0;
-                changed = true;
             }
-            if (changed)
+            if (rotq != m_rot)
             {
                 Prim.ForceOrientation = m_rot;
                 VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 43b1262..0df4310 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,10 +32,39 @@ using OMV = OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSLinkset
+public abstract class BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET]";
 
+    public enum LinksetImplementation
+    {
+        Constraint   = 0,   // linkset tied together with constraints
+        Compound     = 1,   // linkset tied together as a compound object
+        Manual       = 2    // linkset tied together manually (code moves all the pieces)
+    }
+    // Create the correct type of linkset for this child
+    public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
+    {
+        BSLinkset ret = null;
+
+        switch ((int)physScene.Params.linksetImplementation)
+        {
+            case (int)LinksetImplementation.Constraint:
+                ret = new BSLinksetConstraints(physScene, parent);
+                break;
+            case (int)LinksetImplementation.Compound:
+                ret = new BSLinksetCompound(physScene, parent);
+                break;
+            case (int)LinksetImplementation.Manual:
+                // ret = new BSLinksetManual(physScene, parent);
+                break;
+            default:
+                ret = new BSLinksetCompound(physScene, parent);
+                break;
+        }
+        return ret;
+    }
+
     public BSPhysObject LinksetRoot { get; protected set; }
 
     public BSScene PhysicsScene { get; private set; }
@@ -44,33 +73,39 @@ public class BSLinkset
     public int LinksetID { get; private set; }
 
     // The children under the root in this linkset.
-    // There are two lists of children: the current children at runtime
-    //    and the children at taint-time. For instance, if you delink a
-    //    child from the linkset, the child is removed from m_children
-    //    but the constraint won't be removed until taint time.
-    //    Two lists lets this track the 'current' children and
-    //    the physical 'taint' children separately.
-    // After taint processing and before the simulation step, these
-    //    two lists must be the same.
-    private HashSet<BSPhysObject> m_children;
-    private HashSet<BSPhysObject> m_taintChildren;
+    protected HashSet<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
     //    to the physical representation is done via the tainting mechenism.
-    private object m_linksetActivityLock = new Object();
+    protected object m_linksetActivityLock = new Object();
 
+    // Some linksets have a preferred physical shape.
+    // Returns SHAPE_UNKNOWN if there is no preference. Causes the correct shape to be selected.
+    public virtual BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    {
+        return BSPhysicsShapeType.SHAPE_UNKNOWN;
+    }
+
+    // Linksets move around the children so the linkset might need to compute the child position
+    public virtual OMV.Vector3 Position(BSPhysObject member)
+        { return member.RawPosition; }
+    public virtual OMV.Quaternion Orientation(BSPhysObject member)
+        { return member.RawOrientation; }
+    // TODO: does this need to be done for Velocity and RotationalVelocityy?
+    
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
-    private float m_mass;
+    protected float m_mass;
     public float LinksetMass
     {
         get
         {
-            m_mass = ComputeLinksetMass();
             return m_mass;
         }
     }
 
+    public virtual bool LinksetIsColliding { get { return false; } }
+
     public OMV.Vector3 CenterOfMass
     {
         get { return ComputeLinksetCenterOfMass(); }
@@ -81,7 +116,7 @@ public class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    public BSLinkset(BSScene scene, BSPhysObject parent)
+    protected void Initialize(BSScene scene, BSPhysObject parent)
     {
         // A simple linkset of one (no children)
         LinksetID = m_nextLinksetID++;
@@ -91,8 +126,7 @@ public class BSLinkset
         PhysicsScene = scene;
         LinksetRoot = parent;
         m_children = new HashSet<BSPhysObject>();
-        m_taintChildren = new HashSet<BSPhysObject>();
-        m_mass = parent.MassRaw;
+        m_mass = parent.RawMass;
     }
 
     // Link to a linkset where the child knows the parent.
@@ -106,6 +140,7 @@ public class BSLinkset
             // Don't add the root to its own linkset
             if (!IsRoot(child))
                 AddChildToLinkset(child);
+            m_mass = ComputeLinksetMass();
         }
         return this;
     }
@@ -123,12 +158,12 @@ public class BSLinkset
                 // Cannot remove the root from a linkset.
                 return this;
             }
-
             RemoveChildFromLinkset(child);
+            m_mass = ComputeLinksetMass();
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(PhysicsScene, child);
+        return BSLinkset.Factory(PhysicsScene, child);
     }
 
     // Return 'true' if the passed object is the root object of this linkset
@@ -148,6 +183,8 @@ public class BSLinkset
         bool ret = false;
         lock (m_linksetActivityLock)
         {
+            ret = m_children.Contains(child);
+            /* Safer version but the above should work
             foreach (BSPhysObject bp in m_children)
             {
                 if (child.LocalID == bp.LocalID)
@@ -156,159 +193,102 @@ public class BSLinkset
                     break;
                 }
             }
+             */
         }
         return ret;
     }
 
-    // When physical properties are changed the linkset needs to recalculate
-    //   its internal properties.
-    // May be called at runtime or taint-time (just pass the appropriate flag).
-    public void Refresh(BSPhysObject requestor, bool inTaintTime)
+    // Perform an action on each member of the linkset including root prim.
+    // Depends on the action on whether this should be done at taint time.
+    public delegate bool ForEachMemberAction(BSPhysObject obj);
+    public virtual bool ForEachMember(ForEachMemberAction action)
     {
-        // If there are no children, not physical or not root, I am not the one that recomputes the constraints
-        // (For the moment, static linksets do create constraints so remove the test for physical.)
-        if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
-            return;
-
-        BSScene.TaintCallback refreshOperation = delegate()
+        bool ret = false;
+        lock (m_linksetActivityLock)
+        {
+            action(LinksetRoot);
+            foreach (BSPhysObject po in m_children)
             {
-                RecomputeLinksetConstraintVariables();
-                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}", 
-                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
-            };
-        if (inTaintTime)
-            refreshOperation();
-        else
-            PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
+                if (action(po))
+                    break;
+            }
+        }
+        return ret;
     }
 
+    // I am the root of a linkset and a new child is being added
+    // Called while LinkActivity is locked.
+    protected abstract void AddChildToLinkset(BSPhysObject child);
+
+    // I am the root of a linkset and one of my children is being removed.
+    // Safe to call even if the child is not really in my linkset.
+    protected abstract void RemoveChildFromLinkset(BSPhysObject child);
+
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    // May be called at runtime or taint-time.
+    public abstract void Refresh(BSPhysObject requestor);
+
     // 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)
-    {
-        // What is done for each object in BSPrim is what we want.
-        return false;
-    }
+    public abstract bool MakeDynamic(BSPhysObject child);
 
     // 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;
-    }
+    public abstract bool MakeStatic(BSPhysObject child);
 
-    // If the software is handling the movement of all the objects in a linkset
-    // (like if one doesn't use constraints for static linksets), this is called
-    // when an update for the root of the linkset is received.
+    // Called when a parameter update comes from the physics engine for any object
+    //      of the linkset is received.
     // Called at taint-time!!
-    public void UpdateProperties(BSPhysObject physObject)
-    {
-        // The root local properties have been updated. Apply to the children if appropriate.
-        if (IsRoot(physObject) && HasAnyChildren)
-        {
-            if (!physObject.IsPhysical)
-            {
-                // TODO: implement software linkset update for static object linksets
-            }
-        }
-    }
+    public abstract void UpdateProperties(BSPhysObject physObject);
 
     // Routine used when rebuilding the body of the root of the linkset
     // Destroy all the constraints have have been made to root.
     // This is called when the root body is changing.
-    // Returns 'true' of something eas actually removed and would need restoring
+    // Returns 'true' of something was actually removed and would need restoring
     // Called at taint-time!!
-    public bool RemoveBodyDependencies(BSPrim child)
-    {
-        bool ret = false;
-
-        lock (m_linksetActivityLock)
-        {
-            if (IsRoot(child))
-            {
-                // If the one with the dependency is root, must undo all children
-                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
-                                                child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
-
-                ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
-            }
-            else
-            {
-                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
-                                                child.LocalID,
-                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
-                                                child.LocalID, child.BSBody.ptr.ToString("X"));
-                // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
-                // Despite the function name, this removes any link to the specified object.
-                ret = PhysicallyUnlinkAllChildrenFromRoot(child);
-            }
-        }
-        return ret;
-    }
+    public abstract bool RemoveBodyDependencies(BSPrim child);
 
     // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
-    // this routine will restore the removed constraints.
+    //     this routine will restore the removed constraints.
     // Called at taint-time!!
-    public void RestoreBodyDependencies(BSPrim child)
-    {
-        lock (m_linksetActivityLock)
-        {
-            if (IsRoot(child))
-            {
-                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
-                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
-                foreach (BSPhysObject bpo in m_taintChildren)
-                {
-                    PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
-                }
-            }
-            else
-            {
-                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
-                                                LinksetRoot.LocalID,
-                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
-                                                child.LocalID, child.BSBody.ptr.ToString("X"));
-                PhysicallyLinkAChildToRoot(LinksetRoot, child);
-            }
-        }
-    }
+    public abstract void RestoreBodyDependencies(BSPrim child);
 
     // ================================================================
-    // Below this point is internal magic
-
-    private float ComputeLinksetMass()
+    protected virtual float ComputeLinksetMass()
     {
-        float mass;
-        lock (m_linksetActivityLock)
+        float mass = LinksetRoot.RawMass;
+        if (HasAnyChildren)
         {
-            mass = LinksetRoot.MassRaw;
-            foreach (BSPhysObject bp in m_taintChildren)
+            lock (m_linksetActivityLock)
             {
-                mass += bp.MassRaw;
+                foreach (BSPhysObject bp in m_children)
+                {
+                    mass += bp.RawMass;
+                }
             }
         }
         return mass;
     }
 
-    private OMV.Vector3 ComputeLinksetCenterOfMass()
+    protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
     {
         OMV.Vector3 com;
         lock (m_linksetActivityLock)
         {
-            com = LinksetRoot.Position * LinksetRoot.MassRaw;
-            float totalMass = LinksetRoot.MassRaw;
+            com = LinksetRoot.Position * LinksetRoot.RawMass;
+            float totalMass = LinksetRoot.RawMass;
 
-            foreach (BSPhysObject bp in m_taintChildren)
+            foreach (BSPhysObject bp in m_children)
             {
-                com += bp.Position * bp.MassRaw;
-                totalMass += bp.MassRaw;
+                com += bp.Position * bp.RawMass;
+                totalMass += bp.RawMass;
             }
             if (totalMass != 0f)
                 com /= totalMass;
@@ -317,255 +297,25 @@ public class BSLinkset
         return com;
     }
 
-    private OMV.Vector3 ComputeLinksetGeometricCenter()
+    protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
     {
         OMV.Vector3 com;
         lock (m_linksetActivityLock)
         {
             com = LinksetRoot.Position;
 
-            foreach (BSPhysObject bp in m_taintChildren)
+            foreach (BSPhysObject bp in m_children)
             {
-                com += bp.Position * bp.MassRaw;
+                com += bp.Position * bp.RawMass;
             }
-            com /= (m_taintChildren.Count + 1);
+            com /= (m_children.Count + 1);
         }
 
         return com;
     }
 
-    // I am the root of a linkset and a new child is being added
-    // Called while LinkActivity is locked.
-    private void AddChildToLinkset(BSPhysObject child)
-    {
-        if (!HasChild(child))
-        {
-            m_children.Add(child);
-
-            BSPhysObject rootx = LinksetRoot; // capture the root as of now
-            BSPhysObject childx = child;
-
-            DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
-
-            PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
-            {
-                DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}", 
-                                rootx.LocalID,
-                                rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
-                                childx.LocalID, childx.BSBody.ptr.ToString("X"));
-                // Since this is taint-time, the body and shape could have changed for the child
-                rootx.ForcePosition = rootx.Position;   // DEBUG
-                childx.ForcePosition = childx.Position;   // DEBUG
-                PhysicallyLinkAChildToRoot(rootx, childx);
-                m_taintChildren.Add(child);
-            });
-        }
-        return;
-    }
-
-    // Forcefully removing a child from a linkset.
-    // This is not being called by the child so we have to make sure the child doesn't think
-    //    it's still connected to the linkset.
-    // Normal OpenSimulator operation will never do this because other SceneObjectPart information
-    //    also has to be updated (like pointer to prim's parent).
-    private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
-    {
-        pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
-        RemoveChildFromLinkset(pchild);
-    }
-
-    // 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(BSPhysObject child)
-    {
-        if (m_children.Remove(child))
-        {
-            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
-            BSPhysObject childx = child;
-
-            DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
-                            childx.LocalID,
-                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
-                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
-
-            PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
-            {
-                m_taintChildren.Remove(child);
-                PhysicallyUnlinkAChildFromRoot(rootx, childx);
-                RecomputeLinksetConstraintVariables();
-            });
-
-        }
-        else
-        {
-            // This will happen if we remove the root of the linkset first. Non-fatal occurance.
-            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
-        }
-        return;
-    }
-
-    // Create a constraint between me (root of linkset) and the passed prim (the child).
-    // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
-    {
-        // Zero motion for children so they don't interpolate
-        childPrim.ZeroMotion();
-
-        // Relative position normalized to the root prim
-        // Essentually a vector pointing from center of rootPrim to center of childPrim
-        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
-
-        // real world coordinate of midpoint between the two objects
-        OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
-
-        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
-                                        rootPrim.LocalID,
-                                        rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
-                                        childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
-                                        rootPrim.Position, childPrim.Position, midPoint);
-
-        // create a constraint that allows no freedom of movement between the two objects
-        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-
-        BS6DofConstraint constrain = new BS6DofConstraint(
-                            PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
-
-        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
-         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
-         *     of the objects.
-         * Code left as a warning to future programmers.
-        // ==================================================================================
-        // relative position normalized to the root prim
-        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
-        OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
-
-        // relative rotation of the child to the parent
-        OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
-        OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
-
-        // create a constraint that allows no freedom of movement between the two objects
-        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
-        BS6DofConstraint constrain = new BS6DofConstraint(
-                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(rootPrim.Orientation),
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(childPrim.Orientation),
-                        // A point half way between the parent and child
-                        // childRelativePosition/2,
-                        // childRelativeRotation,
-                        // childRelativePosition/2,
-                        // inverseChildRelativeRotation,
-                        true,
-                        true
-                        );
-        // ==================================================================================
-        */
-
-        PhysicsScene.Constraints.AddConstraint(constrain);
-
-        // zero linear and angular limits makes the objects unable to move in relation to each other
-        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
-        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
-
-        // tweek the constraint to increase stability
-        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
-                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
-                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
-        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
-        {
-            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
-        }
-    }
-
-    // Remove linkage between myself and a particular child
-    // The root and child bodies are passed in because we need to remove the constraint between
-    //      the bodies that were at unlink time.
-    // Called at taint time!
-    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
-    {
-        bool ret = false;
-        DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
-                            rootPrim.LocalID,
-                            rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
-                            childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
-
-        // Find the constraint for this link and get rid of it from the overall collection and from my list
-        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
-        {
-            // Make the child refresh its location
-            BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
-            ret = true;
-        }
-
-        return ret;
-    }
-
-    // Remove linkage between myself and any possible children I might have.
-    // Called at taint time!
-    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
-    {
-        DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
-        bool ret = false;
-
-        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
-        {
-            ret = true;
-        }
-        return ret;
-    }
-
-    // Call each of the constraints that make up this linkset and recompute the
-    //    various transforms and variables. Used when objects are added or removed
-    //    from a linkset to make sure the constraints know about the new mass and
-    //    geometry.
-    // Must only be called at taint time!!
-    private void RecomputeLinksetConstraintVariables()
-    {
-        float linksetMass = LinksetMass;
-        foreach (BSPhysObject child in m_taintChildren)
-        {
-            BSConstraint constrain;
-            if (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);
-                constrain.RecomputeConstraintVariables(linksetMass);
-            }
-            else
-            {
-                // Non-fatal error that happens when children are being added to the linkset but
-                //    their constraints have not been created yet.
-                break;
-            }
-        }
-
-        // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
-        if (m_children.Count == m_taintChildren.Count)
-        {
-            // 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);
-            DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
-                                LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
-            foreach (BSPhysObject child in m_taintChildren)
-            {
-                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, 
-                                centerOfMass, OMV.Quaternion.Identity);
-            }
-
-            // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr);  // DEBUG DEBUG DEBUG
-        }
-        return;
-    }
-
-
     // Invoke the detailed logger and output something if it's enabled.
-    private void DetailLog(string msg, params Object[] args)
+    protected void DetailLog(string msg, params Object[] args)
     {
         if (PhysicsScene.PhysicsLogging.Enabled)
             PhysicsScene.DetailLog(msg, args);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
new file mode 100755
index 0000000..b9c2cf9
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -0,0 +1,275 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSLinksetCompound : BSLinkset
+{
+    private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
+
+    public BSLinksetCompound(BSScene scene, BSPhysObject parent)
+    {
+        base.Initialize(scene, parent);
+    }
+
+    // For compound implimented linksets, if there are children, use compound shape for the root.
+    public override BSPhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
+    { 
+        BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        if (IsRoot(requestor) && HasAnyChildren)
+        {
+            ret = BSPhysicsShapeType.SHAPE_COMPOUND;
+        }
+        // DetailLog("{0},BSLinksetCompound.PreferredPhysicalShape,call,shape={1}", LinksetRoot.LocalID, ret);
+        return ret;
+    }
+
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    // This is queued in the 'post taint' queue so the
+    //   refresh will happen once after all the other taints are applied.
+    public override void Refresh(BSPhysObject requestor)
+    {
+        // External request for Refresh (from BSPrim) is not necessary
+        // InternalRefresh(requestor);
+    }
+
+    private void InternalRefresh(BSPhysObject requestor)
+    {
+        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,requestor={1}", LinksetRoot.LocalID, requestor.LocalID);
+        // Queue to happen after all the other taint processing
+        PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", requestor.LocalID, delegate()
+        {
+            if (IsRoot(requestor) && HasAnyChildren)
+                RecomputeLinksetCompound();
+        });
+    }
+
+    // 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 override bool MakeDynamic(BSPhysObject child)
+    {
+        bool ret = false;
+        DetailLog("{0},BSLinksetCompound.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        if (!IsRoot(child))
+        {
+            // Physical children are removed from the world as the shape ofthe root compound
+            //     shape takes over.
+            BulletSimAPI.AddToCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            BulletSimAPI.ForceActivationState2(child.PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
+            ret = true;
+        }
+        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.
+    // This doesn't normally happen -- OpenSim removes the objects from the physical
+    //     world if it is a static linkset.
+    // Called at taint-time!
+    public override bool MakeStatic(BSPhysObject child)
+    {
+        bool ret = false;
+        DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        if (!IsRoot(child))
+        {
+            // The non-physical children can come back to life.
+            BulletSimAPI.RemoveFromCollisionFlags2(child.PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            // Don't force activation so setting of DISABLE_SIMULATION can stay.
+            BulletSimAPI.Activate2(child.PhysBody.ptr, false);
+            ret = true;
+        }
+        return ret;
+    }
+
+    // Called at taint-time!!
+    public override void UpdateProperties(BSPhysObject updated)
+    {
+        // Nothing to do for constraints on property updates
+    }
+
+    // The children move around in relationship to the root.
+    // Just grab the current values of wherever it is right now.
+    public override OMV.Vector3 Position(BSPhysObject member)
+    {
+        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
+    }
+
+    public override OMV.Quaternion Orientation(BSPhysObject member)
+    {
+        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+    }
+
+    // Routine called when rebuilding the body of some member of the linkset.
+    // Since we don't keep in world relationships, do nothing unless it's a child changing.
+    // Returns 'true' of something was actually removed and would need restoring
+    // Called at taint-time!!
+    public override bool RemoveBodyDependencies(BSPrim child)
+    {
+        bool ret = false;
+
+        DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
+                        child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), IsRoot(child));
+
+        if (!IsRoot(child))
+        {
+            // Cause the current shape to be freed and the new one to be built.
+            InternalRefresh(LinksetRoot);
+            ret = true;
+        }
+
+        return ret;
+    }
+
+    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
+    // this routine will restore the removed constraints.
+    // Called at taint-time!!
+    public override void RestoreBodyDependencies(BSPrim child)
+    {
+        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
+    }
+
+    // ================================================================
+
+    // Add a new child to the linkset.
+    // Called while LinkActivity is locked.
+    protected override void AddChildToLinkset(BSPhysObject child)
+    {
+        if (!HasChild(child))
+        {
+            m_children.Add(child);
+
+            DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
+
+            // Cause constraints and assorted properties to be recomputed before the next simulation step.
+            InternalRefresh(LinksetRoot);
+        }
+        return;
+    }
+
+    // Remove the specified child from the linkset.
+    // Safe to call even if the child is not really in my linkset.
+    protected override void RemoveChildFromLinkset(BSPhysObject child)
+    {
+        if (m_children.Remove(child))
+        {
+            DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
+                            child.LocalID,
+                            LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"),
+                            child.LocalID, child.PhysBody.ptr.ToString("X"));
+
+            // Cause the child's body to be rebuilt and thus restored to normal operation
+            child.ForceBodyShapeRebuild(false);
+
+            if (!HasAnyChildren)
+            {
+                // The linkset is now empty. The root needs rebuilding.
+                LinksetRoot.ForceBodyShapeRebuild(false);
+            }
+            else
+            {
+                // Schedule a rebuild of the linkset  before the next simulation tick.
+                InternalRefresh(LinksetRoot);
+            }
+        }
+        return;
+    }
+
+    // Called before the simulation step to make sure the compound based linkset
+    //    is all initialized.
+    // Constraint linksets are rebuilt every time.
+    // Note that this works for rebuilding just the root after a linkset is taken apart.
+    // Called at taint time!!
+    private void RecomputeLinksetCompound()
+    {
+        // Cause the root shape to be rebuilt as a compound object with just the root in it
+        LinksetRoot.ForceBodyShapeRebuild(true);
+
+        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
+                        LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
+
+        // Add a shape for each of the other children in the linkset
+        ForEachMember(delegate(BSPhysObject cPrim)
+        {
+            if (!IsRoot(cPrim))
+            {
+                // Each child position and rotation is given relative to the root.
+                OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+                OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
+                OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
+
+                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
+                    LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, displacementPos, displacementRot);
+
+                if (cPrim.PhysShape.isNativeShape)
+                {
+                    // Native shapes are not shared so we need to create a new one.
+                    // A mesh or hull is created because scale is not available on a native shape.
+                    //     (TODO: Bullet does have a btScaledCollisionShape. Can that be used?)
+                    BulletShape saveShape = cPrim.PhysShape;
+                    cPrim.PhysShape.ptr = IntPtr.Zero;  // Don't let the create free the child's shape
+                    PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
+                    BulletShape newShape = cPrim.PhysShape;
+                    cPrim.PhysShape = saveShape;
+                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot);
+                }
+                else
+                {
+                    // For the shared shapes (meshes and hulls), just use the shape in the child.
+                    if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
+                    {
+                        PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
+                                            LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
+                    }
+                    BulletSimAPI.AddChildShapeToCompoundShape2(LinksetRoot.PhysShape.ptr, cPrim.PhysShape.ptr, displacementPos, displacementRot);
+                }
+            }
+            return false;   // 'false' says to move onto the next child in the list
+        });
+
+        // With all of the linkset packed into the root prim, it has the mass of everyone.
+        float linksetMass = LinksetMass;
+        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+
+        BulletSimAPI.RecalculateCompoundShapeLocalAabb2(LinksetRoot.PhysShape.ptr);
+
+        // DEBUG: see of inter-linkset collisions are causing problems for constraint linksets.
+        // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
+        //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
+
+    }
+}
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
new file mode 100755
index 0000000..c855fda
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -0,0 +1,327 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSLinksetConstraints : BSLinkset
+{
+    // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
+
+    public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
+    {
+        base.Initialize(scene, parent);
+    }
+
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    // This is queued in the 'post taint' queue so the
+    //   refresh will happen once after all the other taints are applied.
+    public override void Refresh(BSPhysObject requestor)
+    {
+        // Queue to happen after all the other taint processing
+        PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
+            {
+                if (HasAnyChildren && IsRoot(requestor))
+                    RecomputeLinksetConstraints();
+            });
+    }
+
+    // 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 override bool MakeDynamic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
+    // 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.
+    // This doesn't normally happen -- OpenSim removes the objects from the physical
+    //     world if it is a static linkset.
+    // Called at taint-time!
+    public override bool MakeStatic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
+    // Called at taint-time!!
+    public override void UpdateProperties(BSPhysObject updated)
+    {
+        // Nothing to do for constraints on property updates
+    }
+
+    // The children of the linkset are moved around by the constraints.
+    // Just grab the current values of wherever it is right now.
+    public override OMV.Vector3 Position(BSPhysObject member)
+    {
+        return BulletSimAPI.GetPosition2(member.PhysBody.ptr);
+    }
+
+    public override OMV.Quaternion Orientation(BSPhysObject member)
+    {
+        return BulletSimAPI.GetOrientation2(member.PhysBody.ptr);
+    }
+
+    // Routine called when rebuilding the body of some member of the linkset.
+    // Destroy all the constraints have have been made to root and set
+    //     up to rebuild the constraints before the next simulation step.
+    // Returns 'true' of something was actually removed and would need restoring
+    // Called at taint-time!!
+    public override bool RemoveBodyDependencies(BSPrim child)
+    {
+        bool ret = false;
+
+        DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
+                                    child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"));
+
+        lock (m_linksetActivityLock)
+        {
+            // Just undo all the constraints for this linkset. Rebuild at the end of the step.
+            ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
+            // Cause the constraints, et al to be rebuilt before the next simulation step.
+            Refresh(LinksetRoot);
+        }
+        return ret;
+    }
+
+    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
+    // this routine will restore the removed constraints.
+    // Called at taint-time!!
+    public override void RestoreBodyDependencies(BSPrim child)
+    {
+        // The Refresh operation queued by RemoveBodyDependencies() will build any missing constraints.
+    }
+
+    // ================================================================
+
+    // Add a new child to the linkset.
+    // Called while LinkActivity is locked.
+    protected override void AddChildToLinkset(BSPhysObject child)
+    {
+        if (!HasChild(child))
+        {
+            m_children.Add(child);
+
+            DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
+
+            // Cause constraints and assorted properties to be recomputed before the next simulation step.
+            Refresh(LinksetRoot);
+        }
+        return;
+    }
+
+    // Remove the specified child from the linkset.
+    // Safe to call even if the child is not really in my linkset.
+    protected override void RemoveChildFromLinkset(BSPhysObject child)
+    {
+        if (m_children.Remove(child))
+        {
+            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
+            BSPhysObject childx = child;
+
+            DetailLog("{0},BSLinksetConstraints.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
+                            childx.LocalID,
+                            rootx.LocalID, rootx.PhysBody.ptr.ToString("X"),
+                            childx.LocalID, childx.PhysBody.ptr.ToString("X"));
+
+            PhysicsScene.TaintedObject("BSLinksetConstraints.RemoveChildFromLinkset", delegate()
+            {
+                PhysicallyUnlinkAChildFromRoot(rootx, childx);
+            });
+            // See that the linkset parameters are recomputed at the end of the taint time.
+            Refresh(LinksetRoot);
+        }
+        else
+        {
+            // Non-fatal occurance.
+            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
+        }
+        return;
+    }
+
+    // Create a constraint between me (root of linkset) and the passed prim (the child).
+    // Called at taint time!
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    {
+        // Don't build the constraint when asked. Put it off until just before the simulation step.
+        Refresh(rootPrim);
+    }
+
+    private BSConstraint BuildConstraint(BSPhysObject rootPrim, BSPhysObject childPrim)
+    {
+        // Zero motion for children so they don't interpolate
+        childPrim.ZeroMotion(true);
+
+        // Relative position normalized to the root prim
+        // Essentually a vector pointing from center of rootPrim to center of childPrim
+        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
+
+        // real world coordinate of midpoint between the two objects
+        OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
+
+        DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+                                        rootPrim.LocalID,
+                                        rootPrim.LocalID, rootPrim.PhysBody.ptr.ToString("X"),
+                                        childPrim.LocalID, childPrim.PhysBody.ptr.ToString("X"),
+                                        rootPrim.Position, childPrim.Position, midPoint);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+
+        BSConstraint6Dof constrain = new BSConstraint6Dof(
+                            PhysicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
+                            // PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
+
+        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
+         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
+         *     of the objects.
+         * Code left for future programmers.
+        // ==================================================================================
+        // relative position normalized to the root prim
+        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
+        OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
+
+        // relative rotation of the child to the parent
+        OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
+        OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
+
+        DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(rootPrim.Orientation),
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(childPrim.Orientation),
+                        true,
+                        true
+                        );
+        // ==================================================================================
+        */
+
+        PhysicsScene.Constraints.AddConstraint(constrain);
+
+        // zero linear and angular limits makes the objects unable to move in relation to each other
+        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+
+        // tweek the constraint to increase stability
+        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
+        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
+                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
+                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
+        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
+        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        {
+            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+        }
+        return constrain;
+    }
+
+    // Remove linkage between the linkset root and a particular child
+    // The root and child bodies are passed in because we need to remove the constraint between
+    //      the bodies that were present at unlink time.
+    // Called at taint time!
+    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    {
+        bool ret = false;
+        DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
+                            rootPrim.LocalID,
+                            rootPrim.LocalID, rootPrim.PhysBody.ptr.ToString("X"),
+                            childPrim.LocalID, childPrim.PhysBody.ptr.ToString("X"));
+
+        // Find the constraint for this link and get rid of it from the overall collection and from my list
+        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
+        {
+            // Make the child refresh its location
+            BulletSimAPI.PushUpdate2(childPrim.PhysBody.ptr);
+            ret = true;
+        }
+
+        return ret;
+    }
+
+    // Remove linkage between myself and any possible children I might have.
+    // Returns 'true' of any constraints were destroyed.
+    // Called at taint time!
+    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
+    {
+        DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+
+        return PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
+    }
+
+    // Call each of the constraints that make up this linkset and recompute the
+    //    various transforms and variables. Create constraints of not created yet.
+    // Called before the simulation step to make sure the constraint based linkset
+    //    is all initialized.
+    // Called at taint time!!
+    private void RecomputeLinksetConstraints()
+    {
+        float linksetMass = LinksetMass;
+        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+
+        // DEBUG: see of inter-linkset collisions are causing problems
+        // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
+        //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
+        DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
+                            LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString("X"), linksetMass);
+
+        foreach (BSPhysObject child in m_children)
+        {
+            // A child in the linkset physically shows the mass of the whole linkset.
+            // This allows Bullet to apply enough force on the child to move the whole linkset.
+            // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
+            child.UpdatePhysicalMassProperties(linksetMass);
+
+            BSConstraint constrain;
+            if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+            {
+                // If constraint doesn't exist yet, create it.
+                constrain = BuildConstraint(LinksetRoot, child);
+            }
+            constrain.RecomputeConstraintVariables(linksetMass);
+
+            // DEBUG: see of inter-linkset collisions are causing problems
+            // BulletSimAPI.SetCollisionFilterMask2(child.BSBody.ptr, 
+            //                 (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
+
+            // BulletSimAPI.DumpConstraint2(PhysicsScene.World.ptr, constrain.Constraint.ptr);    // DEBUG DEBUG
+        }
+
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
new file mode 100755
index 0000000..bc6e4c4
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -0,0 +1,104 @@
+using System;

+using System.Collections.Generic;

+using System.Text;

+using OpenMetaverse;

+

+namespace OpenSim.Region.Physics.BulletSPlugin

+{

+public abstract class BSMotor

+{

+    public virtual void Reset() { }

+    public virtual void Zero() { }

+}

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

+public class BSVMotor : BSMotor

+{

+    public Vector3 FrameOfReference { get; set; }

+    public Vector3 Offset { get; set; }

+

+    public float TimeScale { get; set; }

+    public float TargetValueDecayTimeScale { get; set; }

+    public Vector3 CurrentValueReductionTimescale { get; set; }

+    public float Efficiency { get; set; }

+

+    public Vector3 TargetValue { get; private set; }

+    public Vector3 CurrentValue { get; private set; }

+

+

+

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

+    {

+        TimeScale = timeScale;

+        TargetValueDecayTimeScale = decayTimeScale;

+        CurrentValueReductionTimescale = frictionTimeScale;

+        Efficiency = efficiency;

+    }

+    public void SetCurrent(Vector3 current)

+    {

+        CurrentValue = current;

+    }

+    public void SetTarget(Vector3 target)

+    {

+        TargetValue = target;

+    }

+    public Vector3 Step(float timeStep)

+    {

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

+        {

+            // Vector3 origDir = Target;       // DEBUG

+            // Vector3 origVel = CurrentValue;   // DEBUG

+

+            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete

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

+            CurrentValue += addAmount;

+

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

+            TargetValue *= (1f - decayFactor);

+

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

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

+        }

+        else

+        {

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

+            TargetValue = Vector3.Zero;

+            CurrentValue = Vector3.Zero;

+

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

+        }

+        return CurrentValue;

+    }

+}

+

+public class BSFMotor : BSMotor

+{

+    public float TimeScale { get; set; }

+    public float DecayTimeScale { get; set; }

+    public float Friction { get; set; }

+    public float Efficiency { get; set; }

+

+    public float Target { get; private set; }

+    public float CurrentValue { get; private set; }

+

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

+    {

+    }

+    public void SetCurrent(float target)

+    {

+    }

+    public void SetTarget(float target)

+    {

+    }

+    public float Step(float timeStep)

+    {

+        return 0f;

+    }

+}

+public class BSPIDMotor : BSMotor

+{

+    // TODO: write and use this one

+    BSPIDMotor()

+    {

+    }

+}

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index ead6a08..f6a890e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -34,19 +34,30 @@ 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.
+/*
+ * Class to wrap all objects.
+ * The rest of BulletSim doesn't need to keep checking for avatars or prims
+ *        unless the difference is significant.
+ * 
+ *  Variables in the physicsl objects are in three forms:
+ *      VariableName: used by the simulator and performs taint operations, etc
+ *      RawVariableName: direct reference to the BulletSim storage for the variable value
+ *      ForceVariableName: direct reference (store and fetch) to the value in the physics engine.
+ *  The last two (and certainly the last one) should be referenced only in taint-time.
+ */
 public abstract class BSPhysObject : PhysicsActor
 {
-    protected void BaseInitialize(BSScene parentScene, uint localID, string name, string typeName)
+    protected BSPhysObject()
+    {
+    }
+    protected BSPhysObject(BSScene parentScene, uint localID, string name, string typeName)
     {
         PhysicsScene = parentScene;
         LocalID = localID;
         PhysObjectName = name;
         TypeName = typeName;
 
-        Linkset = new BSLinkset(PhysicsScene, this);
+        Linkset = BSLinkset.Factory(PhysicsScene, this);
         LastAssetBuildFailed = false;
 
         CollisionCollection = new CollisionEventUpdate();
@@ -63,12 +74,17 @@ public abstract class BSPhysObject : PhysicsActor
     public BSLinkset Linkset { get; set; }
 
     // Return the object mass without calculating it or having side effects
-    public abstract float MassRaw { get; }
+    public abstract float RawMass { get; }
+    // Set the raw mass but also update physical mass properties (inertia, ...)
+    public abstract void UpdatePhysicalMassProperties(float mass);
+
+    // The last value calculated for the prim's inertia
+    public OMV.Vector3 Inertia { get; set; }
 
     // Reference to the physical body (btCollisionObject) of this object
-    public BulletBody BSBody;
+    public BulletBody PhysBody;
     // Reference to the physical shape (btCollisionShape) of this object
-    public BulletShape BSShape;
+    public BulletShape PhysShape;
 
     // 'true' if the mesh's underlying asset failed to build.
     // This will keep us from looping after the first time the build failed.
@@ -76,9 +92,15 @@ public abstract class BSPhysObject : PhysicsActor
 
     // The objects base shape information. Null if not a prim type shape.
     public PrimitiveBaseShape BaseShape { get; protected set; }
+    // Some types of objects have preferred physical representations.
+    // Returns SHAPE_UNKNOWN if there is no preference.
+    public virtual BSPhysicsShapeType PreferredPhysicalShape
+    {
+        get { return BSPhysicsShapeType.SHAPE_UNKNOWN; }
+    }
 
     // When the physical properties are updated, an EntityProperty holds the update values.
-    // Keep the current and last EntityProperties to enable computation of differences 
+    // Keep the current and last EntityProperties to enable computation of differences
     //      between the current update and the previous values.
     public EntityProperties CurrentEntityProperties { get; set; }
     public EntityProperties LastEntityProperties { get; set; }
@@ -88,7 +110,8 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract bool IsStatic { get; }
 
     // Stop all physical motion.
-    public abstract void ZeroMotion();
+    public abstract void ZeroMotion(bool inTaintTime);
+    public abstract void ZeroAngularMotion(bool inTaintTime);
 
     // Step the vehicle simulation for this object. A NOOP if the vehicle was not configured.
     public virtual void StepVehicle(float timeStep) { }
@@ -99,8 +122,10 @@ public abstract class BSPhysObject : PhysicsActor
     // Tell the object to clean up.
     public abstract void Destroy();
 
+    public abstract OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
+    public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
     public abstract OMV.Vector3 ForceVelocity { get; set; }
@@ -204,7 +229,7 @@ public abstract class BSPhysObject : PhysicsActor
 
             PhysicsScene.TaintedObject(TypeName+".SubscribeEvents", delegate()
             {
-                CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+                CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
             });
         }
         else
@@ -213,16 +238,16 @@ public abstract class BSPhysObject : PhysicsActor
             UnSubscribeEvents();
         }
     }
-    public override void UnSubscribeEvents() { 
+    public override void UnSubscribeEvents() {
         // DetailLog("{0},{1}.UnSubscribeEvents,unsubscribing", LocalID, TypeName);
         SubscribedEventsMs = 0;
         PhysicsScene.TaintedObject(TypeName+".UnSubscribeEvents", delegate()
         {
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         });
     }
     // Return 'true' if the simulator wants collision events
-    public override bool SubscribedEvents() { 
+    public override bool SubscribedEvents() {
         return (SubscribedEventsMs > 0);
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index aeeb4dd..2b3fa25 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -25,8 +25,6 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-// Uncomment this it enable code to do all shape an body memory management
-//    in the C# code.
 using System;
 using System.Reflection;
 using System.Collections.Generic;
@@ -49,7 +47,6 @@ public sealed class BSPrim : BSPhysObject
     // _size is what the user passed. Scale is what we pass to the physics engine with the mesh.
     // Often Scale is unity because the meshmerizer will apply _size when creating the mesh.
     private OMV.Vector3 _size;  // the multiplier for each mesh dimension as passed by the user
-    // private OMV.Vector3 _scale; // the multiplier for each mesh dimension for the mesh as created by the meshmerizer
 
     private bool _grabbed;
     private bool _isSelected;
@@ -90,13 +87,13 @@ public sealed class BSPrim : BSPhysObject
 
     public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
+            : base(parent_scene, localID, primName, "BSPrim")
     {
         // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID);
-        base.BaseInitialize(parent_scene, localID, primName, "BSPrim");
         _physicsActorType = (int)ActorTypes.Prim;
         _position = pos;
         _size = size;
-        Scale = new OMV.Vector3(1f, 1f, 1f);   // the scale will be set by CreateGeom depending on object type
+        Scale = size;   // the scale will be set by CreateGeom depending on object type
         _orientation = rotation;
         _buoyancy = 1f;
         _velocity = OMV.Vector3.Zero;
@@ -111,8 +108,8 @@ public sealed class BSPrim : BSPhysObject
         _mass = CalculateMass();
 
         // No body or shape yet
-        BSBody = new BulletBody(LocalID, IntPtr.Zero);
-        BSShape = new BulletShape(IntPtr.Zero);
+        PhysBody = new BulletBody(LocalID, IntPtr.Zero);
+        PhysShape = new BulletShape(IntPtr.Zero);
 
         DetailLog("{0},BSPrim.constructor,call", LocalID);
         // do the actual object creation at taint time
@@ -120,7 +117,7 @@ public sealed class BSPrim : BSPhysObject
         {
             CreateGeomAndObject(true);
 
-            CurrentCollisionFlags = BulletSimAPI.GetCollisionFlags2(BSBody.ptr);
+            CurrentCollisionFlags = BulletSimAPI.GetCollisionFlags2(PhysBody.ptr);
         });
     }
 
@@ -145,8 +142,8 @@ public sealed class BSPrim : BSPhysObject
         {
             DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
             // If there are physical body and shape, release my use of same.
-            PhysicsScene.Shapes.DereferenceBody(BSBody, true, null);
-            PhysicsScene.Shapes.DereferenceShape(BSShape, true, null);
+            PhysicsScene.Shapes.DereferenceBody(PhysBody, true, null);
+            PhysicsScene.Shapes.DereferenceShape(PhysShape, true, null);
         });
     }
 
@@ -157,6 +154,8 @@ public sealed class BSPrim : BSPhysObject
     public override OMV.Vector3 Size {
         get { return _size; }
         set {
+            // We presume the scale and size are the same. If scale must be changed for
+            //     the physical shape, that is done when the geometry is built.
             _size = value;
             ForceBodyShapeRebuild(false);
         }
@@ -171,17 +170,18 @@ public sealed class BSPrim : BSPhysObject
             ForceBodyShapeRebuild(false);
         }
     }
+    // Whatever the linkset wants is what I want.
+    public override BSPhysicsShapeType PreferredPhysicalShape
+        { get { return Linkset.PreferredPhysicalShape(this); } }
+
     public override bool ForceBodyShapeRebuild(bool inTaintTime)
     {
-        BSScene.TaintCallback rebuildOperation = delegate()
+        LastAssetBuildFailed = false;
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ForceBodyShapeRebuild", delegate()
         {
             _mass = CalculateMass();   // changing the shape changes the mass
             CreateGeomAndObject(true);
-        };
-        if (inTaintTime)
-            rebuildOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.ForceBodyShapeRebuild", rebuildOperation);
+        });
         return true;
     }
     public override bool Grabbed {
@@ -235,14 +235,27 @@ public sealed class BSPrim : BSPhysObject
     // 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 override void ZeroMotion()
+    public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
-        // Zero some other properties directly into the physics engine
-        BulletSimAPI.ClearForces2(BSBody.ptr);
+        // Zero some other properties in the physics engine
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        {
+            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+        });
+    }
+    public override void ZeroAngularMotion(bool inTaintTime)
+    {
+        _rotationalVelocity = OMV.Vector3.Zero;
+        // Zero some other properties in the physics engine
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        {
+            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+        });
     }
 
     public override void LockAngularMotion(OMV.Vector3 axis)
@@ -251,97 +264,91 @@ public sealed class BSPrim : BSPhysObject
         return;
     }
 
+    public override OMV.Vector3 RawPosition
+    {
+        get { return _position; }
+        set { _position = value; }
+    }
     public override OMV.Vector3 Position {
         get {
+            // child prims move around based on their parent. Need to get the latest location
             if (!Linkset.IsRoot(this))
-                // child prims move around based on their parent. Need to get the latest location
-                _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+                _position = Linkset.Position(this);
 
-            // don't do the GetObjectPosition for root elements because this function is called a zillion times
+            // don't do the GetObjectPosition for root elements because this function is called a zillion times.
             // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
             return _position;
         }
         set {
-            // If you must push the position into the physics engine, use ForcePosition.
+            // If the position must be forced into the physics engine, use ForcePosition.
             if (_position == value)
             {
                 return;
             }
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
-            PositionSanityCheck();
+            PositionSanityCheck(false);
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+                ActivateIfPhysical(false);
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = BulletSimAPI.GetPosition2(BSBody.ptr);
+            _position = BulletSimAPI.GetPosition2(PhysBody.ptr);
             return _position;
         }
         set {
             _position = value;
-            PositionSanityCheck();
-            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+            // PositionSanityCheck();   // Don't do this! Causes a loop and caller should know better.
+            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+            ActivateIfPhysical(false);
         }
     }
 
     // Check that the current position is sane and, if not, modify the position to make it so.
     // Check for being below terrain and being out of bounds.
     // Returns 'true' of the position was made sane by some action.
-    private bool PositionSanityCheck()
+    private bool PositionSanityCheck(bool inTaintTime)
     {
         bool ret = false;
-      
-        // If totally below the ground, move the prim up
-        // TODO: figure out the right solution for this... only for dynamic objects?
-        /*
+
         float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        OMV.Vector3 upForce = OMV.Vector3.Zero;
         if (Position.Z < terrainHeight)
         {
             DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + 2.0f;
+            float targetHeight = terrainHeight + (Size.Z / 2f);
+            // Upforce proportional to the distance away from the terrain. Correct the error in 1 sec.
+            upForce.Z = (terrainHeight - Position.Z) * 1f;
             ret = true;
         }
-         */
+
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
             float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
-            if (Position.Z < waterHeight)
+            // TODO: a floating motor so object will bob in the water
+            if (Math.Abs(Position.Z - waterHeight) > 0.1f)
             {
-                _position.Z = waterHeight;
+                // Upforce proportional to the distance away from the water. Correct the error in 1 sec.
+                upForce.Z = (waterHeight - Position.Z) * 1f;
                 ret = true;
             }
         }
 
         // TODO: check for out of bounds
-        return ret;
-    }
 
-    // A version of the sanity check that also makes sure a new position value is
-    //    pushed to the physics engine. This routine would be used by anyone
-    //    who is not already pushing the value.
-    private bool PositionSanityCheck(bool inTaintTime)
-    {
-        bool ret = false;
-        if (PositionSanityCheck())
+        // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
+        if (ret)
         {
-            // The new position value must be pushed into the physics engine but we can't
-            //    just assign to "Position" because of potential call loops.
-            BSScene.TaintCallback sanityOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck:belowTerrain", delegate()
             {
-                DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
-            };
-            if (inTaintTime)
-                sanityOperation();
-            else
-                PhysicsScene.TaintedObject("BSPrim.PositionSanityCheck", sanityOperation);
-
-            ret = true;
+                // Apply upforce and overcome gravity.
+                ForceVelocity = ForceVelocity + upForce - PhysicsScene.DefaultGravity;
+            });
         }
         return ret;
     }
@@ -352,13 +359,35 @@ public sealed class BSPrim : BSPhysObject
     {
         get
         {
-            // return Linkset.LinksetMass;
-            return _mass;
+            return Linkset.LinksetMass;
+            // 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 float RawMass { 
+        get { return _mass; }
+    }
+    // Set the physical mass to the passed mass.
+    // Note that this does not change _mass!
+    public override void UpdatePhysicalMassProperties(float physMass)
+    {
+        if (IsStatic)
+        {
+            Inertia = OMV.Vector3.Zero;
+            BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, Inertia);
+            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
+        }
+        else
+        {
+            Inertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
+            BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, Inertia);
+            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
+            // center of mass is at the zero of the object
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
+            DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, Inertia);
+        }
+    }
 
     // Is this used?
     public override OMV.Vector3 CenterOfMass
@@ -379,7 +408,7 @@ public sealed class BSPrim : BSPhysObject
             PhysicsScene.TaintedObject("BSPrim.setForce", delegate()
             {
                 // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                BulletSimAPI.SetObjectForce2(BSBody.ptr, _force);
+                BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
             });
         }
     }
@@ -398,7 +427,8 @@ public sealed class BSPrim : BSPhysObject
             {
                 // Done at taint time so we're sure the physics engine is not using the variables
                 // Vehicle code changes the parameters for this vehicle type.
-                this._vehicle.ProcessTypeChange(type);
+                _vehicle.ProcessTypeChange(type);
+                ActivateIfPhysical(false);
             });
         }
     }
@@ -407,6 +437,7 @@ public sealed class BSPrim : BSPhysObject
         PhysicsScene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
             _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+            ActivateIfPhysical(false);
         });
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value)
@@ -414,6 +445,7 @@ public sealed class BSPrim : BSPhysObject
         PhysicsScene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
             _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+            ActivateIfPhysical(false);
         });
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation)
@@ -421,6 +453,7 @@ public sealed class BSPrim : BSPhysObject
         PhysicsScene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
         {
             _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+            ActivateIfPhysical(false);
         });
     }
     public override void VehicleFlags(int param, bool remove)
@@ -435,9 +468,16 @@ public sealed class BSPrim : BSPhysObject
     // Called from Scene when doing simulation step so we're in taint processing time.
     public override void StepVehicle(float timeStep)
     {
-        if (IsPhysical)
+        if (IsPhysical && _vehicle.IsActive)
         {
             _vehicle.Step(timeStep);
+            /* // TEST TEST DEBUG DEBUG -- trying to reduce the extra action of Bullet simulation step
+            PhysicsScene.PostTaintObject("BSPrim.StepVehicles", LocalID, delegate()
+            {
+                // This resets the interpolation values and recomputes the tensor variables
+                BulletSimAPI.SetCenterOfMassByPosRot2(BSBody.ptr, ForcePosition, ForceOrientation);
+            });
+             */
         }
     }
 
@@ -462,7 +502,7 @@ public sealed class BSPrim : BSPhysObject
             PhysicsScene.TaintedObject("BSPrim.setVelocity", delegate()
             {
                 // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
-                BulletSimAPI.SetLinearVelocity2(BSBody.ptr, _velocity);
+                BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
             });
         }
     }
@@ -470,12 +510,14 @@ public sealed class BSPrim : BSPhysObject
         get { return _velocity; }
         set {
             _velocity = value;
-            BulletSimAPI.SetLinearVelocity2(BSBody.ptr, _velocity);
+            BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, _velocity);
         }
     }
     public override OMV.Vector3 Torque {
         get { return _torque; }
-        set { _torque = value;
+        set {
+            _torque = value;
+            AddAngularForce(_torque, false, false);
             // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         }
     }
@@ -488,12 +530,17 @@ public sealed class BSPrim : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
+    public override OMV.Quaternion RawOrientation
+    {
+        get { return _orientation; }
+        set { _orientation = value; }
+    }
     public override OMV.Quaternion Orientation {
         get {
+            // Children move around because tied to parent. Get a fresh value.
             if (!Linkset.IsRoot(this))
             {
-                // Children move around because tied to parent. Get a fresh value.
-                _orientation = BulletSimAPI.GetOrientation2(BSBody.ptr);
+                _orientation = Linkset.Orientation(this);
             }
             return _orientation;
         }
@@ -506,22 +553,22 @@ public sealed class BSPrim : BSPhysObject
             {
                 // _position = BulletSimAPI.GetObjectPosition2(PhysicsScene.World.ptr, BSBody.ptr);
                 // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+                BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
             });
         }
     }
-    // Go directly to Bullet to get/set the value. 
+    // Go directly to Bullet to get/set the value.
     public override OMV.Quaternion ForceOrientation
     {
         get
         {
-            _orientation = BulletSimAPI.GetOrientation2(BSBody.ptr);
+            _orientation = BulletSimAPI.GetOrientation2(PhysBody.ptr);
             return _orientation;
         }
         set
         {
             _orientation = value;
-            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
         }
     }
     public override int PhysicsActorType {
@@ -538,6 +585,8 @@ public sealed class BSPrim : BSPhysObject
                 {
                     // DetailLog("{0},setIsPhysical,taint,isPhys={1}", LocalID, _isPhysical);
                     SetObjectDynamic(true);
+                    // whether phys-to-static or static-to-phys, the object is not moving.
+                    ZeroMotion(true);
                 });
             }
         }
@@ -575,7 +624,7 @@ public sealed class BSPrim : BSPhysObject
 
         // Mangling all the physical properties requires the object not be in the physical world.
         // This is a NOOP if the object is not in the world (BulletSim and Bullet ignore objects not found).
-        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, BSBody.ptr);
+        BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
@@ -589,24 +638,25 @@ public sealed class BSPrim : BSPhysObject
         // Make solid or not (do things bounce off or pass through this object).
         MakeSolid(IsSolid);
 
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, BSBody.ptr);
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Rebuild its shape
-        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, BSBody.ptr);
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, PhysBody.ptr);
 
         // Collision filter can be set only when the object is in the world
-        if (BSBody.collisionFilter != 0 || BSBody.collisionMask != 0)
+        if (PhysBody.collisionFilter != 0 || PhysBody.collisionMask != 0)
         {
-            BulletSimAPI.SetCollisionFilterMask2(BSBody.ptr, (uint)BSBody.collisionFilter, (uint)BSBody.collisionMask);
+            BulletSimAPI.SetCollisionFilterMask2(PhysBody.ptr, (uint)PhysBody.collisionFilter, (uint)PhysBody.collisionMask);
         }
 
         // Recompute any linkset parameters.
         // When going from non-physical to physical, this re-enables the constraints that
         //     had been automatically disabled when the mass was set to zero.
-        Linkset.Refresh(this, true);
+        // For compound based linksets, this enables and disables interactions of the children.
+        Linkset.Refresh(this);
 
-        DetailLog("{0},BSPrim.UpdatePhysicalParameters,exit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
-                        LocalID, IsStatic, IsSolid, _mass, SubscribedEvents(), CurrentCollisionFlags, BSBody, BSShape);
+        DetailLog("{0},BSPrim.UpdatePhysicalParameters,taintExit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
+                        LocalID, IsStatic, IsSolid, _mass, SubscribedEvents(), CurrentCollisionFlags, PhysBody, PhysShape);
     }
 
     // "Making dynamic" means changing to and from static.
@@ -619,75 +669,74 @@ public sealed class BSPrim : BSPhysObject
         if (makeStatic)
         {
             // Become a Bullet 'static' object type
-            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
             // Stop all movement
-            BulletSimAPI.ClearAllForces2(BSBody.ptr);
+            ZeroMotion(true);
             // Center of mass is at the center of the object
-            BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.BSBody.ptr, _position, _orientation);
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.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);
+            UpdatePhysicalMassProperties(0f);
             // Set collision detection parameters
             if (PhysicsScene.Params.ccdMotionThreshold > 0f)
             {
-                BulletSimAPI.SetCcdMotionThreshold2(BSBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-                BulletSimAPI.SetCcdSweepSphereRadius2(BSBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+                BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
+                BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
             }
             // There can be special things needed for implementing linksets
             Linkset.MakeStatic(this);
             // The activation state is 'disabled' so Bullet will not try to act on it.
-            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
+            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.DISABLE_SIMULATION);
             // Start it out sleeping and physical actions could wake it up.
             // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
 
-            BSBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
-            BSBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
+            PhysBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
+            PhysBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
         }
         else
         {
             // Not a Bullet static object
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
 
             // Set various physical properties so internal dynamic properties will get computed correctly as they are set
-            BulletSimAPI.SetFriction2(BSBody.ptr, PhysicsScene.Params.defaultFriction);
-            BulletSimAPI.SetRestitution2(BSBody.ptr, PhysicsScene.Params.defaultRestitution);
+            BulletSimAPI.SetFriction2(PhysBody.ptr, PhysicsScene.Params.defaultFriction);
+            BulletSimAPI.SetRestitution2(PhysBody.ptr, PhysicsScene.Params.defaultRestitution);
 
             // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
-            BulletSimAPI.ClearAllForces2(BSBody.ptr);
+            // Since this can be called multiple times, only zero forces when becoming physical
+            // BulletSimAPI.ClearAllForces2(BSBody.ptr);
 
             // For good measure, make sure the transform is set through to the motion state
-            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+            BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
+
+            // Center of mass is at the center of the object
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
 
             // A dynamic object has mass
-            IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
-            OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
-            BulletSimAPI.SetMassProps2(BSBody.ptr, _mass, inertia);
-            BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
+            UpdatePhysicalMassProperties(RawMass);
 
             // Set collision detection parameters
             if (PhysicsScene.Params.ccdMotionThreshold > 0f)
             {
-                BulletSimAPI.SetCcdMotionThreshold2(BSBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
-                BulletSimAPI.SetCcdSweepSphereRadius2(BSBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
+                BulletSimAPI.SetCcdMotionThreshold2(PhysBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
+                BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
             }
 
             // Various values for simulation limits
-            BulletSimAPI.SetDamping2(BSBody.ptr, PhysicsScene.Params.linearDamping, PhysicsScene.Params.angularDamping);
-            BulletSimAPI.SetDeactivationTime2(BSBody.ptr, PhysicsScene.Params.deactivationTime);
-            BulletSimAPI.SetSleepingThresholds2(BSBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
-            BulletSimAPI.SetContactProcessingThreshold2(BSBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
+            BulletSimAPI.SetDamping2(PhysBody.ptr, PhysicsScene.Params.linearDamping, PhysicsScene.Params.angularDamping);
+            BulletSimAPI.SetDeactivationTime2(PhysBody.ptr, PhysicsScene.Params.deactivationTime);
+            BulletSimAPI.SetSleepingThresholds2(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
+            BulletSimAPI.SetContactProcessingThreshold2(PhysBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
 
             // There might be special things needed for implementing linksets.
             Linkset.MakeDynamic(this);
 
             // Force activation of the object so Bullet will act on it.
             // Must do the ForceActivationState2() to overcome the DISABLE_SIMULATION from static objects.
-            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
+            BulletSimAPI.ForceActivationState2(PhysBody.ptr, ActivationState.ACTIVE_TAG);
             // BulletSimAPI.Activate2(BSBody.ptr, true);
 
-            BSBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
-            BSBody.collisionMask = CollisionFilterGroups.ObjectMask;
+            PhysBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
+            PhysBody.collisionMask = CollisionFilterGroups.ObjectMask;
         }
     }
 
@@ -697,7 +746,7 @@ public sealed class BSPrim : BSPhysObject
     //     the functions after this one set up the state of a possibly newly created collision body.
     private void MakeSolid(bool makeSolid)
     {
-        CollisionObjectTypes bodyType = (CollisionObjectTypes)BulletSimAPI.GetBodyType2(BSBody.ptr);
+        CollisionObjectTypes bodyType = (CollisionObjectTypes)BulletSimAPI.GetBodyType2(PhysBody.ptr);
         if (makeSolid)
         {
             // Verify the previous code created the correct shape for this type of thing.
@@ -705,7 +754,7 @@ public sealed class BSPrim : BSPhysObject
             {
                 m_log.ErrorFormat("{0} MakeSolid: physical body of wrong type for solidity. id={1}, type={2}", LogHeader, LocalID, bodyType);
             }
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
         }
         else
         {
@@ -713,22 +762,31 @@ public sealed class BSPrim : BSPhysObject
             {
                 m_log.ErrorFormat("{0} MakeSolid: physical body of wrong type for non-solidness. id={1}, type={2}", LogHeader, LocalID, bodyType);
             }
-            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
-            BSBody.collisionFilter = CollisionFilterGroups.VolumeDetectFilter;
-            BSBody.collisionMask = CollisionFilterGroups.VolumeDetectMask;
+            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_NO_CONTACT_RESPONSE);
+            PhysBody.collisionFilter = CollisionFilterGroups.VolumeDetectFilter;
+            PhysBody.collisionMask = CollisionFilterGroups.VolumeDetectMask;
         }
     }
 
+    // Enable physical actions. Bullet will keep sleeping non-moving physical objects so
+    //     they need waking up when parameters are changed.
+    // Called in taint-time!!
+    private void ActivateIfPhysical(bool forceIt)
+    {
+        if (IsPhysical)
+            BulletSimAPI.Activate2(PhysBody.ptr, forceIt);
+    }
+
     // Turn on or off the flag controlling whether collision events are returned to the simulator.
     private void EnableCollisions(bool wantsCollisionEvents)
     {
         if (wantsCollisionEvents)
         {
-            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         }
         else
         {
-            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         }
     }
 
@@ -768,14 +826,14 @@ public sealed class BSPrim : BSPhysObject
         }
     }
     public override bool FloatOnWater {
-        set { 
+        set {
             _floatOnWater = value;
             PhysicsScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
             {
                 if (_floatOnWater)
-                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
                 else
-                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(PhysBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
             });
         }
     }
@@ -797,8 +855,8 @@ public sealed class BSPrim : BSPhysObject
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             PhysicsScene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                // DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
-                BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity);
+                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
             });
         }
     }
@@ -808,7 +866,7 @@ public sealed class BSPrim : BSPhysObject
         }
         set {
             _rotationalVelocity = value;
-            BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, _rotationalVelocity);
         }
     }
     public override bool Kinematic {
@@ -834,7 +892,7 @@ public sealed class BSPrim : BSPhysObject
             // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Buoyancy is faked by changing the gravity applied to the object
             float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
-            BulletSimAPI.SetGravity2(BSBody.ptr, new OMV.Vector3(0f, 0f, grav));
+            BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
         }
     }
 
@@ -874,6 +932,7 @@ public sealed class BSPrim : BSPhysObject
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
         AddForce(force, pushforce, false);
     }
+    // Applying a force just adds this to the total force on the object.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
@@ -887,7 +946,7 @@ public sealed class BSPrim : BSPhysObject
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        BSScene.TaintCallback addForceOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
         {
             OMV.Vector3 fSum = OMV.Vector3.Zero;
             lock (m_accumulatedForces)
@@ -899,20 +958,70 @@ public sealed class BSPrim : BSPhysObject
                 }
                 m_accumulatedForces.Clear();
             }
-            // 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);
-        };
-        if (inTaintTime)
-            addForceOperation();
-        else
-            PhysicsScene.TaintedObject("BSPrim.AddForce", addForceOperation);
+            DetailLog("{0},BSPrim.AddForce,taint,force={1}", LocalID, fSum);
+            if (fSum != OMV.Vector3.Zero)
+                BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
+        });
+    }
+
+    // An impulse force is scaled by the mass of the object.
+    public void ApplyForceImpulse(OMV.Vector3 impulse, bool inTaintTime)
+    {
+        OMV.Vector3 applyImpulse = impulse;
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyForceImpulse", delegate()
+        {
+            DetailLog("{0},BSPrim.ApplyForceImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
+            BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
+        });
     }
 
+    private List<OMV.Vector3> m_accumulatedAngularForces = new List<OMV.Vector3>();
     public override void AddAngularForce(OMV.Vector3 force, bool 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);
+        AddAngularForce(force, pushforce, false);
     }
+    public void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
+    {
+        if (force.IsFinite())
+        {
+            // _force += force;
+            lock (m_accumulatedAngularForces)
+                m_accumulatedAngularForces.Add(new OMV.Vector3(force));
+        }
+        else
+        {
+            m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
+            return;
+        }
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
+        {
+            OMV.Vector3 fSum = OMV.Vector3.Zero;
+            lock (m_accumulatedAngularForces)
+            {
+                // Sum the accumulated additional forces for one big force to apply once.
+                foreach (OMV.Vector3 v in m_accumulatedAngularForces)
+                {
+                    fSum += v;
+                }
+                m_accumulatedAngularForces.Clear();
+            }
+            DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum);
+            if (fSum != OMV.Vector3.Zero)
+            {
+                BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
+                _torque = fSum;
+            }
+        });
+    }
+    // A torque impulse.
+    public void ApplyTorqueImpulse(OMV.Vector3 impulse, bool inTaintTime)
+    {
+        OMV.Vector3 applyImpulse = impulse;
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
+        {
+            BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
+        });
+    }
+
     public override void SetMomentum(OMV.Vector3 momentum) {
         // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
     }
@@ -971,7 +1080,7 @@ public sealed class BSPrim : BSPhysObject
                     if (hollowAmount > 0.0)
                         {
                         hollowVolume *= hollowAmount;
-   
+
                         switch (BaseShape.HollowShape)
                             {
                             case HollowShape.Square:
@@ -1194,9 +1303,7 @@ public sealed class BSPrim : BSPhysObject
 
         returnMass = _density * volume;
 
-        /*
-         * This change means each object keeps its own mass and the Mass property
-         * will return the sum if we're part of a linkset.
+        /* Comment out code that computes the mass of the linkset. That is done in the Linkset class.
         if (IsRootOfLinkset)
         {
             foreach (BSPrim prim in _childrenPrims)
@@ -1216,48 +1323,27 @@ public sealed class BSPrim : BSPhysObject
     }// end CalculateMass
     #endregion Mass Calculation
 
-    // Copy prim's info into the BulletSim shape description structure
-    public void FillShapeInfo(out ShapeData shape)
-    {
-        shape.ID = LocalID;
-        shape.Type = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
-        shape.Position = _position;
-        shape.Rotation = _orientation;
-        shape.Velocity = _velocity;
-        shape.Size = _size;
-        shape.Scale = Scale;
-        shape.Mass = _isPhysical ? _mass : 0f;
-        shape.Buoyancy = _buoyancy;
-        shape.HullKey = 0;
-        shape.MeshKey = 0;
-        shape.Friction = _friction;
-        shape.Restitution = _restitution;
-        shape.Collidable = (!IsPhantom) ? ShapeData.numericTrue : ShapeData.numericFalse;
-        shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue;
-        shape.Solid = IsSolid ? ShapeData.numericFalse : ShapeData.numericTrue;
-    }
     // Rebuild the geometry and object.
     // This is called when the shape changes so we need to recreate the mesh/hull.
     // Called at taint-time!!!
     private void CreateGeomAndObject(bool forceRebuild)
     {
-        ShapeData shapeData;
-        FillShapeInfo(out shapeData);
-
         // If this prim is part of a linkset, we must remove and restore the physical
-        //    links of the body is rebuilt.
+        //    links if the body is rebuilt.
         bool needToRestoreLinkset = false;
+        bool needToRestoreVehicle = false;
 
         // Create the correct physical representation for this type of object.
-        // Updates BSBody and BSShape with the new information.
+        // Updates PhysBody and PhysShape with the new information.
         // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
-        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, shapeData, BaseShape, 
-                        null, delegate(BulletBody dBody)
+        // Returns 'true' if either the body or the shape was changed.
+        PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, null, delegate(BulletBody dBody)
         {
             // Called if the current prim body is about to be destroyed.
             // Remove all the physical dependencies on the old body.
             // (Maybe someday make the changing of BSShape an event handled by BSLinkset.)
             needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
+            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
         });
 
         if (needToRestoreLinkset)
@@ -1265,6 +1351,11 @@ public sealed class BSPrim : BSPhysObject
             // If physical body dependencies were removed, restore them
             Linkset.RestoreBodyDependencies(this);
         }
+        if (needToRestoreVehicle)
+        {
+            // If physical body dependencies were removed, restore them
+            _vehicle.RestoreBodyDependencies(this);
+        }
 
         // Make sure the properties are set on the new object
         UpdatePhysicalParameters();
@@ -1326,7 +1417,7 @@ public sealed class BSPrim : BSPhysObject
         if (changed != 0)
         {
             // Only update the position of single objects and linkset roots
-            if (this._parentPrim == null)
+            if (Linkset.IsRoot(this))
             {
                 base.RequestPhysicsterseUpdate();
             }
@@ -1338,23 +1429,29 @@ public sealed class BSPrim : BSPhysObject
         // Updates only for individual prims and for the root object of a linkset.
         if (Linkset.IsRoot(this))
         {
-            // Assign to the local variables so the normal set action does not happen
+            // Assign directly to the local variables so the normal set action does not happen
             _position = entprop.Position;
             _orientation = entprop.Rotation;
             _velocity = entprop.Velocity;
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
+            // The sanity check can change the velocity and/or position.
+            if (PositionSanityCheck(true))
+            {
+                entprop.Position = _position;
+                entprop.Velocity = _velocity;
+            }
+
             // remember the current and last set values
             LastEntityProperties = CurrentEntityProperties;
             CurrentEntityProperties = entprop;
 
-            PositionSanityCheck(true);
-
-            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
+            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
+                    LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);
 
-            // BulletSimAPI.DumpRigidBody2(Scene.World.Ptr, BSBody.Ptr);
+            // BulletSimAPI.DumpRigidBody2(PhysicsScene.World.ptr, BSBody.ptr);   // DEBUG DEBUG DEBUG
 
             base.RequestPhysicsterseUpdate();
         }
@@ -1367,8 +1464,8 @@ public sealed class BSPrim : BSPhysObject
                     entprop.Acceleration, entprop.RotationalVelocity);
         }
              */
-        // The linkset implimentation might want to know about this.
 
+        // The linkset implimentation might want to know about this.
         Linkset.UpdateProperties(this);
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 6621d39..27a78d1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -62,7 +62,7 @@ using OpenMetaverse;
 //
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSScene : PhysicsScene, IPhysicsParameters
+public sealed class BSScene : PhysicsScene, IPhysicsParameters
 {
     private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS SCENE]";
@@ -116,6 +116,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // True if initialized and ready to do simulation steps
     private bool m_initialized = false;
 
+    // Flag which is true when processing taints.
+    // Not guaranteed to be correct all the time (don't depend on this) but good for debugging.
+    public bool InTaintTime { get; private set; }
+
     // Pinned memory used to pass step information between managed and unmanaged
     private int m_maxCollisionsPerFrame;
     private CollisionDesc[] m_collisionArray;
@@ -171,7 +175,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         }
     }
     private Object _taintLock = new Object();   // lock for using the next object
-    private List<TaintCallbackEntry> _taintedObjects;
+    private List<TaintCallbackEntry> _taintOperations;
+    private Dictionary<string, TaintCallbackEntry> _postTaintOperations;
+    private List<TaintCallbackEntry> _postStepOperations;
 
     // A pointer to an instance if this structure is passed to the C++ code
     // Used to pass basic configuration values to the unmanaged code.
@@ -203,7 +209,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public override void Initialise(IMesher meshmerizer, IConfigSource config)
     {
         mesher = meshmerizer;
-        _taintedObjects = new List<TaintCallbackEntry>();
+        _taintOperations = new List<TaintCallbackEntry>();
+        _postTaintOperations = new Dictionary<string, TaintCallbackEntry>();
+        _postStepOperations = new List<TaintCallbackEntry>();
         PhysObjects = new Dictionary<uint, BSPhysObject>();
         Shapes = new BSShapeCollection(this);
 
@@ -266,6 +274,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         TerrainManager = new BSTerrainManager(this);
         TerrainManager.CreateInitialGroundPlaneAndTerrain();
 
+        m_log.WarnFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)Params.linksetImplementation);
+
+        InTaintTime = false;
         m_initialized = true;
     }
 
@@ -320,7 +331,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     {
         m_log.Debug("[BULLETS UNMANAGED]:" + msg);
     }
-  
+
     // Called directly from unmanaged code so don't do much
     private void BulletLoggerPhysLog(string msg)
     {
@@ -475,23 +486,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (!m_initialized) return 5.0f;
 
         // update the prim states while we know the physics engine is not busy
-        int numTaints = _taintedObjects.Count;
+        int numTaints = _taintOperations.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
         m_simulationStep++;
         int numSubSteps = 0;
 
-        // DEBUG
-        // DetailLog("{0},BSScene.Simulate,beforeStep,ntaimts={1},step={2}", DetailLogZero, numTaints, m_simulationStep);
-
         try
         {
+            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
             if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
 
             numSubSteps = BulletSimAPI.PhysicsStep2(World.ptr, timeStep, m_maxSubSteps, m_fixedTimeStep,
@@ -500,6 +508,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             if (PhysicsLogging.Enabled) simTime = Util.EnvironmentTickCountSubtract(beforeTime);
             DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}",
                         DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
+            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
         }
         catch (Exception e)
         {
@@ -511,7 +520,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             collidersCount = 0;
         }
 
-
         // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in
 
         // Get a value for 'now' so all the collision and update routines don't have to get their own
@@ -545,7 +553,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         }
 
         // This is a kludge to get avatar movement updates.
-        // The simulator expects collisions for avatars even if there are have been no collisions. 
+        // The simulator expects collisions for avatars even if there are have been no collisions.
         //    The event updates avatar animations and stuff.
         // If you fix avatar animation updates, remove this overhead and let normal collision processing happen.
         foreach (BSPhysObject bsp in m_avatars)
@@ -575,6 +583,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
+        ProcessPostStepTaints();
+
         // This causes the unmanaged code to output ALL the values found in ALL the objects in the world.
         // Only enable this in a limited test world with few objects.
         // BulletSimAPI.DumpAllInfo2(World.ptr);    // DEBUG DEBUG DEBUG
@@ -670,8 +680,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public override bool IsThreaded { get { return false;  } }
 
+    #region Taints
+
     // Calls to the PhysicsActors can't directly call into the physics engine
-    // because it might be busy. We delay changes to a known time.
+    //       because it might be busy. We delay changes to a known time.
     // We rely on C#'s closure to save and restore the context for the delegate.
     public void TaintedObject(String ident, TaintCallback callback)
     {
@@ -679,31 +691,51 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         lock (_taintLock)
         {
-            _taintedObjects.Add(new TaintCallbackEntry(ident, callback));
+            _taintOperations.Add(new TaintCallbackEntry(ident, callback));
         }
 
         return;
     }
 
+    // Sometimes a potentially tainted operation can be used in and out of taint time.
+    // This routine executes the command immediately if in taint-time otherwise it is queued.
+    public void TaintedObject(bool inTaintTime, string ident, TaintCallback callback)
+    {
+        if (inTaintTime)
+            callback();
+        else
+            TaintedObject(ident, callback);
+    }
+
     // When someone tries to change a property on a BSPrim or BSCharacter, the object queues
     // a callback into itself to do the actual property change. That callback is called
     // here just before the physics engine is called to step the simulation.
     public void ProcessTaints()
     {
-        if (_taintedObjects.Count > 0)  // save allocating new list if there is nothing to process
+        InTaintTime = true; // Only used for debugging so locking is not necessary.
+        ProcessRegularTaints();
+        ProcessPostTaintTaints();
+        InTaintTime = false;
+    }
+
+    private void ProcessRegularTaints()
+    {
+        if (_taintOperations.Count > 0)  // save allocating new list if there is nothing to process
         {
-            // swizzle a new list into the list location so we can process what's there
+            /*
+            // Code to limit the number of taints processed per step. Meant to limit step time.
+            // Unsure if a good idea as code assumes that taints are done before the step.
             int taintCount = m_taintsToProcessPerStep;
             TaintCallbackEntry oneCallback = new TaintCallbackEntry();
-            while (_taintedObjects.Count > 0 && taintCount-- > 0)
+            while (_taintOperations.Count > 0 && taintCount-- > 0)
             {
                 bool gotOne = false;
                 lock (_taintLock)
                 {
-                    if (_taintedObjects.Count > 0)
+                    if (_taintOperations.Count > 0)
                     {
-                        oneCallback = _taintedObjects[0];
-                        _taintedObjects.RemoveAt(0);
+                        oneCallback = _taintOperations[0];
+                        _taintOperations.RemoveAt(0);
                         gotOne = true;
                     }
                 }
@@ -711,22 +743,28 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 {
                     try
                     {
-                        DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, oneCallback.ident); // DEBUG DEBUG DEBUG
+                        DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, oneCallback.ident);
                         oneCallback.callback();
                     }
                     catch (Exception e)
                     {
+                        DetailLog("{0},BSScene.ProcessTaints,doTaintException,id={1}", DetailLogZero, oneCallback.ident); // DEBUG DEBUG DEBUG
                         m_log.ErrorFormat("{0}: ProcessTaints: {1}: Exception: {2}", LogHeader, oneCallback.ident, e);
                     }
                 }
             }
-            /*
+            if (_taintOperations.Count > 0)
+            {
+                DetailLog("{0},BSScene.ProcessTaints,leftTaintsOnList,numNotProcessed={1}", DetailLogZero, _taintOperations.Count);
+            }
+             */
+
             // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
             {
-                oldList = _taintedObjects;
-                _taintedObjects = new List<TaintCallbackEntry>();
+                oldList = _taintOperations;
+                _taintOperations = new List<TaintCallbackEntry>();
             }
 
             foreach (TaintCallbackEntry tcbe in oldList)
@@ -742,10 +780,103 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 }
             }
             oldList.Clear();
-             */
         }
     }
 
+    // Schedule an update to happen after all the regular taints are processed.
+    // Note that new requests for the same operation ("ident") for the same object ("ID")
+    //     will replace any previous operation by the same object.
+    public void PostTaintObject(String ident, uint ID, TaintCallback callback)
+    {
+        string uniqueIdent = ident + "-" + ID.ToString();
+        lock (_taintLock)
+        {
+            _postTaintOperations[uniqueIdent] = new TaintCallbackEntry(uniqueIdent, callback);
+        }
+
+        return;
+    }
+
+    private void ProcessPostTaintTaints()
+    {
+        if (_postTaintOperations.Count > 0)
+        {
+            Dictionary<string, TaintCallbackEntry> oldList;
+            lock (_taintLock)
+            {
+                oldList = _postTaintOperations;
+                _postTaintOperations = new Dictionary<string, TaintCallbackEntry>();
+            }
+
+            foreach (KeyValuePair<string,TaintCallbackEntry> kvp in oldList)
+            {
+                try
+                {
+                    DetailLog("{0},BSScene.ProcessPostTaintTaints,doTaint,id={1}", DetailLogZero, kvp.Key); // DEBUG DEBUG DEBUG
+                    kvp.Value.callback();
+                }
+                catch (Exception e)
+                {
+                    m_log.ErrorFormat("{0}: ProcessPostTaintTaints: {1}: Exception: {2}", LogHeader, kvp.Key, e);
+                }
+            }
+            oldList.Clear();
+        }
+    }
+
+    public void PostStepTaintObject(String ident, TaintCallback callback)
+    {
+        if (!m_initialized) return;
+
+        lock (_taintLock)
+        {
+            _postStepOperations.Add(new TaintCallbackEntry(ident, callback));
+        }
+
+        return;
+    }
+
+    private void ProcessPostStepTaints()
+    {
+        if (_postStepOperations.Count > 0)
+        {
+            List<TaintCallbackEntry> oldList;
+            lock (_taintLock)
+            {
+                oldList = _postStepOperations;
+                _postStepOperations = new List<TaintCallbackEntry>();
+            }
+
+            foreach (TaintCallbackEntry tcbe in oldList)
+            {
+                try
+                {
+                    DetailLog("{0},BSScene.ProcessPostStepTaints,doTaint,id={1}", DetailLogZero, tcbe.ident); // DEBUG DEBUG DEBUG
+                    tcbe.callback();
+                }
+                catch (Exception e)
+                {
+                    m_log.ErrorFormat("{0}: ProcessPostStepTaints: {1}: Exception: {2}", LogHeader, tcbe.ident, e);
+                }
+            }
+            oldList.Clear();
+        }
+    }
+
+    // Only used for debugging. Does not change state of anything so locking is not necessary.
+    public bool AssertInTaintTime(string whereFrom)
+    {
+        if (!InTaintTime)
+        {
+            DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
+            m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
+            Util.PrintCallStack();  // Prints the stack into the DEBUG log file.
+        }
+        return InTaintTime;
+    }
+
+    #endregion // Taints
+
     #region Vehicles
 
     public void VehicleInSceneTypeChanged(BSPrim vehic, Vehicle newType)
@@ -916,7 +1047,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return (float)s.m_maxUpdatesPerFrame; },
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
         new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
-            100f,
+            500f,
             (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_taintsToProcessPerStep; },
             (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
@@ -970,50 +1101,55 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (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.UpdateParameterObject(ref s.m_params[0].linearDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.BSBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, v, s.m_params[0].angularDamping); } ),
         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.UpdateParameterObject(ref s.m_params[0].angularDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.BSBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, s.m_params[0].linearDamping, 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.UpdateParameterObject(ref s.m_params[0].deactivationTime, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDeactivationTime2(o.BSBody.ptr, v); } ),
+            (s,o,v) => { BulletSimAPI.SetDeactivationTime2(o.PhysBody.ptr, 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.UpdateParameterObject(ref s.m_params[0].linearSleepingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.BSBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.PhysBody.ptr, v, 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.UpdateParameterObject(ref s.m_params[0].angularSleepingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.BSBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetSleepingThresholds2(o.PhysBody.ptr, v, 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.UpdateParameterObject(ref s.m_params[0].ccdMotionThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetCcdMotionThreshold2(o.BSBody.ptr, v); } ),
+            (s,o,v) => { BulletSimAPI.SetCcdMotionThreshold2(o.PhysBody.ptr, 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.UpdateParameterObject(ref s.m_params[0].ccdSweptSphereRadius, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetCcdSweepSphereRadius2(o.BSBody.ptr, v); } ),
+            (s,o,v) => { BulletSimAPI.SetCcdSweptSphereRadius2(o.PhysBody.ptr, 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.UpdateParameterObject(ref s.m_params[0].contactProcessingThreshold, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetContactProcessingThreshold2(o.BSBody.ptr, v); } ),
+            (s,o,v) => { BulletSimAPI.SetContactProcessingThreshold2(o.PhysBody.ptr, v); } ),
 
+            new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)",
+            (float)BSTerrainPhys.TerrainImplementation.Mesh,
+            (s,cf,p,v) => { s.m_params[0].terrainImplementation = cf.GetFloat(p,v); },
+            (s) => { return s.m_params[0].terrainImplementation; },
+            (s,p,l,v) => { s.m_params[0].terrainImplementation = v; } ),
         new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
             0.5f,
             (s,cf,p,v) => { s.m_params[0].terrainFriction = cf.GetFloat(p, v); },
@@ -1049,11 +1185,16 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].avatarRestitution = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].avatarRestitution; },
             (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarRestitution, p, l, v); } ),
-        new ParameterDefn("AvatarCapsuleRadius", "Radius of space around an avatar",
-            0.37f,
-            (s,cf,p,v) => { s.m_params[0].avatarCapsuleRadius = cf.GetFloat(p, v); },
-            (s) => { return s.m_params[0].avatarCapsuleRadius; },
-            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleRadius, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleWidth", "The distance between the sides of the avatar capsule",
+            0.6f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleWidth = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleWidth; },
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleWidth, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleDepth", "The distance between the front and back of the avatar capsule",
+            0.45f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleDepth = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleDepth; },
+            (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].avatarCapsuleDepth, p, l, v); } ),
         new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar",
             1.5f,
             (s,cf,p,v) => { s.m_params[0].avatarCapsuleHeight = cf.GetFloat(p, v); },
@@ -1107,6 +1248,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].numberOfSolverIterations; },
             (s,p,l,v) => { s.m_params[0].numberOfSolverIterations = v; } ),
 
+            new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
+            (float)BSLinkset.LinksetImplementation.Compound,
+            (s,cf,p,v) => { s.m_params[0].linksetImplementation = cf.GetFloat(p,v); },
+            (s) => { return s.m_params[0].linksetImplementation; },
+            (s,p,l,v) => { s.m_params[0].linksetImplementation = v; } ),
             new ParameterDefn("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
             ConfigurationParameters.numericFalse,
             (s,cf,p,v) => { s.m_params[0].linkConstraintUseFrameOffset = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
@@ -1128,12 +1274,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (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=no violation, 1=infinite. Default=0.1",
-            0.001f,
+            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; } ),
             new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
-            0.8f,
+            0.1f,
             (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; } ),
@@ -1259,7 +1405,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // If the local ID is APPLY_TO_NONE, just change the default value
     // If the localID is APPLY_TO_ALL change the default value and apply the new value to all the lIDs
     // If the localID is a specific object, apply the parameter change to only that object
-    protected void UpdateParameterObject(ref float defaultLoc, string parm, uint localID, float val)
+    private void UpdateParameterObject(ref float defaultLoc, string parm, uint localID, float val)
     {
         List<uint> objectIDs = new List<uint>();
         switch (localID)
@@ -1284,7 +1430,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     // schedule the actual updating of the paramter to when the phys engine is not busy
-    protected void TaintedUpdateParameter(string parm, List<uint> lIDs, float val)
+    private void TaintedUpdateParameter(string parm, List<uint> lIDs, float val)
     {
         float xval = val;
         List<uint> xlIDs = lIDs;
@@ -1326,14 +1472,24 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     #endregion Runtime settable parameters
 
+    // Debugging routine for dumping detailed physical information for vehicle prims
+    private void DumpVehicles()
+    {
+        foreach (BSPrim prim in m_vehicles)
+        {
+            BulletSimAPI.DumpRigidBody2(World.ptr, prim.PhysBody.ptr);
+            BulletSimAPI.DumpCollisionShape2(World.ptr, prim.PhysShape.ptr);
+        }
+    }
+
     // Invoke the detailed logger and output something if it's enabled.
     public void DetailLog(string msg, params Object[] args)
     {
         PhysicsLogging.Write(msg, args);
-        // Add the Flush() if debugging crashes to get all the messages written out.
+        // Add the Flush() if debugging crashes. Gets all the messages written out.
         // PhysicsLogging.Flush();
     }
-    // used to fill in the LocalID when there isn't one
+    // Used to fill in the LocalID when there isn't one. It's the correct number of characters.
     public const string DetailLogZero = "0000000000";
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index d3ba273..892c34b 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -34,11 +34,11 @@ using OpenSim.Region.Physics.ConvexDecompositionDotNet;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSShapeCollection : IDisposable
+public sealed class BSShapeCollection : IDisposable
 {
-    // private static string LogHeader = "[BULLETSIM SHAPE COLLECTION]";
+    private static string LogHeader = "[BULLETSIM SHAPE COLLECTION]";
 
-    protected BSScene PhysicsScene { get; set; }
+    private BSScene PhysicsScene { get; set; }
 
     private Object m_collectionActivityLock = new Object();
 
@@ -48,6 +48,7 @@ public class BSShapeCollection : IDisposable
         public IntPtr ptr;
         public int referenceCount;
         public DateTime lastReferenced;
+        public UInt64 shapeKey;
     }
 
     // Description of a hull.
@@ -57,6 +58,7 @@ public class BSShapeCollection : IDisposable
         public IntPtr ptr;
         public int referenceCount;
         public DateTime lastReferenced;
+        public UInt64 shapeKey;
     }
 
     // The sharable set of meshes and hulls. Indexed by their shape hash.
@@ -89,10 +91,11 @@ public class BSShapeCollection : IDisposable
     //    higher level dependencies on the shape or body. Mostly used for LinkSets to
     //    remove the physical constraints before the body is destroyed.
     // Called at taint-time!!
-    public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim, 
-                    ShapeData shapeData, PrimitiveBaseShape pbs,
+    public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim,
                     ShapeDestructionCallback shapeCallback, BodyDestructionCallback bodyCallback)
     {
+        PhysicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
+
         bool ret = false;
 
         // This lock could probably be pushed down lower but building shouldn't take long
@@ -100,41 +103,38 @@ public class BSShapeCollection : IDisposable
         {
             // Do we have the correct geometry for this type of object?
             // Updates prim.BSShape with information/pointers to shape.
-            // CreateGeom returns 'true' of BSShape as changed to a new shape.
-            bool newGeom = CreateGeom(forceRebuild, prim, shapeData, pbs, shapeCallback);
+            // Returns 'true' of BSShape is changed to a new shape.
+            bool newGeom = CreateGeom(forceRebuild, prim, shapeCallback);
             // If we had to select a new shape geometry for the object,
             //    rebuild the body around it.
             // Updates prim.BSBody with information/pointers to requested body
-            bool newBody = CreateBody((newGeom || forceRebuild), prim, PhysicsScene.World, 
-                                    prim.BSShape, shapeData, bodyCallback);
+            // Returns 'true' if BSBody was changed.
+            bool newBody = CreateBody((newGeom || forceRebuild), prim, PhysicsScene.World,
+                                    prim.PhysShape, bodyCallback);
             ret = newGeom || newBody;
         }
-        DetailLog("{0},BSShapeCollection.GetBodyAndShape,force={1},ret={2},body={3},shape={4}",
-                                prim.LocalID, forceRebuild, ret, prim.BSBody, prim.BSShape);
+        DetailLog("{0},BSShapeCollection.GetBodyAndShape,taintExit,force={1},ret={2},body={3},shape={4}",
+                                prim.LocalID, forceRebuild, ret, prim.PhysBody, prim.PhysShape);
 
         return ret;
     }
 
-    // Track another user of a body
+    // Track another user of a body.
     // We presume the caller has allocated the body.
     // Bodies only have one user so the body is just put into the world if not already there.
     public void ReferenceBody(BulletBody body, bool inTaintTime)
     {
         lock (m_collectionActivityLock)
         {
-            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody", body.ID, body);
-            BSScene.TaintCallback createOperation = delegate()
+            DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,body={1}", body.ID, body);
+            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.ReferenceBody", delegate()
             {
                 if (!BulletSimAPI.IsInWorld2(body.ptr))
                 {
                     BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, body.ptr);
                     DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", body.ID, body);
                 }
-            };
-            if (inTaintTime)
-                createOperation();
-            else
-                PhysicsScene.TaintedObject("BSShapeCollection.ReferenceBody", createOperation);
+            });
         }
     }
 
@@ -147,25 +147,23 @@ public class BSShapeCollection : IDisposable
 
         lock (m_collectionActivityLock)
         {
-            BSScene.TaintCallback removeOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceBody", delegate()
             {
-                DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}, inTaintTime={2}",
-                                            body.ID, body.ptr.ToString("X"), inTaintTime);
+                DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1},inTaintTime={2}",
+                                            body.ID, body, inTaintTime);
                 // If the caller needs to know the old body is going away, pass the event up.
                 if (bodyCallback != null) bodyCallback(body);
 
-                // It may have already been removed from the world in which case the next is a NOOP.
-                BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
+                if (BulletSimAPI.IsInWorld2(body.ptr))
+                {
+                    BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
+                    DetailLog("{0},BSShapeCollection.DereferenceBody,removingFromWorld. Body={1}", body.ID, body);
+                }
 
                 // Zero any reference to the shape so it is not freed when the body is deleted.
                 BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
                 BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
-            };
-            // If already in taint-time, do the operations now. Otherwise queue for later.
-            if (inTaintTime)
-                removeOperation();
-            else
-                PhysicsScene.TaintedObject("BSShapeCollection.DereferenceBody", removeOperation);
+            });
         }
     }
 
@@ -175,12 +173,12 @@ public class BSShapeCollection : IDisposable
     // Meshes and hulls for the same shape have the same hash key.
     // NOTE that native shapes are not added to the mesh list or removed.
     // Returns 'true' if this is the initial reference to the shape. Otherwise reused.
-    private bool ReferenceShape(BulletShape shape)
+    public bool ReferenceShape(BulletShape shape)
     {
         bool ret = false;
         switch (shape.type)
         {
-            case ShapeData.PhysicsShapeType.SHAPE_MESH:
+            case BSPhysicsShapeType.SHAPE_MESH:
                 MeshDesc meshDesc;
                 if (Meshes.TryGetValue(shape.shapeKey, out meshDesc))
                 {
@@ -193,6 +191,7 @@ public class BSShapeCollection : IDisposable
                 {
                     // This is a new reference to a mesh
                     meshDesc.ptr = shape.ptr;
+                    meshDesc.shapeKey = shape.shapeKey;
                     // We keep a reference to the underlying IMesh data so a hull can be built
                     meshDesc.referenceCount = 1;
                     DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
@@ -202,7 +201,7 @@ public class BSShapeCollection : IDisposable
                 meshDesc.lastReferenced = System.DateTime.Now;
                 Meshes[shape.shapeKey] = meshDesc;
                 break;
-            case ShapeData.PhysicsShapeType.SHAPE_HULL:
+            case BSPhysicsShapeType.SHAPE_HULL:
                 HullDesc hullDesc;
                 if (Hulls.TryGetValue(shape.shapeKey, out hullDesc))
                 {
@@ -215,6 +214,7 @@ public class BSShapeCollection : IDisposable
                 {
                     // This is a new reference to a hull
                     hullDesc.ptr = shape.ptr;
+                    hullDesc.shapeKey = shape.shapeKey;
                     hullDesc.referenceCount = 1;
                     DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
@@ -224,7 +224,7 @@ public class BSShapeCollection : IDisposable
                 hullDesc.lastReferenced = System.DateTime.Now;
                 Hulls[shape.shapeKey] = hullDesc;
                 break;
-            case ShapeData.PhysicsShapeType.SHAPE_UNKNOWN:
+            case BSPhysicsShapeType.SHAPE_UNKNOWN:
                 break;
             default:
                 // Native shapes are not tracked and they don't go into any list
@@ -239,7 +239,7 @@ public class BSShapeCollection : IDisposable
         if (shape.ptr == IntPtr.Zero)
             return;
 
-        BSScene.TaintCallback dereferenceOperation = delegate()
+        PhysicsScene.TaintedObject(inTaintTime, "BSShapeCollection.DereferenceShape", delegate()
         {
             if (shape.ptr != IntPtr.Zero)
             {
@@ -255,31 +255,23 @@ public class BSShapeCollection : IDisposable
                 {
                     switch (shape.type)
                     {
-                        case ShapeData.PhysicsShapeType.SHAPE_HULL:
+                        case BSPhysicsShapeType.SHAPE_HULL:
                             DereferenceHull(shape, shapeCallback);
                             break;
-                        case ShapeData.PhysicsShapeType.SHAPE_MESH:
+                        case BSPhysicsShapeType.SHAPE_MESH:
                             DereferenceMesh(shape, shapeCallback);
                             break;
-                        case ShapeData.PhysicsShapeType.SHAPE_UNKNOWN:
+                        case BSPhysicsShapeType.SHAPE_COMPOUND:
+                            DereferenceCompound(shape, shapeCallback);
+                            break;
+                        case BSPhysicsShapeType.SHAPE_UNKNOWN:
                             break;
                         default:
                             break;
                     }
                 }
             }
-        };
-        if (inTaintTime)
-        {
-            lock (m_collectionActivityLock)
-            {
-                dereferenceOperation();
-            }
-        }
-        else
-        {
-            PhysicsScene.TaintedObject("BSShapeCollection.DereferenceShape", dereferenceOperation);
-        }
+        });
     }
 
     // Count down the reference count for a mesh shape
@@ -294,8 +286,8 @@ public class BSShapeCollection : IDisposable
             if (shapeCallback != null) shapeCallback(shape);
             meshDesc.lastReferenced = System.DateTime.Now;
             Meshes[shape.shapeKey] = meshDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceMesh,key={1},refCnt={2}",
-                    BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
+            DetailLog("{0},BSShapeCollection.DereferenceMesh,shape={1},refCnt={2}",
+                                BSScene.DetailLogZero, shape, meshDesc.referenceCount);
 
         }
     }
@@ -309,11 +301,94 @@ public class BSShapeCollection : IDisposable
         {
             hullDesc.referenceCount--;
             // TODO: release the Bullet storage (aging old entries?)
+
+            // Tell upper layers that, if they have dependencies on this shape, this link is going away
             if (shapeCallback != null) shapeCallback(shape);
+
             hullDesc.lastReferenced = System.DateTime.Now;
             Hulls[shape.shapeKey] = hullDesc;
-            DetailLog("{0},BSShapeCollection.DereferenceHull,key={1},refCnt={2}",
-                    BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
+            DetailLog("{0},BSShapeCollection.DereferenceHull,shape={1},refCnt={2}",
+                    BSScene.DetailLogZero, shape, hullDesc.referenceCount);
+        }
+    }
+
+    // Remove a reference to a compound shape.
+    // Taking a compound shape apart is a little tricky because if you just delete the
+    //      physical shape, it will free all the underlying children. We can't do that because
+    //      they could be shared. So, this removes each of the children from the compound and
+    //      dereferences them separately before destroying the compound collision object itself.
+    // Called at taint-time.
+    private void DereferenceCompound(BulletShape shape, ShapeDestructionCallback shapeCallback)
+    {
+        if (!BulletSimAPI.IsCompound2(shape.ptr))
+        {
+            // Failed the sanity check!!
+            PhysicsScene.Logger.ErrorFormat("{0} Attempt to free a compound shape that is not compound!! type={1}, ptr={2}",
+                                        LogHeader, shape.type, shape.ptr.ToString("X"));
+            DetailLog("{0},BSShapeCollection.DereferenceCompound,notACompoundShape,type={1},ptr={2}",
+                                        BSScene.DetailLogZero, shape.type, shape.ptr.ToString("X"));
+            return;
+        }
+
+        int numChildren = BulletSimAPI.GetNumberOfCompoundChildren2(shape.ptr);
+        DetailLog("{0},BSShapeCollection.DereferenceCompound,shape={1},children={2}", BSScene.DetailLogZero, shape, numChildren);
+
+        for (int ii = numChildren - 1; ii >= 0; ii--)
+        {
+            IntPtr childShape = BulletSimAPI.RemoveChildShapeFromCompoundShapeIndex2(shape.ptr, ii);
+            DereferenceAnonCollisionShape(childShape);
+        }
+        BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
+    }
+
+    // Sometimes we have a pointer to a collision shape but don't know what type it is.
+    // Figure out type and call the correct dereference routine.
+    // Called at taint-time.
+    private void DereferenceAnonCollisionShape(IntPtr cShape)
+    {
+        MeshDesc meshDesc;
+        HullDesc hullDesc;
+
+        BulletShape shapeInfo = new BulletShape(cShape);
+        if (TryGetMeshByPtr(cShape, out meshDesc))
+        {
+            shapeInfo.type = BSPhysicsShapeType.SHAPE_MESH;
+            shapeInfo.shapeKey = meshDesc.shapeKey;
+        }
+        else
+        {
+            if (TryGetHullByPtr(cShape, out hullDesc))
+            {
+                shapeInfo.type = BSPhysicsShapeType.SHAPE_HULL;
+                shapeInfo.shapeKey = hullDesc.shapeKey;
+            }
+            else
+            {
+                if (BulletSimAPI.IsCompound2(cShape))
+                {
+                    shapeInfo.type = BSPhysicsShapeType.SHAPE_COMPOUND;
+                }
+                else
+                {
+                    if (BulletSimAPI.IsNativeShape2(cShape))
+                    {
+                        shapeInfo.isNativeShape = true;
+                        shapeInfo.type = BSPhysicsShapeType.SHAPE_BOX; // (technically, type doesn't matter)
+                    }
+                }
+            }
+        }
+
+        DetailLog("{0},BSShapeCollection.DereferenceAnonCollisionShape,shape={1}", BSScene.DetailLogZero, shapeInfo);
+
+        if (shapeInfo.type != BSPhysicsShapeType.SHAPE_UNKNOWN)
+        {
+            DereferenceShape(shapeInfo, true, null);
+        }
+        else
+        {
+            PhysicsScene.Logger.ErrorFormat("{0} Could not decypher shape type. Region={1}, addr={2}",
+                                    LogHeader, PhysicsScene.RegionName, cShape.ToString("X"));
         }
     }
 
@@ -325,21 +400,46 @@ public class BSShapeCollection : IDisposable
     // Info in prim.BSShape is updated to the new shape.
     // Returns 'true' if the geometry was rebuilt.
     // Called at taint-time!
-    private bool CreateGeom(bool forceRebuild, BSPhysObject prim, ShapeData shapeData, 
-                            PrimitiveBaseShape pbs, ShapeDestructionCallback shapeCallback)
+    private bool CreateGeom(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
         bool ret = false;
         bool haveShape = false;
-        bool nativeShapePossible = true;
 
-        if (shapeData.Type == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
             // an avatar capsule is close to a native shape (it is not shared)
-            ret = GetReferenceToNativeShape(prim, shapeData, ShapeData.PhysicsShapeType.SHAPE_AVATAR, 
-                            ShapeData.FixedShapeKey.KEY_CAPSULE, shapeCallback);
-            DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.BSShape);
+            ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_CAPSULE,
+                            FixedShapeKey.KEY_CAPSULE, shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,avatarCapsule,shape={1}", prim.LocalID, prim.PhysShape);
+            ret = true;
             haveShape = true;
         }
+
+        // Compound shapes are handled special as they are rebuilt from scratch.
+        // This isn't too great a hardship since most of the child shapes will already been created.
+        if (!haveShape && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
+        {
+            ret = GetReferenceToCompoundShape(prim, shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, prim.PhysShape);
+            haveShape = true;
+        }
+
+        if (!haveShape)
+        {
+            ret = CreateGeomNonSpecial(forceRebuild, prim, shapeCallback);
+        }
+
+        return ret;
+    }
+
+    // Create a mesh/hull shape or a native shape if 'nativeShapePossible' is 'true'.
+    private bool CreateGeomNonSpecial(bool forceRebuild, BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+        bool ret = false;
+        bool haveShape = false;
+        bool nativeShapePossible = true;
+        PrimitiveBaseShape pbs = prim.BaseShape;
+
         // If the prim attributes are simple, this could be a simple Bullet native shape
         if (!haveShape
                 && pbs != null
@@ -353,97 +453,120 @@ public class BSShapeCollection : IDisposable
                         && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                         && pbs.PathShearX == 0 && pbs.PathShearY == 0) ) )
         {
+            // It doesn't look like Bullet scales spheres so make sure the scales are all equal
             if ((pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
                                 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)
             {
                 haveShape = true;
                 if (forceRebuild
-                        || prim.Scale != shapeData.Size
-                        || prim.BSShape.type != ShapeData.PhysicsShapeType.SHAPE_SPHERE
+                        || prim.Scale != prim.Size
+                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_SPHERE
                         )
                 {
-                    ret = GetReferenceToNativeShape(prim, shapeData, ShapeData.PhysicsShapeType.SHAPE_SPHERE, 
-                                            ShapeData.FixedShapeKey.KEY_SPHERE, shapeCallback);
+                    ret = GetReferenceToNativeShape(prim, BSPhysicsShapeType.SHAPE_SPHERE,
+                                            FixedShapeKey.KEY_SPHERE, shapeCallback);
                     DetailLog("{0},BSShapeCollection.CreateGeom,sphere,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.BSShape);
+                                        prim.LocalID, forceRebuild, prim.PhysShape);
                 }
             }
-            if (pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
+            if (!haveShape && pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
             {
                 haveShape = true;
                 if (forceRebuild
-                        || prim.Scale != shapeData.Size
-                        || prim.BSShape.type != ShapeData.PhysicsShapeType.SHAPE_BOX
+                        || prim.Scale != prim.Size
+                        || prim.PhysShape.type != BSPhysicsShapeType.SHAPE_BOX
                         )
                 {
-                    ret = GetReferenceToNativeShape( prim, shapeData, ShapeData.PhysicsShapeType.SHAPE_BOX, 
-                                            ShapeData.FixedShapeKey.KEY_BOX, shapeCallback);
+                    ret = GetReferenceToNativeShape( prim, BSPhysicsShapeType.SHAPE_BOX,
+                                            FixedShapeKey.KEY_BOX, shapeCallback);
                     DetailLog("{0},BSShapeCollection.CreateGeom,box,force={1},shape={2}",
-                                        prim.LocalID, forceRebuild, prim.BSShape);
+                                        prim.LocalID, forceRebuild, prim.PhysShape);
                 }
             }
         }
+
         // If a simple shape is not happening, create a mesh and possibly a hull.
-        // Note that if it's a native shape, the check for physical/non-physical is not
-        //     made. Native shapes are best used in either case.
         if (!haveShape && pbs != null)
         {
-            if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
-            {
-                // Update prim.BSShape to reference a hull of this shape.
-                ret = GetReferenceToHull(prim, shapeData, pbs, shapeCallback);
-                DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
-                                        shapeData.ID, prim.BSShape, prim.BSShape.shapeKey.ToString("X"));
-            }
-            else
-            {
-                ret = GetReferenceToMesh(prim, shapeData, pbs, shapeCallback);
-                DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
-                                        shapeData.ID, prim.BSShape, prim.BSShape.shapeKey.ToString("X"));
-            }
+            ret = CreateGeomMeshOrHull(prim, shapeCallback);
+        }
+
+        return ret;
+    }
+
+    public bool CreateGeomMeshOrHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+
+        bool ret = false;
+        // Note that if it's a native shape, the check for physical/non-physical is not
+        //     made. Native shapes work in either case.
+        if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
+        {
+            // Update prim.BSShape to reference a hull of this shape.
+            ret = GetReferenceToHull(prim,shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,hull,shape={1},key={2}",
+                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
+        }
+        else
+        {
+            ret = GetReferenceToMesh(prim, shapeCallback);
+            DetailLog("{0},BSShapeCollection.CreateGeom,mesh,shape={1},key={2}",
+                                    prim.LocalID, prim.PhysShape, prim.PhysShape.shapeKey.ToString("X"));
         }
         return ret;
     }
 
     // Creates a native shape and assignes it to prim.BSShape.
     // "Native" shapes are never shared. they are created here and destroyed in DereferenceShape().
-    private bool GetReferenceToNativeShape(BSPhysObject prim, ShapeData shapeData,
-                            ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey,
+    private bool GetReferenceToNativeShape(BSPhysObject prim,
+                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey,
                             ShapeDestructionCallback shapeCallback)
     {
-
-        shapeData.Type = shapeType;
-        // Bullet native objects are scaled by the Bullet engine so pass the size in
-        prim.Scale = shapeData.Size;
-        shapeData.Scale = shapeData.Size;
-
         // release any previous shape
-        DereferenceShape(prim.BSShape, true, shapeCallback);
+        DereferenceShape(prim.PhysShape, true, shapeCallback);
 
-        BulletShape newShape = BuildPhysicalNativeShape(shapeType, shapeData, shapeKey);
+        BulletShape newShape = BuildPhysicalNativeShape(prim, shapeType, shapeKey);
 
         // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
-        DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}", 
-                                shapeData.ID, newShape, shapeData.Scale);
+        DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}",
+                                prim.LocalID, newShape, prim.Scale);
 
-        prim.BSShape = newShape;
+        prim.PhysShape = newShape;
         return true;
     }
 
-    private BulletShape BuildPhysicalNativeShape(ShapeData.PhysicsShapeType shapeType, 
-                        ShapeData shapeData, ShapeData.FixedShapeKey shapeKey)
+    private BulletShape BuildPhysicalNativeShape(BSPhysObject prim, BSPhysicsShapeType shapeType,
+                                    FixedShapeKey shapeKey)
     {
         BulletShape newShape;
-
-        if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
+        // Need to make sure the passed shape information is for the native type.
+        ShapeData nativeShapeData = new ShapeData();
+        nativeShapeData.Type = shapeType;
+        nativeShapeData.ID = prim.LocalID;
+        nativeShapeData.Scale = prim.Scale;
+        nativeShapeData.Size = prim.Scale;  // unneeded, I think.
+        nativeShapeData.MeshKey = (ulong)shapeKey;
+        nativeShapeData.HullKey = (ulong)shapeKey;
+
+        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
+            // The proper scale has been calculated in the prim.
             newShape = new BulletShape(
-                        BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1.0f, 1.0f, shapeData.Scale), 
-                        shapeType);
+                        BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1f, 1f, prim.Scale)
+                        , shapeType);
+            DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
         }
         else
         {
-            newShape = new BulletShape(BulletSimAPI.BuildNativeShape2(PhysicsScene.World.ptr, shapeData), shapeType);
+            // Native shapes are scaled in Bullet so set the scaling to the size
+            prim.Scale = prim.Size;
+            nativeShapeData.Scale = prim.Scale;
+            newShape = new BulletShape(BulletSimAPI.BuildNativeShape2(PhysicsScene.World.ptr, nativeShapeData), shapeType);
+        }
+        if (newShape.ptr == IntPtr.Zero)
+        {
+            PhysicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
+                                    LogHeader, prim.LocalID, shapeType);
         }
         newShape.shapeKey = (System.UInt64)shapeKey;
         newShape.isNativeShape = true;
@@ -455,33 +578,32 @@ public class BSShapeCollection : IDisposable
     // Dereferences previous shape in BSShape and adds a reference for this new shape.
     // Returns 'true' of a mesh was actually built. Otherwise .
     // Called at taint-time!
-    private bool GetReferenceToMesh(BSPhysObject prim, ShapeData shapeData, PrimitiveBaseShape pbs,
-                                ShapeDestructionCallback shapeCallback)
+    private bool GetReferenceToMesh(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
         BulletShape newShape = new BulletShape(IntPtr.Zero);
 
         float lod;
-        System.UInt64 newMeshKey = ComputeShapeKey(shapeData, pbs, out lod);
+        System.UInt64 newMeshKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
 
         // if this new shape is the same as last time, don't recreate the mesh
-        if (newMeshKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_MESH)
+        if (newMeshKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_MESH)
             return false;
 
-        DetailLog("{0},BSShapeCollection.CreateGeomMesh,create,oldKey={1},newKey={2}",
-                                prim.LocalID, prim.BSShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
+        DetailLog("{0},BSShapeCollection.GetReferenceToMesh,create,oldKey={1},newKey={2}",
+                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"));
 
         // Since we're recreating new, get rid of the reference to the previous shape
-        DereferenceShape(prim.BSShape, true, shapeCallback);
+        DereferenceShape(prim.PhysShape, true, shapeCallback);
 
-        newShape = CreatePhysicalMesh(prim.PhysObjectName, newMeshKey, pbs, shapeData.Size, lod);
+        newShape = CreatePhysicalMesh(prim.PhysObjectName, newMeshKey, prim.BaseShape, prim.Size, lod);
         // Take evasive action if the mesh was not constructed.
-        newShape = VerifyMeshCreated(newShape, prim, shapeData, pbs);
+        newShape = VerifyMeshCreated(newShape, prim);
 
         ReferenceShape(newShape);
 
         // meshes are already scaled by the meshmerizer
         prim.Scale = new OMV.Vector3(1f, 1f, 1f);
-        prim.BSShape = newShape;
+        prim.PhysShape = newShape;
 
         return true;        // 'true' means a new shape has been added to this prim
     }
@@ -515,14 +637,14 @@ public class BSShapeCollection : IDisposable
                     verticesAsFloats[vi++] = vv.Z;
                 }
 
-                // m_log.DebugFormat("{0}: CreateGeomMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
+                // m_log.DebugFormat("{0}: BSShapeCollection.CreatePhysicalMesh: calling CreateMesh. lid={1}, key={2}, indices={3}, vertices={4}",
                 //                  LogHeader, prim.LocalID, newMeshKey, indices.Length, vertices.Count);
 
                 meshPtr = BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
                                     indices.GetLength(0), indices, vertices.Count, verticesAsFloats);
             }
         }
-        BulletShape newShape = new BulletShape(meshPtr, ShapeData.PhysicsShapeType.SHAPE_MESH);
+        BulletShape newShape = new BulletShape(meshPtr, BSPhysicsShapeType.SHAPE_MESH);
         newShape.shapeKey = newMeshKey;
 
         return newShape;
@@ -530,32 +652,31 @@ public class BSShapeCollection : IDisposable
 
     // See that hull shape exists in the physical world and update prim.BSShape.
     // We could be creating the hull because scale changed or whatever.
-    private bool GetReferenceToHull(BSPhysObject prim, ShapeData shapeData, PrimitiveBaseShape pbs,
-                                ShapeDestructionCallback shapeCallback)
+    private bool GetReferenceToHull(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
     {
         BulletShape newShape;
 
         float lod;
-        System.UInt64 newHullKey = ComputeShapeKey(shapeData, pbs, out lod);
+        System.UInt64 newHullKey = ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
 
         // if the hull hasn't changed, don't rebuild it
-        if (newHullKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_HULL)
+        if (newHullKey == prim.PhysShape.shapeKey && prim.PhysShape.type == BSPhysicsShapeType.SHAPE_HULL)
             return false;
 
-        DetailLog("{0},BSShapeCollection.CreateGeomHull,create,oldKey={1},newKey={2}",
-                        prim.LocalID, prim.BSShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
+        DetailLog("{0},BSShapeCollection.GetReferenceToHull,create,oldKey={1},newKey={2}",
+                        prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
 
         // Remove usage of the previous shape.
-        DereferenceShape(prim.BSShape, true, shapeCallback);
+        DereferenceShape(prim.PhysShape, true, shapeCallback);
 
-        newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, pbs, shapeData.Size, lod);
-        newShape = VerifyMeshCreated(newShape, prim, shapeData, pbs);
+        newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, prim.BaseShape, prim.Size, lod);
+        newShape = VerifyMeshCreated(newShape, prim);
 
         ReferenceShape(newShape);
 
         // hulls are already scaled by the meshmerizer
         prim.Scale = new OMV.Vector3(1f, 1f, 1f);
-        prim.BSShape = newShape;
+        prim.PhysShape = newShape;
         return true;        // 'true' means a new shape has been added to this prim
     }
 
@@ -660,7 +781,7 @@ public class BSShapeCollection : IDisposable
             }
         }
 
-        BulletShape newShape = new BulletShape(hullPtr, ShapeData.PhysicsShapeType.SHAPE_HULL);
+        BulletShape newShape = new BulletShape(hullPtr, BSPhysicsShapeType.SHAPE_HULL);
         newShape.shapeKey = newHullKey;
 
         return newShape;        // 'true' means a new shape has been added to this prim
@@ -674,9 +795,31 @@ public class BSShapeCollection : IDisposable
         return;
     }
 
+    // Compound shapes are always built from scratch.
+    // This shouldn't be to bad since most of the parts will be meshes that had been built previously.
+    private bool GetReferenceToCompoundShape(BSPhysObject prim, ShapeDestructionCallback shapeCallback)
+    {
+        // Remove reference to the old shape
+        // Don't need to do this as the shape is freed when the new root shape is created below.
+        // DereferenceShape(prim.PhysShape, true, shapeCallback);
+
+        BulletShape cShape = new BulletShape(
+            BulletSimAPI.CreateCompoundShape2(PhysicsScene.World.ptr, false), BSPhysicsShapeType.SHAPE_COMPOUND);
+
+        // Create the shape for the root prim and add it to the compound shape. Cannot be a native shape.
+        CreateGeomMeshOrHull(prim, shapeCallback);
+        BulletSimAPI.AddChildShapeToCompoundShape2(cShape.ptr, prim.PhysShape.ptr, OMV.Vector3.Zero, OMV.Quaternion.Identity);
+        DetailLog("{0},BSShapeCollection.GetReferenceToCompoundShape,addRootPrim,compShape={1},rootShape={2}",
+                                    prim.LocalID, cShape, prim.PhysShape);
+
+        prim.PhysShape = cShape;
+
+        return true;
+    }
+
     // Create a hash of all the shape parameters to be used as a key
     //    for this particular shape.
-    private System.UInt64 ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs, out float retLod)
+    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
     {
         // level of detail based on size and type of the object
         float lod = PhysicsScene.MeshLOD;
@@ -684,33 +827,40 @@ public class BSShapeCollection : IDisposable
             lod = PhysicsScene.SculptLOD;
 
         // Mega prims usually get more detail because one can interact with shape approximations at this size.
-        float maxAxis = Math.Max(shapeData.Size.X, Math.Max(shapeData.Size.Y, shapeData.Size.Z));
+        float maxAxis = Math.Max(size.X, Math.Max(size.Y, size.Z));
         if (maxAxis > PhysicsScene.MeshMegaPrimThreshold)
             lod = PhysicsScene.MeshMegaPrimLOD;
 
         retLod = lod;
-        return pbs.GetMeshKey(shapeData.Size, lod);
+        return pbs.GetMeshKey(size, lod);
     }
     // For those who don't want the LOD
-    private System.UInt64 ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs)
+    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs)
     {
         float lod;
-        return ComputeShapeKey(shapeData, pbs, out lod);
+        return ComputeShapeKey(size, pbs, out lod);
     }
 
-    private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim, ShapeData shapeData, PrimitiveBaseShape pbs)
+    // The creation of a mesh or hull can fail if an underlying asset is not available.
+    // There are two cases: 1) the asset is not in the cache and it needs to be fetched;
+    //     and 2) the asset cannot be converted (like failed decompression of JPEG2000s).
+    //     The first case causes the asset to be fetched. The second case requires
+    //     us to not loop forever.
+    // Called after creating a physical mesh or hull. If the physical shape was created,
+    //     just return.
+    private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim)
     {
         // If the shape was successfully created, nothing more to do
         if (newShape.ptr != IntPtr.Zero)
             return newShape;
 
-        // The most common reason for failure is that an underlying asset is not available
-
         // If this mesh has an underlying asset and we have not failed getting it before, fetch the asset
-        if (pbs.SculptEntry && !prim.LastAssetBuildFailed && pbs.SculptTexture != OMV.UUID.Zero)
+        if (prim.BaseShape.SculptEntry && !prim.LastAssetBuildFailed && prim.BaseShape.SculptTexture != OMV.UUID.Zero)
         {
             prim.LastAssetBuildFailed = true;
             BSPhysObject xprim = prim;
+            DetailLog("{0},BSShapeCollection.VerifyMeshCreated,fetchAsset,lID={1},lastFailed={2}",
+                            LogHeader, prim.LocalID, prim.LastAssetBuildFailed);
             Util.FireAndForget(delegate
                 {
                     RequestAssetDelegate assetProvider = PhysicsScene.RequestAssetMethod;
@@ -724,20 +874,28 @@ public class BSShapeCollection : IDisposable
                             if (yprim.BaseShape.SculptTexture.ToString() != asset.ID)
                                 return;
 
-                            yprim.BaseShape.SculptData = new byte[asset.Data.Length];
-                            asset.Data.CopyTo(yprim.BaseShape.SculptData, 0);
+                            yprim.BaseShape.SculptData = asset.Data;
                             // This will cause the prim to see that the filler shape is not the right
                             //    one and try again to build the object.
+                            // No race condition with the normal shape setting since the rebuild is at taint time.
                             yprim.ForceBodyShapeRebuild(false);
 
                         });
                     }
                 });
         }
+        else
+        {
+            if (prim.LastAssetBuildFailed)
+            {
+                PhysicsScene.Logger.ErrorFormat("{0} Mesh failed to fetch asset. lID={1}, texture={2}",
+                                            LogHeader, prim.LocalID, prim.BaseShape.SculptTexture);
+            }
+        }
 
         // While we figure out the real problem, stick a simple native shape on the object.
         BulletShape fillinShape =
-            BuildPhysicalNativeShape(ShapeData.PhysicsShapeType.SHAPE_SPHERE, shapeData, ShapeData.FixedShapeKey.KEY_SPHERE);
+            BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
 
         return fillinShape;
     }
@@ -746,52 +904,51 @@ public class BSShapeCollection : IDisposable
     // Updates prim.BSBody with the information about the new body if one is created.
     // Returns 'true' if an object was actually created.
     // Called at taint-time.
-    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletSim sim, BulletShape shape, 
-                            ShapeData shapeData, BodyDestructionCallback bodyCallback)
+    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletSim sim, BulletShape shape,
+                            BodyDestructionCallback bodyCallback)
     {
         bool ret = false;
 
         // the mesh, hull or native shape must have already been created in Bullet
-        bool mustRebuild = (prim.BSBody.ptr == IntPtr.Zero);
+        bool mustRebuild = (prim.PhysBody.ptr == IntPtr.Zero);
 
         // If there is an existing body, verify it's of an acceptable type.
         // If not a solid object, body is a GhostObject. Otherwise a RigidBody.
         if (!mustRebuild)
         {
-            CollisionObjectTypes bodyType = (CollisionObjectTypes)BulletSimAPI.GetBodyType2(prim.BSBody.ptr);
+            CollisionObjectTypes bodyType = (CollisionObjectTypes)BulletSimAPI.GetBodyType2(prim.PhysBody.ptr);
             if (prim.IsSolid && bodyType != CollisionObjectTypes.CO_RIGID_BODY
                 || !prim.IsSolid && bodyType != CollisionObjectTypes.CO_GHOST_OBJECT)
             {
                 // If the collisionObject is not the correct type for solidness, rebuild what's there
                 mustRebuild = true;
             }
-
         }
 
         if (mustRebuild || forceRebuild)
         {
             // Free any old body
-            DereferenceBody(prim.BSBody, true, bodyCallback);
+            DereferenceBody(prim.PhysBody, true, bodyCallback);
 
             BulletBody aBody;
             IntPtr bodyPtr = IntPtr.Zero;
             if (prim.IsSolid)
             {
                 bodyPtr = BulletSimAPI.CreateBodyFromShape2(sim.ptr, shape.ptr,
-                                        shapeData.ID, shapeData.Position, shapeData.Rotation);
+                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
                 DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
             }
             else
             {
                 bodyPtr = BulletSimAPI.CreateGhostFromShape2(sim.ptr, shape.ptr,
-                                        shapeData.ID, shapeData.Position, shapeData.Rotation);
+                                        prim.LocalID, prim.RawPosition, prim.RawOrientation);
                 DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
             }
-            aBody = new BulletBody(shapeData.ID, bodyPtr);
+            aBody = new BulletBody(prim.LocalID, bodyPtr);
 
             ReferenceBody(aBody, true);
 
-            prim.BSBody = aBody;
+            prim.PhysBody = aBody;
 
             ret = true;
         }
@@ -799,6 +956,42 @@ public class BSShapeCollection : IDisposable
         return ret;
     }
 
+    private bool TryGetMeshByPtr(IntPtr addr, out MeshDesc outDesc)
+    {
+        bool ret = false;
+        MeshDesc foundDesc = new MeshDesc();
+        foreach (MeshDesc md in Meshes.Values)
+        {
+            if (md.ptr == addr)
+            {
+                foundDesc = md;
+                ret = true;
+                break;
+            }
+
+        }
+        outDesc = foundDesc;
+        return ret;
+    }
+
+    private bool TryGetHullByPtr(IntPtr addr, out HullDesc outDesc)
+    {
+        bool ret = false;
+        HullDesc foundDesc = new HullDesc();
+        foreach (HullDesc hd in Hulls.Values)
+        {
+            if (hd.ptr == addr)
+            {
+                foundDesc = hd;
+                ret = true;
+                break;
+            }
+
+        }
+        outDesc = foundDesc;
+        return ret;
+    }
+
     private void DetailLog(string msg, params Object[] args)
     {
         if (PhysicsScene.PhysicsLogging.Enabled)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
new file mode 100755
index 0000000..96cd55e
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public abstract class BSShape
+{
+    public IntPtr ptr { get; set; }
+    public BSPhysicsShapeType type { get; set; }
+    public System.UInt64 key { get; set; }
+    public int referenceCount { get; set; }
+    public DateTime lastReferenced { get; set; }
+
+    public BSShape()
+    {
+        ptr = IntPtr.Zero;
+        type = BSPhysicsShapeType.SHAPE_UNKNOWN;
+        key = 0;
+        referenceCount = 0;
+        lastReferenced = DateTime.Now;
+    }
+
+    // Get a reference to a physical shape. Create if it doesn't exist
+    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        BSShape ret = null;
+
+        if (prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_CAPSULE)
+        {
+            // an avatar capsule is close to a native shape (it is not shared)
+            ret = BSShapeNative.GetReference(physicsScene, prim, BSPhysicsShapeType.SHAPE_CAPSULE,
+                                        FixedShapeKey.KEY_CAPSULE);
+            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);
+        }
+
+        // Compound shapes are handled special as they are rebuilt from scratch.
+        // This isn't too great a hardship since most of the child shapes will already been created.
+        if (ret == null  && prim.PreferredPhysicalShape == BSPhysicsShapeType.SHAPE_COMPOUND)
+        {
+            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added
+            ret = BSShapeCompound.GetReference(prim);
+            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);
+        }
+
+        if (ret == null)
+            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);
+
+        return ret;
+    }
+    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        return null;
+    }
+    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        return null;
+    }
+
+    // Release the use of a physical shape.
+    public abstract void Dereference(BSScene physicsScene);
+
+    // All shapes have a static call to get a reference to the physical shape
+    // protected abstract static BSShape GetReference();
+
+    public override string ToString()
+    {
+        StringBuilder buff = new StringBuilder();
+        buff.Append("<p=");
+        buff.Append(ptr.ToString("X"));
+        buff.Append(",s=");
+        buff.Append(type.ToString());
+        buff.Append(",k=");
+        buff.Append(key.ToString("X"));
+        buff.Append(",c=");
+        buff.Append(referenceCount.ToString());
+        buff.Append(">");
+        return buff.ToString();
+    }
+}
+
+public class BSShapeNull : BSShape
+{
+    public BSShapeNull() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
+}
+
+public class BSShapeNative : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
+    public BSShapeNative() : base()
+    {
+    }
+    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 
+                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
+    {
+        // Native shapes are not shared and are always built anew.
+        return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
+    }
+
+    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
+                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
+    {
+        ShapeData nativeShapeData = new ShapeData();
+        nativeShapeData.Type = shapeType;
+        nativeShapeData.ID = prim.LocalID;
+        nativeShapeData.Scale = prim.Scale;
+        nativeShapeData.Size = prim.Scale;
+        nativeShapeData.MeshKey = (ulong)shapeKey;
+        nativeShapeData.HullKey = (ulong)shapeKey;
+
+       
+        if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
+        {
+            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);
+            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+        }
+        else
+        {
+            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);
+        }
+        if (ptr == IntPtr.Zero)
+        {
+            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
+                                    LogHeader, prim.LocalID, shapeType);
+        }
+        type = shapeType;
+        key = (UInt64)shapeKey;
+    }
+    // Make this reference to the physical shape go away since native shapes are not shared.
+    public override void Dereference(BSScene physicsScene)
+    {
+        // Native shapes are not tracked and are released immediately
+        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
+        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);
+        ptr = IntPtr.Zero;
+        // Garbage collection will free up this instance.
+    }
+}
+
+public class BSShapeMesh : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE MESH]";
+    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
+
+    public BSShapeMesh() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { }
+}
+
+public class BSShapeHull : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE HULL]";
+    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
+
+    public BSShapeHull() : base()
+    {
+    }
+    public static BSShape GetReference() { return new BSShapeNull();  }
+    public override void Dereference(BSScene physicsScene) { }
+}
+
+public class BSShapeCompound : BSShape
+{
+    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
+    public BSShapeCompound() : base()
+    {
+    }
+    public static BSShape GetReference(BSPhysObject prim) 
+    { 
+        return new BSShapeNull();
+    }
+    public override void Dereference(BSScene physicsScene) { }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
new file mode 100755
index 0000000..3ca756c
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using OpenSim.Region.Physics.Manager;
+
+using Nini.Config;
+using log4net;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSTerrainHeightmap : BSTerrainPhys
+{
+    static string LogHeader = "[BULLETSIM TERRAIN HEIGHTMAP]";
+
+    BulletHeightMapInfo m_mapInfo = null;
+
+    // Constructor to build a default, flat heightmap terrain.
+    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize)
+        : base(physicsScene, regionBase, id)
+    {
+        Vector3 minTerrainCoords = new Vector3(0f, 0f, BSTerrainManager.HEIGHT_INITIALIZATION - BSTerrainManager.HEIGHT_EQUAL_FUDGE);
+        Vector3 maxTerrainCoords = new Vector3(regionSize.X, regionSize.Y, BSTerrainManager.HEIGHT_INITIALIZATION);
+        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
+        float[] initialMap = new float[totalHeights];
+        for (int ii = 0; ii < totalHeights; ii++)
+        {
+            initialMap[ii] = BSTerrainManager.HEIGHT_INITIALIZATION;
+        }
+        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo.minCoords = minTerrainCoords;
+        m_mapInfo.maxCoords = maxTerrainCoords;
+        m_mapInfo.terrainRegionBase = TerrainBase;
+        // Don't have to free any previous since we just got here.
+        BuildHeightmapTerrain();
+    }
+
+    // This minCoords and maxCoords passed in give the size of the terrain (min and max Z
+    //         are the high and low points of the heightmap).
+    public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+                                                    Vector3 minCoords, Vector3 maxCoords)
+        : base(physicsScene, regionBase, id)
+    {
+        m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero);
+        m_mapInfo.minCoords = minCoords;
+        m_mapInfo.maxCoords = maxCoords;
+        m_mapInfo.minZ = minCoords.Z;
+        m_mapInfo.maxZ = maxCoords.Z;
+        m_mapInfo.terrainRegionBase = TerrainBase;
+
+        // Don't have to free any previous since we just got here.
+        BuildHeightmapTerrain();
+    }
+
+    public override void Dispose()
+    {
+        ReleaseHeightMapTerrain();
+    }
+
+    // Using the information in m_mapInfo, create the physical representation of the heightmap.
+    private void BuildHeightmapTerrain()
+    {
+        m_mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, m_mapInfo.ID,
+                                m_mapInfo.minCoords, m_mapInfo.maxCoords, 
+                                m_mapInfo.heightMap, BSTerrainManager.TERRAIN_COLLISION_MARGIN);
+
+        // Create the terrain shape from the mapInfo
+        m_mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(m_mapInfo.Ptr),
+                                                    BSPhysicsShapeType.SHAPE_TERRAIN);
+
+        // The terrain object initial position is at the center of the object
+        Vector3 centerPos;
+        centerPos.X = m_mapInfo.minCoords.X + (m_mapInfo.sizeX / 2f);
+        centerPos.Y = m_mapInfo.minCoords.Y + (m_mapInfo.sizeY / 2f);
+        centerPos.Z = m_mapInfo.minZ + ((m_mapInfo.maxZ - m_mapInfo.minZ) / 2f);
+
+        m_mapInfo.terrainBody = new BulletBody(m_mapInfo.ID,
+                BulletSimAPI.CreateBodyWithDefaultMotionState2(m_mapInfo.terrainShape.ptr,
+                                            m_mapInfo.ID, centerPos, Quaternion.Identity));
+
+        // Set current terrain attributes
+        BulletSimAPI.SetFriction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainFriction);
+        BulletSimAPI.SetHitFraction2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+        BulletSimAPI.SetRestitution2(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
+        BulletSimAPI.SetCollisionFlags2(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+        // Return the new terrain to the world of physical objects
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+
+        // redo its bounding box now that it is in the world
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+
+        BulletSimAPI.SetCollisionFilterMask2(m_mapInfo.terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainFilter,
+                            (uint)CollisionFilterGroups.TerrainMask);
+
+        // Make it so the terrain will not move or be considered for movement.
+        BulletSimAPI.ForceActivationState2(m_mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
+
+        return;
+    }
+
+    // If there is information in m_mapInfo pointing to physical structures, release same.
+    private void ReleaseHeightMapTerrain()
+    {
+        if (m_mapInfo != null)
+        {
+            if (m_mapInfo.terrainBody.ptr != IntPtr.Zero)
+            {
+                BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+                // Frees both the body and the shape.
+                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr);
+                BulletSimAPI.ReleaseHeightMapInfo2(m_mapInfo.Ptr);
+            }
+        }
+        m_mapInfo = null;
+    }
+
+    // The passed position is relative to the base of the region.
+    public override float GetHeightAtXYZ(Vector3 pos)
+    {
+        float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+
+        int mapIndex = (int)pos.Y * (int)m_mapInfo.sizeY + (int)pos.X;
+        try
+        {
+            ret = m_mapInfo.heightMap[mapIndex];
+        }
+        catch
+        {
+            // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+                                LogHeader, m_mapInfo.terrainRegionBase, pos);
+            ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+        }
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 4106534..23fcfd3 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -40,7 +40,33 @@ using OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSTerrainManager
+
+// The physical implementation of the terrain is wrapped in this class.
+public abstract class BSTerrainPhys : IDisposable
+{
+    public enum TerrainImplementation
+    {
+        Heightmap   = 0,
+        Mesh        = 1
+    }
+
+    public BSScene PhysicsScene { get; private set; }
+    // Base of the region in world coordinates. Coordinates inside the region are relative to this.
+    public Vector3 TerrainBase { get; private set; }
+    public uint ID { get; private set; }
+
+    public BSTerrainPhys(BSScene physicsScene, Vector3 regionBase, uint id)
+    {
+        PhysicsScene = physicsScene;
+        TerrainBase = regionBase;
+        ID = id;
+    }
+    public abstract void Dispose();
+    public abstract float GetHeightAtXYZ(Vector3 pos);
+}
+
+// ==========================================================================================
+public sealed class BSTerrainManager
 {
     static string LogHeader = "[BULLETSIM TERRAIN MANAGER]";
 
@@ -67,11 +93,10 @@ public class BSTerrainManager
 
     // If doing mega-regions, if we're region zero we will be managing multiple
     //    region terrains since region zero does the physics for the whole mega-region.
-    private Dictionary<Vector2, BulletHeightMapInfo> m_heightMaps;
+    private Dictionary<Vector3, BSTerrainPhys> m_terrains;
 
-    // True of the terrain has been modified.
-    // Used to force recalculation of terrain height after terrain has been modified
-    private bool m_terrainModified;
+    // Flags used to know when to recalculate the height.
+    private bool m_terrainModified = false;
 
     // If we are doing mega-regions, terrains are added from TERRAIN_ID to m_terrainCount.
     // This is incremented before assigning to new region so it is the last ID allocated.
@@ -89,8 +114,7 @@ public class BSTerrainManager
     public BSTerrainManager(BSScene physicsScene)
     {
         PhysicsScene = physicsScene;
-        m_heightMaps = new Dictionary<Vector2,BulletHeightMapInfo>();
-        m_terrainModified = false;
+        m_terrains = new Dictionary<Vector3,BSTerrainPhys>();
 
         // Assume one region of default size
         m_worldOffset = Vector3.Zero;
@@ -99,9 +123,6 @@ public class BSTerrainManager
     }
 
     // Create the initial instance of terrain and the underlying ground plane.
-    // The objects are allocated in the unmanaged space and the pointers are tracked
-    //    by the managed code.
-    // The terrains and the groundPlane are not added to the list of PhysObjects.
     // This is called from the initialization routine so we presume it is
     //    safe to call Bullet in real time. We hope no one is moving prims around yet.
     public void CreateInitialGroundPlaneAndTerrain()
@@ -109,7 +130,7 @@ public class BSTerrainManager
         // The ground plane is here to catch things that are trying to drop to negative infinity
         BulletShape groundPlaneShape = new BulletShape(
                     BulletSimAPI.CreateGroundPlaneShape2(BSScene.GROUNDPLANE_ID, 1f, TERRAIN_COLLISION_MARGIN),
-                    ShapeData.PhysicsShapeType.SHAPE_GROUNDPLANE);
+                    BSPhysicsShapeType.SHAPE_GROUNDPLANE);
         m_groundPlane = new BulletBody(BSScene.GROUNDPLANE_ID,
                         BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
                                                             Vector3.Zero, Quaternion.Identity));
@@ -121,15 +142,9 @@ public class BSTerrainManager
         BulletSimAPI.SetCollisionFilterMask2(m_groundPlane.ptr,
                         (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
 
-        Vector3 minTerrainCoords = new Vector3(0f, 0f, HEIGHT_INITIALIZATION - HEIGHT_EQUAL_FUDGE);
-        Vector3 maxTerrainCoords = new Vector3(DefaultRegionSize.X, DefaultRegionSize.Y, HEIGHT_INITIALIZATION);
-        int totalHeights = (int)maxTerrainCoords.X * (int)maxTerrainCoords.Y;
-        float[] initialMap = new float[totalHeights];
-        for (int ii = 0; ii < totalHeights; ii++)
-        {
-            initialMap[ii] = HEIGHT_INITIALIZATION;
-        }
-        UpdateOrCreateTerrain(BSScene.TERRAIN_ID, initialMap, minTerrainCoords, maxTerrainCoords, true);
+        // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain.
+        BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize);
+        m_terrains.Add(Vector3.Zero, initialTerrain);
     }
 
     // Release all the terrain structures we might have allocated
@@ -150,15 +165,11 @@ public class BSTerrainManager
     // Release all the terrain we have allocated
     public void ReleaseTerrain()
     {
-        foreach (KeyValuePair<Vector2, BulletHeightMapInfo> kvp in m_heightMaps)
+        foreach (KeyValuePair<Vector3, BSTerrainPhys> kvp in m_terrains)
         {
-            if (BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, kvp.Value.terrainBody.ptr))
-            {
-                BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, kvp.Value.terrainBody.ptr);
-                BulletSimAPI.ReleaseHeightMapInfo2(kvp.Value.Ptr);
-            }
+            kvp.Value.Dispose();
         }
-        m_heightMaps.Clear();
+        m_terrains.Clear();
     }
 
     // The simulator wants to set a new heightmap for the terrain.
@@ -176,8 +187,9 @@ public class BSTerrainManager
                 {
                     DetailLog("{0},SetTerrain.ToParent,offset={1},worldMax={2}",
                                     BSScene.DetailLogZero, m_worldOffset, m_worldMax);
-                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateOrCreateTerrain(BSScene.CHILDTERRAIN_ID,
-                                    localHeightMap, m_worldOffset, m_worldOffset + DefaultRegionSize, true);
+                    ((BSScene)MegaRegionParentPhysicsScene).TerrainManager.UpdateTerrain(
+                                    BSScene.CHILDTERRAIN_ID, localHeightMap, 
+                                    m_worldOffset, m_worldOffset + DefaultRegionSize, true);
                 }
             }
             else
@@ -185,7 +197,7 @@ public class BSTerrainManager
                 // If not doing the mega-prim thing, just change the terrain
                 DetailLog("{0},SetTerrain.Existing", BSScene.DetailLogZero);
 
-                UpdateOrCreateTerrain(BSScene.TERRAIN_ID, localHeightMap,
+                UpdateTerrain(BSScene.TERRAIN_ID, localHeightMap,
                                     m_worldOffset, m_worldOffset + DefaultRegionSize, true);
             }
         });
@@ -195,58 +207,63 @@ public class BSTerrainManager
     //     based on the passed information. The 'id' should be either the terrain id or
     //     BSScene.CHILDTERRAIN_ID. If the latter, a new child terrain ID will be allocated and used.
     //     The latter feature is for creating child terrains for mega-regions.
-    // If called with a mapInfo in m_heightMaps but the terrain has no body yet (mapInfo.terrainBody.Ptr == 0)
-    //     then a new body and shape is created and the mapInfo is filled.
-    //     This call is used for doing the initial terrain creation.
     // If called with a mapInfo in m_heightMaps and there is an existing terrain body, a new
     //     terrain shape is created and added to the body.
     //     This call is most often used to update the heightMap and parameters of the terrain.
-    // The '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 inTaintTime)
+    // (The above does suggest that some simplification/refactoring is in order.)
+    private void UpdateTerrain(uint id, float[] heightMap, 
+                            Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
     {
-        DetailLog("{0},BSTerrainManager.UpdateOrCreateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
+        DetailLog("{0},BSTerrainManager.UpdateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
                             BSScene.DetailLogZero, minCoords, maxCoords, inTaintTime);
 
+        // Find high and low points of passed heightmap.
+        // The min and max passed in is usually the area objects can be in (maximum
+        //     object height, for instance). The terrain wants the bounding box for the
+        //     terrain so we replace passed min and max Z with the actual terrain min/max Z.
         float minZ = float.MaxValue;
         float maxZ = float.MinValue;
-        Vector2 terrainRegionBase = new Vector2(minCoords.X, minCoords.Y);
-
-        int heightMapSize = heightMap.Length;
-        for (int ii = 0; ii < heightMapSize; ii++)
+        foreach (float height in heightMap)
         {
-            float height = heightMap[ii];
             if (height < minZ) minZ = height;
             if (height > maxZ) maxZ = height;
         }
-
-        // The shape of the terrain is from its base to its extents.
+        if (minZ == maxZ)
+        {
+            // If min and max are the same, reduce min a little bit so a good bounding box is created.
+            minZ -= BSTerrainManager.HEIGHT_EQUAL_FUDGE;
+        }
         minCoords.Z = minZ;
         maxCoords.Z = maxZ;
 
-        BulletHeightMapInfo mapInfo;
-        if (m_heightMaps.TryGetValue(terrainRegionBase, out mapInfo))
+        Vector3 terrainRegionBase = new Vector3(minCoords.X, minCoords.Y, 0f);
+
+        BSTerrainPhys terrainPhys;
+        if (m_terrains.TryGetValue(terrainRegionBase, out terrainPhys))
         {
-            // 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()
+            // There is already a terrain in this spot. Free the old and build the new.
+            DetailLog("{0},UpdateTerrain:UpdateExisting,call,id={1},base={2},minC={3},maxC={4}",
+                            BSScene.DetailLogZero, id, terrainRegionBase, minCoords, minCoords);
+
+            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:UpdateExisting", delegate()
             {
-                if (MegaRegionParentPhysicsScene != null)
+                // Remove old terrain from the collection
+                m_terrains.Remove(terrainRegionBase);
+                // Release any physical memory it may be using.
+                terrainPhys.Dispose();
+
+                if (MegaRegionParentPhysicsScene == null)
+                {
+                    BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
+                    m_terrains.Add(terrainRegionBase, newTerrainPhys);
+
+                    m_terrainModified = true;
+                }
+                else
                 {
                     // It's possible that Combine() was called after this code was queued.
                     // If we are a child of combined regions, we don't create any terrain for us.
-                    DetailLog("{0},UpdateOrCreateTerrain:AmACombineChild,taint", BSScene.DetailLogZero);
+                    DetailLog("{0},BSTerrainManager.UpdateTerrain:AmACombineChild,taint", BSScene.DetailLogZero);
 
                     // Get rid of any terrain that may have been allocated for us.
                     ReleaseGroundPlaneAndTerrain();
@@ -254,99 +271,7 @@ public class BSTerrainManager
                     // 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(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                    // Get rid of the old terrain
-                    BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-                    BulletSimAPI.ReleaseHeightMapInfo2(mapInfo.Ptr);
-                    mapInfo.Ptr = IntPtr.Zero;
-
-                    /*
-                    // NOTE: This routine is half here because I can't get the terrain shape replacement
-                    //   to work. In the short term, the above three lines completely delete the old
-                    //   terrain and the code below recreates one from scratch.
-                    // Hopefully the Bullet community will help me out on this one.
-
-                    // First, release the old collision shape (there is only one terrain)
-                    BulletSimAPI.DeleteCollisionShape2(m_physicsScene.World.Ptr, mapInfo.terrainShape.Ptr);
-
-                    // Fill the existing height map info with the new location and size information
-                    BulletSimAPI.FillHeightMapInfo2(m_physicsScene.World.Ptr, mapInfo.Ptr, mapInfo.ID,
-                                    mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
-
-                    // Create a terrain shape based on the new info
-                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr));
-
-                    // Stuff the shape into the existing terrain body
-                    BulletSimAPI.SetBodyShape2(m_physicsScene.World.Ptr, mapInfo.terrainBody.Ptr, mapInfo.terrainShape.Ptr);
-                    */
-                }
-                // else
-                {
-                    // Creating a new terrain.
-                    DetailLog("{0},UpdateOrCreateTerrain:CreateNewTerrain,taint,baseX={1},baseY={2},minZ={3},maxZ={4}",
-                                    BSScene.DetailLogZero, mapInfo.minCoords.X, mapInfo.minCoords.Y, minZ, maxZ);
-
-                    mapInfo.ID = id;
-                    mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, mapInfo.ID,
-                        mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
-
-                    // Create the terrain shape from the mapInfo
-                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr),
-                                ShapeData.PhysicsShapeType.SHAPE_TERRAIN);
-
-                    // The terrain object initial position is at the center of the object
-                    Vector3 centerPos;
-                    centerPos.X = minCoords.X + (mapInfo.sizeX / 2f);
-                    centerPos.Y = minCoords.Y + (mapInfo.sizeY / 2f);
-                    centerPos.Z = minZ + ((maxZ - minZ) / 2f);
-
-                    mapInfo.terrainBody = new BulletBody(mapInfo.ID,
-                            BulletSimAPI.CreateBodyWithDefaultMotionState2(mapInfo.terrainShape.ptr,
-                                                        id, centerPos, Quaternion.Identity));
-                }
-
-                // Make sure the entry is in the heightmap table
-                m_heightMaps[terrainRegionBase] = mapInfo;
-
-                // Set current terrain attributes
-                BulletSimAPI.SetFriction2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainFriction);
-                BulletSimAPI.SetHitFraction2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
-                BulletSimAPI.SetRestitution2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
-                BulletSimAPI.SetCollisionFlags2(mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
-
-                // Return the new terrain to the world of physical objects
-                BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                // redo its bounding box now that it is in the world
-                BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
-
-                BulletSimAPI.SetCollisionFilterMask2(mapInfo.terrainBody.ptr,
-                                    (uint)CollisionFilterGroups.TerrainFilter,
-                                    (uint)CollisionFilterGroups.TerrainMask);
-
-                // Make sure the new shape is processed.
-                // BulletSimAPI.Activate2(mapInfo.terrainBody.ptr, true);
-                BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.ISLAND_SLEEPING);
-                // BulletSimAPI.ForceActivationState2(mapInfo.terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
-
-                m_terrainModified = true;
-            };
-
-            // There is the option to do the changes now (we're already in 'taint time'), or
-            //     to do the Bullet operations later.
-            if (inTaintTime)
-                rebuildOperation();
-            else
-                PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:UpdateExisting", rebuildOperation);
+            });
         }
         else
         {
@@ -362,40 +287,51 @@ public class BSTerrainManager
             Vector3 minCoordsX = minCoords;
             Vector3 maxCoordsX = maxCoords;
 
-            DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,call,id={1}, minC={2}, maxC={3}",
+            DetailLog("{0},UpdateTerrain:NewTerrain,call,id={1}, minC={2}, maxC={3}",
                             BSScene.DetailLogZero, newTerrainID, minCoords, minCoords);
 
             // Code that must happen at taint-time
-            BSScene.TaintCallback createOperation = delegate()
+            PhysicsScene.TaintedObject(inTaintTime, "BSScene.UpdateTerrain:NewTerrain", delegate()
             {
-                DetailLog("{0},UpdateOrCreateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoords.X, minCoords.Y);
-                // Create a new mapInfo that will be filled with the new info
-                mapInfo = new BulletHeightMapInfo(id, heightMapX,
-                        BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, newTerrainID,
-                                    minCoordsX, maxCoordsX, heightMapX, TERRAIN_COLLISION_MARGIN));
-                // Put the unfilled heightmap info into the collection of same
-                m_heightMaps.Add(terrainRegionBase, mapInfo);
-                // Build the terrain
-                UpdateOrCreateTerrain(newTerrainID, heightMap, minCoords, maxCoords, true);
+                DetailLog("{0},UpdateTerrain:NewTerrain,taint,baseX={1},baseY={2}", 
+                                            BSScene.DetailLogZero, minCoordsX.X, minCoordsX.Y);
+                BSTerrainPhys newTerrainPhys = BuildPhysicalTerrain(terrainRegionBase, id, heightMap, minCoords, maxCoords);
+                m_terrains.Add(terrainRegionBase, newTerrainPhys);
 
                 m_terrainModified = true;
-            };
-
-            // If already in taint-time, just call Bullet. Otherwise queue the operations for the safe time.
-            if (inTaintTime)
-                createOperation();
-            else
-                PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:NewTerrain", createOperation);
+            });
         }
     }
 
-    // Someday we will have complex terrain with caves and tunnels
-    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    // TODO: redo terrain implementation selection to allow other base types than heightMap.
+    private BSTerrainPhys BuildPhysicalTerrain(Vector3 terrainRegionBase, uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords)
     {
-        // For the moment, it's flat and convex
-        return GetTerrainHeightAtXY(loc.X, loc.Y);
+        PhysicsScene.Logger.DebugFormat("{0} Terrain for {1}/{2} created with {3}", 
+                                            LogHeader, PhysicsScene.RegionName, terrainRegionBase, 
+                                            (BSTerrainPhys.TerrainImplementation)PhysicsScene.Params.terrainImplementation);
+        BSTerrainPhys newTerrainPhys = null;
+        switch ((int)PhysicsScene.Params.terrainImplementation)
+        {
+            case (int)BSTerrainPhys.TerrainImplementation.Heightmap:
+                newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, id,
+                                            heightMap, minCoords, maxCoords);
+                break;
+            case (int)BSTerrainPhys.TerrainImplementation.Mesh:
+                newTerrainPhys = new BSTerrainMesh(PhysicsScene, terrainRegionBase, id,
+                                            heightMap, minCoords, maxCoords);
+                break;
+            default:
+                PhysicsScene.Logger.ErrorFormat("{0} Bad terrain implementation specified. Type={1}/{2},Region={3}/{4}",
+                                            LogHeader, 
+                                            (int)PhysicsScene.Params.terrainImplementation, 
+                                            PhysicsScene.Params.terrainImplementation,
+                                            PhysicsScene.RegionName, terrainRegionBase);
+                break;
+        }
+        return newTerrainPhys;
     }
 
+
     // Given an X and Y, find the height of the terrain.
     // Since we could be handling multiple terrains for a mega-region,
     //    the base of the region is calcuated assuming all regions are
@@ -405,8 +341,10 @@ public class BSTerrainManager
     private float lastHeightTX = 999999f;
     private float lastHeightTY = 999999f;
     private float lastHeight = HEIGHT_INITIAL_LASTHEIGHT;
-    private float GetTerrainHeightAtXY(float tX, float tY)
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
     {
+        float tX = loc.X;
+        float tY = loc.Y;
         // You'd be surprized at the number of times this routine is called
         //    with the same parameters as last time.
         if (!m_terrainModified && lastHeightTX == tX && lastHeightTY == tY)
@@ -418,27 +356,14 @@ public class BSTerrainManager
 
         int offsetX = ((int)(tX / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
         int offsetY = ((int)(tY / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector2 terrainBaseXY = new Vector2(offsetX, offsetY);
+        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
 
-        BulletHeightMapInfo mapInfo;
-        if (m_heightMaps.TryGetValue(terrainBaseXY, out mapInfo))
+        BSTerrainPhys physTerrain;
+        if (m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain))
         {
-            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.
-                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);
+            ret = physTerrain.GetHeightAtXYZ(loc - terrainBaseXYZ);
+            DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,loc={1},base={2},height={3}",
+                                             BSScene.DetailLogZero, loc, terrainBaseXYZ, ret);
         }
         else
         {
@@ -481,7 +406,7 @@ public class BSTerrainManager
     // Unhook all the combining that I know about.
     public void UnCombine(PhysicsScene pScene)
     {
-        // Just like ODE, for the moment a NOP
+        // Just like ODE, we don't do anything yet.
         DetailLog("{0},BSTerrainManager.UnCombine", BSScene.DetailLogZero);
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
new file mode 100755
index 0000000..dca7150
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -0,0 +1,256 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using OpenSim.Region.Physics.Manager;
+
+using Nini.Config;
+using log4net;
+
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public sealed class BSTerrainMesh : BSTerrainPhys
+{
+    static string LogHeader = "[BULLETSIM TERRAIN MESH]";
+
+    private float[] m_savedHeightMap;
+    int m_sizeX;
+    int m_sizeY;
+
+    BulletShape m_terrainShape;
+    BulletBody m_terrainBody;
+
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize) 
+        : base(physicsScene, regionBase, id)
+    {
+    }
+
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id /* parameters for making mesh */)
+        : base(physicsScene, regionBase, id)
+    {
+    }
+
+    // Create terrain mesh from a heightmap.
+    public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, 
+                                                    Vector3 minCoords, Vector3 maxCoords)
+        : base(physicsScene, regionBase, id)
+    {
+        int indicesCount;
+        int[] indices;
+        int verticesCount;
+        float[] vertices;
+
+        m_savedHeightMap = initialMap;
+
+        m_sizeX = (int)(maxCoords.X - minCoords.X);
+        m_sizeY = (int)(maxCoords.Y - minCoords.Y);
+
+        if (!BSTerrainMesh.ConvertHeightmapToMesh(PhysicsScene, initialMap,
+                            m_sizeX, m_sizeY,
+                            (float)m_sizeX, (float)m_sizeY,
+                            Vector3.Zero, 1.0f,
+                            out indicesCount, out indices, out verticesCount, out vertices))
+        {
+            // DISASTER!!
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap", ID);
+            PhysicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh! base={1}", LogHeader, TerrainBase);
+            // Something is very messed up and a crash is in our future.
+            return;
+        }
+
+        m_terrainShape = new BulletShape(BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr,
+                                        indicesCount, indices, verticesCount, vertices),
+                                        BSPhysicsShapeType.SHAPE_MESH);
+        if (m_terrainShape.ptr == IntPtr.Zero)
+        {
+            // DISASTER!!
+            PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape", ID);
+            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain mesh! base={1}", LogHeader, TerrainBase);
+            // Something is very messed up and a crash is in our future.
+            return;
+        }
+
+        Vector3 pos = regionBase;
+        Quaternion rot = Quaternion.Identity;
+
+        m_terrainBody = new BulletBody(id, BulletSimAPI.CreateBodyWithDefaultMotionState2( m_terrainShape.ptr, ID, pos, rot));
+        if (m_terrainBody.ptr == IntPtr.Zero)
+        {
+            // DISASTER!!
+            physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase);
+            // Something is very messed up and a crash is in our future.
+            return;
+        }
+
+        // Set current terrain attributes
+        BulletSimAPI.SetFriction2(m_terrainBody.ptr, PhysicsScene.Params.terrainFriction);
+        BulletSimAPI.SetHitFraction2(m_terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
+        BulletSimAPI.SetRestitution2(m_terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
+        BulletSimAPI.SetCollisionFlags2(m_terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
+
+        // Static objects are not very massive.
+        BulletSimAPI.SetMassProps2(m_terrainBody.ptr, 0f, Vector3.Zero);
+
+        // Return the new terrain to the world of physical objects
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+
+        // redo its bounding box now that it is in the world
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+
+        BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr,
+                            (uint)CollisionFilterGroups.TerrainFilter,
+                            (uint)CollisionFilterGroups.TerrainMask);
+
+        // Make it so the terrain will not move or be considered for movement.
+        BulletSimAPI.ForceActivationState2(m_terrainBody.ptr, ActivationState.DISABLE_SIMULATION);
+    }
+
+    public override void Dispose()
+    {
+        if (m_terrainBody.ptr != IntPtr.Zero)
+        {
+            BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+            // Frees both the body and the shape.
+            BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_terrainBody.ptr);
+        }
+    }
+
+    public override float GetHeightAtXYZ(Vector3 pos)
+    {
+        // For the moment use the saved heightmap to get the terrain height.
+        // TODO: raycast downward to find the true terrain below the position.
+        float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+
+        int mapIndex = (int)pos.Y * m_sizeY + (int)pos.X;
+        try
+        {
+            ret = m_savedHeightMap[mapIndex];
+        }
+        catch
+        {
+            // Sometimes they give us wonky values of X and Y. Give a warning and return something.
+            PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}",
+                                                LogHeader, TerrainBase, pos);
+            ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
+        }
+        return ret;
+    }
+
+    // Convert the passed heightmap to mesh information suitable for CreateMeshShape2().
+    // Return 'true' if successfully created.
+    public static bool ConvertHeightmapToMesh(
+                                BSScene physicsScene,
+                                float[] heightMap, int sizeX, int sizeY,    // parameters of incoming heightmap
+                                float extentX, float extentY,               // zero based range for output vertices
+                                Vector3 extentBase,                         // base to be added to all vertices
+                                float magnification,                        // number of vertices to create between heightMap coords
+                                out int indicesCountO, out int[] indicesO,
+                                out int verticesCountO, out float[] verticesO)
+    {
+        bool ret = false;
+
+        int indicesCount = 0;
+        int verticesCount = 0;
+        int[] indices = new int[0];
+        float[] vertices = new float[0];
+
+        // Simple mesh creation which assumes magnification == 1.
+        // TODO: do a more general solution that scales, adds new vertices and smoothes the result.
+
+        try
+        {
+            // One vertice per heightmap value plus the vertices off the top and bottom edge.
+            int totalVertices = (sizeX + 1) * (sizeY + 1);
+            vertices = new float[totalVertices * 3];
+            int totalIndices = sizeX * sizeY * 6;
+            indices = new int[totalIndices];
+
+            float magX = (float)sizeX / extentX;
+            float magY = (float)sizeY / extentY;
+            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3},magX={4},magY={5}",
+                                    BSScene.DetailLogZero, totalVertices, totalIndices, extentBase, magX, magY);
+            // Note that sizeX+1 vertices are created since there is land between this and the next region.
+            for (int yy = 0; yy <= sizeY; yy++)
+            {
+                for (int xx = 0; xx <= sizeX; xx++)     // Hint: the "<=" means we got through sizeX + 1 times
+                {
+                    int offset = yy * sizeX + xx;
+                    // Extend the height from the height from the last row or column
+                    if (yy == sizeY) offset -= sizeX;
+                    if (xx == sizeX) offset -= 1;
+                    float height = heightMap[offset];
+                    vertices[verticesCount + 0] = (float)xx * magX + extentBase.X;
+                    vertices[verticesCount + 1] = (float)yy * magY + extentBase.Y;
+                    vertices[verticesCount + 2] = height + extentBase.Z;
+                    verticesCount += 3;
+                }
+            }
+            verticesCount = verticesCount / 3;
+            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeVerts,verCount={1}", 
+                                            BSScene.DetailLogZero, verticesCount);
+
+            for (int yy = 0; yy < sizeY; yy++)
+            {
+                for (int xx = 0; xx < sizeX; xx++)
+                {
+                    int offset = yy * sizeX + xx;
+                    // Each vertices is presumed to be the upper left corner of a box of two triangles
+                    indices[indicesCount + 0] = offset;
+                    indices[indicesCount + 1] = offset + 1;
+                    indices[indicesCount + 2] = offset + sizeX + 1; // accounting for the extra column
+                    indices[indicesCount + 3] = offset + 1;
+                    indices[indicesCount + 4] = offset + sizeX + 2;
+                    indices[indicesCount + 5] = offset + sizeX + 1;
+                    indicesCount += 6;
+                }
+            }
+            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeIndices,indCount={1}",   // DEEBUG DEBUG DEBUG
+                                                        LogHeader, indicesCount);                        // DEBUG
+            ret = true;
+        }
+        catch (Exception e)
+        {
+            physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
+                                                LogHeader, physicsScene.RegionName, extentBase, e);
+        }
+
+        indicesCountO = indicesCount;
+        indicesO = indices;
+        verticesCountO = verticesCount;
+        verticesO = vertices;
+
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 5ffd591..e60a760 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -88,11 +88,11 @@ public struct BulletShape
     public BulletShape(IntPtr xx)
     {
         ptr = xx;
-        type=ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
-        shapeKey = 0;
+        type=BSPhysicsShapeType.SHAPE_UNKNOWN;
+        shapeKey = (System.UInt64)FixedShapeKey.KEY_NONE;
         isNativeShape = false;
     }
-    public BulletShape(IntPtr xx, ShapeData.PhysicsShapeType typ)
+    public BulletShape(IntPtr xx, BSPhysicsShapeType typ)
     {
         ptr = xx;
         type = typ;
@@ -100,7 +100,7 @@ public struct BulletShape
         isNativeShape = false;
     }
     public IntPtr ptr;
-    public ShapeData.PhysicsShapeType type;
+    public BSPhysicsShapeType type;
     public System.UInt64 shapeKey;
     public bool isNativeShape;
     public override string ToString()
@@ -152,7 +152,7 @@ public class BulletHeightMapInfo
         ID = id;
         Ptr = xx;
         heightMap = hm;
-        terrainRegionBase = new Vector2(0f, 0f);
+        terrainRegionBase = Vector3.Zero;
         minCoords = new Vector3(100f, 100f, 25f);
         maxCoords = new Vector3(101f, 101f, 26f);
         minZ = maxZ = 0f;
@@ -161,7 +161,7 @@ public class BulletHeightMapInfo
     public uint ID;
     public IntPtr Ptr;
     public float[] heightMap;
-    public Vector2 terrainRegionBase;
+    public Vector3 terrainRegionBase;
     public Vector3 minCoords;
     public Vector3 maxCoords;
     public float sizeX, sizeY;
@@ -178,25 +178,39 @@ public struct ConvexHull
         int VertexCount;
         Vector3[] Vertices;
 }
+public enum BSPhysicsShapeType
+{
+        SHAPE_UNKNOWN   = 0,
+        SHAPE_CAPSULE   = 1,
+        SHAPE_BOX       = 2,
+        SHAPE_CONE      = 3,
+        SHAPE_CYLINDER  = 4,
+        SHAPE_SPHERE    = 5,
+        SHAPE_MESH      = 6,
+        SHAPE_HULL      = 7,
+    // following defined by BulletSim
+        SHAPE_GROUNDPLANE  = 20,
+        SHAPE_TERRAIN   = 21,
+        SHAPE_COMPOUND  = 22,
+        SHAPE_HEIGHTMAP = 23,
+};
+
+// The native shapes have predefined shape hash keys
+public enum FixedShapeKey : ulong
+{
+    KEY_NONE        = 0,
+    KEY_BOX         = 1,
+    KEY_SPHERE      = 2,
+    KEY_CONE        = 3,
+    KEY_CYLINDER    = 4,
+    KEY_CAPSULE     = 5,
+}
+
 [StructLayout(LayoutKind.Sequential)]
 public struct ShapeData
 {
-    public enum PhysicsShapeType
-    {
-                SHAPE_UNKNOWN   = 0,
-                SHAPE_AVATAR    = 1,
-                SHAPE_BOX       = 2,
-                SHAPE_CONE      = 3,
-                SHAPE_CYLINDER  = 4,
-                SHAPE_SPHERE    = 5,
-                SHAPE_MESH      = 6,
-                SHAPE_HULL      = 7,
-        // following defined by BulletSim
-                SHAPE_GROUNDPLANE  = 20,
-                SHAPE_TERRAIN   = 21,
-    };
     public uint ID;
-    public PhysicsShapeType Type;
+    public BSPhysicsShapeType Type;
     public Vector3 Position;
     public Quaternion Rotation;
     public Vector3 Velocity;
@@ -215,16 +229,6 @@ public struct ShapeData
     // note that bools are passed as floats since bool size changes by language and architecture
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
-
-    // The native shapes have predefined shape hash keys
-    public enum FixedShapeKey : ulong
-    {
-        KEY_BOX         = 1,
-        KEY_SPHERE      = 2,
-        KEY_CONE        = 3,
-        KEY_CYLINDER    = 4,
-        KEY_CAPSULE     = 5,
-    }
 }
 [StructLayout(LayoutKind.Sequential)]
 public struct SweepHit
@@ -279,6 +283,7 @@ public struct ConfigurationParameters
         public float ccdSweptSphereRadius;
     public float contactProcessingThreshold;
 
+    public float terrainImplementation;
     public float terrainFriction;
     public float terrainHitFraction;
     public float terrainRestitution;
@@ -286,7 +291,8 @@ public struct ConfigurationParameters
     public float avatarStandingFriction;
     public float avatarDensity;
     public float avatarRestitution;
-    public float avatarCapsuleRadius;
+    public float avatarCapsuleWidth;
+    public float avatarCapsuleDepth;
     public float avatarCapsuleHeight;
         public float avatarContactProcessingThreshold;
 
@@ -299,6 +305,7 @@ public struct ConfigurationParameters
         public float shouldEnableFrictionCaching;
         public float numberOfSolverIterations;
 
+    public float linksetImplementation;
     public float linkConstraintUseFrameOffset;
     public float linkConstraintEnableTransMotor;
     public float linkConstraintTransMotorMaxVel;
@@ -378,6 +385,7 @@ public enum CollisionFilterGroups : uint
     BTerrainFilter           = 1 << 11,
     BRaycastFilter           = 1 << 12,
     BSolidFilter             = 1 << 13,
+    BLinksetFilter           = 1 << 14,
 
     // The collsion filters and masked are defined in one place -- don't want them scattered
     AvatarFilter            = BCharacterFilter,
@@ -385,18 +393,18 @@ public enum CollisionFilterGroups : uint
     ObjectFilter            = BSolidFilter,
     ObjectMask              = BAllFilter,
     StaticObjectFilter      = BStaticFilter,
-    StaticObjectMask        = BAllFilter,
+    StaticObjectMask        = BAllFilter & ~BStaticFilter,   // static objects don't collide with each other
+    LinksetFilter           = BLinksetFilter,
+    LinksetMask             = BAllFilter & ~BLinksetFilter, // linkset objects don't collide with each other
     VolumeDetectFilter      = BSensorTrigger,
     VolumeDetectMask        = ~BSensorTrigger,
     TerrainFilter           = BTerrainFilter,
-    TerrainMask             = BAllFilter & ~BStaticFilter,
+    TerrainMask             = BAllFilter & ~BStaticFilter,  // static objects on the ground don't collide
     GroundPlaneFilter       = BGroundPlaneFilter,
     GroundPlaneMask         = BAllFilter
 
 };
 
-
-
 // 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
@@ -426,140 +434,6 @@ static class BulletSimAPI {
 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-[return: MarshalAs(UnmanagedType.LPStr)]
-public static extern string GetVersion();
-
-/* Remove the linkage to the old api methods
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern uint Initialize(Vector3 maxPosition, IntPtr parms,
-                        int maxCollisions, IntPtr collisionArray,
-                        int maxUpdates, IntPtr updateArray,
-                        DebugLogCallback logRoutine);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void CreateInitialGroundPlaneAndTerrain(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightmap(uint worldID, [MarshalAs(UnmanagedType.LPArray)] float[] heightMap);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void Shutdown(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateParameter(uint worldID, uint localID,
-                        [MarshalAs(UnmanagedType.LPStr)]string paramCode, float value);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int PhysicsStep(uint worldID, float timeStep, int maxSubSteps, float fixedTimeStep,
-                        out int updatedEntityCount,
-                        out IntPtr updatedEntitiesPtr,
-                        out int collidersCount,
-                        out IntPtr collidersPtr);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateHull(uint worldID, System.UInt64 meshKey,
-                            int hullCount, [MarshalAs(UnmanagedType.LPArray)] float[] hulls
-    );
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateMesh(uint worldID, System.UInt64 meshKey,
-                            int indexCount, [MarshalAs(UnmanagedType.LPArray)] int[] indices,
-                            int verticesCount, [MarshalAs(UnmanagedType.LPArray)] float[] vertices
-    );
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyHull(uint worldID, System.UInt64 meshKey);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyMesh(uint worldID, System.UInt64 meshKey);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool CreateObject(uint worldID, ShapeData shapeData);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Quaternion GetObjectOrientation(uint WorldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectVelocity(uint worldID, uint id, Vector3 velocity);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectAngularVelocity(uint worldID, uint id, Vector3 angularVelocity);
-
-// Set the current force acting on the object
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectForce(uint worldID, uint id, Vector3 force);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectScaleMass(uint worldID, uint id, Vector3 scale, float mass, bool isDynamic);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectCollidable(uint worldID, uint id, bool phantom);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectDynamic(uint worldID, uint id, bool isDynamic, float mass);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectGhost(uint worldID, uint id, bool ghostly);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectProperties(uint worldID, uint id, bool isStatic, bool isSolid, bool genCollisions, float mass);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetObjectBuoyancy(uint worldID, uint id, float buoyancy);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool HasObject(uint worldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyObject(uint worldID, uint id);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern SweepHit ConvexSweepTest(uint worldID, uint id, Vector3 to, float extraMargin);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern RaycastHit RayTest(uint worldID, uint id, Vector3 from, Vector3 to);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 RecoverFromPenetration(uint worldID, uint id);
-
-// ===============================================================================
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void DumpBulletStatistics();
-*/
-// Log a debug message
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetDebugLogCallback(DebugLogCallback callback);
-
-// ===============================================================================
-// ===============================================================================
-// ===============================================================================
-// A new version of the API that enables moving all the logic out of the C++ code and into
-//    the C# code. This will make modifications easier for the next person.
-// This interface passes the actual pointers to the objects in the unmanaged
-//    address space. All the management (calls for creation/destruction/lookup)
-//    is done in the C# code.
-// The names have a "2" tacked on. This will be removed as the C# code gets rebuilt
-//    and the old code is removed.
-
-// Functions use while converting from API1 to API2. Can be removed when totally converted.
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetSimHandle2(uint worldID);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandle2(IntPtr world, uint id);
-
 // ===============================================================================
 // Initialization and simulation
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -611,13 +485,25 @@ public static extern bool IsNativeShape2(IntPtr shape);
 public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float height, Vector3 scale);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateCompoundShape2(IntPtr sim);
+public static extern IntPtr CreateCompoundShape2(IntPtr sim, bool enableDynamicAabbTree);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void AddChildToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+public static extern int GetNumberOfCompoundChildren2(IntPtr cShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void RemoveChildFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
+public static extern void AddChildShapeToCompoundShape2(IntPtr cShape, IntPtr addShape, Vector3 pos, Quaternion rot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShape, int indx);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr DuplicateCollisionShape2(IntPtr sim, IntPtr srcShape, uint id);
@@ -881,10 +767,10 @@ public static extern float GetCcdMotionThreshold2(IntPtr obj);
 public static extern void SetCcdMotionThreshold2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern float GetCcdSweepSphereRadius2(IntPtr obj);
+public static extern float GetCcdSweptSphereRadius2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetCcdSweepSphereRadius2(IntPtr obj, float val);
+public static extern void SetCcdSweptSphereRadius2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr GetUserPointer2(IntPtr obj);
@@ -907,6 +793,12 @@ public static extern Vector3 GetGravity2(IntPtr obj);
 public static extern void SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearDamping2(IntPtr obj, float lin_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularDamping2(IntPtr obj, float ang_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern float GetLinearDamping2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..0d1db3b
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/Properties/AssemblyInfo.cs
@@ -0,0 +1,33 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// General Information about an assembly is controlled through the following 
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("OpenSim.Region.Physics.BulletSPlugin")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("http://opensimulator.org")]
+[assembly: AssemblyProduct("OpenSim")]
+[assembly: AssemblyCopyright("OpenSimulator developers")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible 
+// to COM components.  If you need to access a type in this assembly from 
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("520ea11b-20cb-449d-ba05-c01015fed841")]
+
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version 
+//      Build Number
+//      Revision
+//
+[assembly: AssemblyVersion("0.7.5.*")]
+[assembly: AssemblyFileVersion("1.0.0.0")]