10 files changed, 845 insertions, 455 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
new file mode 100755
index 0000000..683bc51
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
@@ -0,0 +1,118 @@
+/*
+ * 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 OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+
+public class BS6DofConstraint : BSConstraint
+{
+    // Create a btGeneric6DofConstraint
+    public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1, Quaternion frame1rot,
+                    Vector3 frame2, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+    {
+        m_world = world;
+        m_body1 = obj1;
+        m_body2 = obj2;
+        m_constraint = new BulletConstraint(
+                            BulletSimAPI.Create6DofConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
+                                frame1, frame1rot,
+                                frame2, frame2rot,
+                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+        m_enabled = true;
+    }
+
+    public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+                    Vector3 joinPoint,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
+    {
+        m_world = world;
+        m_body1 = obj1;
+        m_body2 = obj2;
+        m_constraint = new BulletConstraint(
+                            BulletSimAPI.Create6DofConstraintToPoint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
+                                joinPoint,
+                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));
+        m_enabled = true;
+    }
+
+    public bool SetFrames(Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot)
+    {
+        bool ret = false;
+        if (m_enabled)
+        {
+            BulletSimAPI.SetFrames2(m_constraint.Ptr, frameA, frameArot, frameB, frameBrot);
+            ret = true;
+        }
+        return ret;
+    }
+
+    public bool SetCFMAndERP(float cfm, float erp)
+    {
+        bool ret = false;
+        if (m_enabled)
+        {
+            BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+            BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_ERP, erp, ConstraintParamAxis.AXIS_ALL);
+            BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
+            ret = true;
+        }
+        return ret;
+    }
+
+    public bool UseFrameOffset(bool useOffset)
+    {
+        bool ret = false;
+        float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.UseFrameOffset2(m_constraint.Ptr, onOff);
+        return ret;
+    }
+
+    public bool TranslationalLimitMotor(bool enable, float targetVelocity, float maxMotorForce)
+    {
+        bool ret = false;
+        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
+        return ret;
+    }
+
+    public bool SetBreakingImpulseThreshold(float threshold)
+    {
+        bool ret = false;
+        if (m_enabled)
+            ret = BulletSimAPI.SetBreakingImpulseThreshold2(m_constraint.Ptr, threshold);
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 09e1f0c..e2f7af9 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -40,6 +40,7 @@ public class BSCharacter : PhysicsActor
     private static readonly string LogHeader = "[BULLETS CHAR]";
 
     private BSScene _scene;
+    public BSScene Scene { get { return _scene; } }
     private String _avName;
     // private bool _stopped;
     private Vector3 _size;
@@ -73,6 +74,12 @@ public class BSCharacter : PhysicsActor
     private bool _kinematic;
     private float _buoyancy;
 
+    private BulletBody m_body;
+    public BulletBody Body { 
+        get { return m_body; }
+        set { m_body = value; }
+    }
+
     private int _subscribedEventsMs = 0;
     private int _nextCollisionOkTime = 0;
 
@@ -95,7 +102,9 @@ public class BSCharacter : PhysicsActor
         _orientation = Quaternion.Identity;
         _velocity = Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
-        _scale = new Vector3(1f, 1f, 1f);
+        // The dimensions of the avatar capsule are kept in the scale.
+        // Physics creates a unit capsule which is scaled by the physics engine.
+        _scale = new Vector3(_scene.Params.avatarCapsuleRadius, _scene.Params.avatarCapsuleRadius, size.Z);
         _density = _scene.Params.avatarDensity;
         ComputeAvatarVolumeAndMass();   // set _avatarVolume and _mass based on capsule size, _density and _scale
 
@@ -113,9 +122,13 @@ public class BSCharacter : PhysicsActor
         shapeData.Restitution = _scene.Params.avatarRestitution;
 
         // do actual create at taint time
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSCharacter.create", delegate()
         {
             BulletSimAPI.CreateObject(parent_scene.WorldID, shapeData);
+
+            m_body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
+            // avatars get all collisions no matter what
+            BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
         });
             
         return;
@@ -124,7 +137,8 @@ public class BSCharacter : PhysicsActor
     // called when this character is being destroyed and the resources should be released
     public void Destroy()
     {
-        _scene.TaintedObject(delegate()
+        // DetailLog("{0},BSCharacter.Destroy", LocalID);
+        _scene.TaintedObject("BSCharacter.destroy", delegate()
         {
             BulletSimAPI.DestroyObject(_scene.WorldID, _localID);
         });
@@ -138,9 +152,28 @@ public class BSCharacter : PhysicsActor
     public override bool Stopped { 
         get { return false; } 
     }
-    public override Vector3 Size { 
-        get { return _size; } 
-        set { _size = value;
+    public override Vector3 Size {
+        get
+        {
+            // Avatar capsule size is kept in the scale parameter.
+            return new Vector3(_scale.X * 2, _scale.Y * 2, _scale.Z);
+        }
+
+        set { 
+            // When an avatar's size is set, only the height is changed
+            //    and that really only depends on the radius.
+            _size = value;
+            _scale.Z = (_size.Z * 1.15f) - (_scale.X + _scale.Y);
+
+            // TODO: something has to be done with the avatar's vertical position
+
+            ComputeAvatarVolumeAndMass();
+
+            _scene.TaintedObject("BSCharacter.setSize", delegate()
+            {
+                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, LocalID, _scale, _mass, true);
+            });
+
         } 
     }
     public override PrimitiveBaseShape Shape { 
@@ -172,12 +205,37 @@ public class BSCharacter : PhysicsActor
         } 
         set {
             _position = value;
-            _scene.TaintedObject(delegate()
+            PositionSanityCheck();
+
+            _scene.TaintedObject("BSCharacter.setPosition", delegate()
             {
+                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _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.
+    // 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 = Scene.GetTerrainHeightAtXYZ(_position);
+        if (_position.Z < terrainHeight)
+        {
+            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},orient={2}", LocalID, _position, _orientation);
+            _position.Z = terrainHeight + 2.0f;
+            ret = true;
+        }
+
+        // TODO: check for out of bounds
+
+        return ret;
+    }
+
     public override float Mass { 
         get { 
             return _mass; 
@@ -188,9 +246,10 @@ public class BSCharacter : PhysicsActor
         set {
             _force = value;
             // m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force);
-            _scene.TaintedObject(delegate()
+            Scene.TaintedObject("BSCharacter.SetForce", delegate()
             {
-                BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                DetailLog("{0},BSCharacter.setForce,taint,force={1}", LocalID, _force);
+                BulletSimAPI.SetObjectForce(Scene.WorldID, LocalID, _force);
             });
         } 
     }
@@ -214,8 +273,9 @@ public class BSCharacter : PhysicsActor
         set {
             _velocity = value;
             // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, _velocity);
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSCharacter.setVelocity", delegate()
             {
+                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, _velocity);
                 BulletSimAPI.SetObjectVelocity(_scene.WorldID, _localID, _velocity);
             });
         } 
@@ -239,7 +299,7 @@ public class BSCharacter : PhysicsActor
         set {
             _orientation = value;
             // m_log.DebugFormat("{0}: set orientation to {1}", LogHeader, _orientation);
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSCharacter.setOrientation", delegate()
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
@@ -259,9 +319,12 @@ public class BSCharacter : PhysicsActor
     public override bool Flying { 
         get { return _flying; } 
         set {
-            _flying = value;
-            // simulate flying by changing the effect of gravity
-            this.Buoyancy = ComputeBuoyancyFromFlying(_flying);
+            if (_flying != value)
+            {
+                _flying = value;
+                // simulate flying by changing the effect of gravity
+                this.Buoyancy = ComputeBuoyancyFromFlying(_flying);
+            }
         } 
     }
     private float ComputeBuoyancyFromFlying(bool ifFlying) {
@@ -303,8 +366,9 @@ public class BSCharacter : PhysicsActor
     public override float Buoyancy { 
         get { return _buoyancy; } 
         set { _buoyancy = value; 
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSCharacter.setBuoyancy", delegate()
             {
+                DetailLog("{0},BSCharacter.setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _buoyancy);
             });
         } 
@@ -349,9 +413,10 @@ public class BSCharacter : PhysicsActor
             _force.Y += force.Y;
             _force.Z += force.Z;
             // m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSCharacter.AddForce", delegate()
             {
-                BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
+                BulletSimAPI.AddObjectForce2(Body.Ptr, _force);
             });
         }
         else
@@ -369,11 +434,25 @@ public class BSCharacter : PhysicsActor
     // Turn on collision events at a rate no faster than one every the given milliseconds
     public override void SubscribeEvents(int ms) {
         _subscribedEventsMs = ms;
-        _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen
+        if (ms > 0)
+        {
+            // make sure first collision happens
+            _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs;
+
+            Scene.TaintedObject("BSCharacter.SubscribeEvents", delegate()
+            {
+                BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            });
+        }
     }
     // Stop collision events
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
+        // Avatars get all their collision events
+        // Scene.TaintedObject("BSCharacter.UnSubscribeEvents", delegate()
+        // {
+        //     BulletSimAPI.RemoveFromCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        // });
     }
     // Return 'true' if someone has subscribed to events
     public override bool SubscribedEvents() {
@@ -385,9 +464,15 @@ public class BSCharacter : PhysicsActor
     {
         _avatarVolume = (float)(
                         Math.PI
-                        * _scene.Params.avatarCapsuleRadius * _scale.X
-                        * _scene.Params.avatarCapsuleRadius * _scale.Y
-                        * _scene.Params.avatarCapsuleHeight * _scale.Z);
+                        * _scale.X
+                        * _scale.Y  // the area of capsule cylinder
+                        * _scale.Z  // times height of capsule cylinder
+                      + 1.33333333f
+                        * Math.PI
+                        * _scale.X
+                        * Math.Min(_scale.X, _scale.Y)
+                        * _scale.Y  // plus the volume of the capsule end caps
+                        );
         _mass = _density * _avatarVolume;
     }
 
@@ -395,43 +480,19 @@ public class BSCharacter : PhysicsActor
     // the world that things have changed.
     public void UpdateProperties(EntityProperties entprop)
     {
-        /*
-        bool changed = false;
-        // we assign to the local variables so the normal set action does not happen
-        if (_position != entprop.Position) {
-            _position = entprop.Position;
-            changed = true;
-        }
-        if (_orientation != entprop.Rotation) {
-            _orientation = entprop.Rotation;
-            changed = true;
-        }
-        if (_velocity != entprop.Velocity) {
-            _velocity = entprop.Velocity;
-            changed = true;
-        }
-        if (_acceleration != entprop.Acceleration) {
-            _acceleration = entprop.Acceleration;
-            changed = true;
-        }
-        if (_rotationalVelocity != entprop.RotationalVelocity) {
-            _rotationalVelocity = entprop.RotationalVelocity;
-            changed = true;
-        }
-        if (changed) {
-            // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-            // Avatar movement is not done by generating this event. There is code in the heartbeat
-            //   loop that updates avatars.
-            // base.RequestPhysicsterseUpdate();
-        }
-        */
         _position = entprop.Position;
         _orientation = entprop.Rotation;
         _velocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
-        // Avatars don't report theirr changes the usual way. Changes are checked for in the heartbeat loop.
+        // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
+
+        /*
+        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
+                entprop.Acceleration, entprop.RotationalVelocity);
+         */
     }
 
     // Called by the scene when a collision with this object is reported
@@ -448,6 +509,7 @@ public class BSCharacter : PhysicsActor
         {
             _collidingGroundStep = _scene.SimulationStep;
         }
+        // DetailLog("{0},BSCharacter.Collison,call,with={1}", LocalID, collidingWith);
 
         // throttle collisions to the rate specified in the subscription
         if (_subscribedEventsMs != 0) {
@@ -476,9 +538,17 @@ public class BSCharacter : PhysicsActor
         if (collisionCollection == null)
             collisionCollection = new CollisionEventUpdate();
         base.SendCollisionUpdate(collisionCollection);
-        collisionCollection.Clear();
+        // If there were any collisions in the collection, make sure we don't use the
+        //    same instance next time.
+        if (collisionCollection.Count > 0)
+            collisionCollection = null;
         // End kludge
     }
 
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DetailLog(string msg, params Object[] args)
+    {
+        Scene.PhysicsLogging.Write(msg, args);
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index ea3093a..25084d8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -32,35 +32,26 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-public class BSConstraint : IDisposable
+public abstract class BSConstraint : IDisposable
 {
-    private BulletSim m_world;
-    private BulletBody m_body1;
-    private BulletBody m_body2;
-    private BulletConstraint m_constraint;
-    private bool m_enabled = false;
+    protected BulletSim m_world;
+    protected BulletBody m_body1;
+    protected BulletBody m_body2;
+    protected BulletConstraint m_constraint;
+    protected bool m_enabled = false;
 
-    public BSConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
-                    Vector3 frame1, Quaternion frame1rot,
-                    Vector3 frame2, Quaternion frame2rot
-        )
+    public BSConstraint()
     {
-        m_world = world;
-        m_body1 = obj1;
-        m_body2 = obj2;
-        m_constraint = new BulletConstraint(BulletSimAPI.CreateConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
-                            frame1, frame1rot,
-                            frame2, frame2rot,
-                            true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
-        m_enabled = true;
     }
 
-    public void Dispose()
+    public virtual void Dispose()
     {
         if (m_enabled)
         {
             // BulletSimAPI.RemoveConstraint(m_world.ID, m_body1.ID, m_body2.ID);
-            BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
+            bool success = BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
+            m_world.scene.DetailLog("{0},BSConstraint.Dispose,taint,body1={1},body2={2},success={3}", BSScene.DetailLogZero, m_body1.ID, m_body2.ID, success);
+            m_constraint.Ptr = System.IntPtr.Zero;
             m_enabled = false;
         }
     }
@@ -68,7 +59,7 @@ public class BSConstraint : IDisposable
     public BulletBody Body1 { get { return m_body1; } }
     public BulletBody Body2 { get { return m_body2; } }
 
-    public bool SetLinearLimits(Vector3 low, Vector3 high)
+    public virtual bool SetLinearLimits(Vector3 low, Vector3 high)
     {
         bool ret = false;
         if (m_enabled)
@@ -76,7 +67,7 @@ public class BSConstraint : IDisposable
         return ret;
     }
 
-    public bool SetAngularLimits(Vector3 low, Vector3 high)
+    public virtual bool SetAngularLimits(Vector3 low, Vector3 high)
     {
         bool ret = false;
         if (m_enabled)
@@ -84,40 +75,36 @@ public class BSConstraint : IDisposable
         return ret;
     }
 
-    public bool SetCFMAndERP(float cfm, float erp)
-    {
-        bool ret = true;
-        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
-        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_STOP_ERP, erp, ConstraintParamAxis.AXIS_ALL);
-        BulletSimAPI.SetConstraintParam2(m_constraint.Ptr, ConstraintParams.BT_CONSTRAINT_CFM, cfm, ConstraintParamAxis.AXIS_ALL);
-        return ret;
-    }
-
-    public bool UseFrameOffset(bool useOffset)
-    {
-        bool ret = false;
-        float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
-        if (m_enabled)
-            ret = BulletSimAPI.UseFrameOffset2(m_constraint.Ptr, onOff);
-        return ret;
-    }
-
-    public bool TranslationalLimitMotor(bool enable, float targetVelocity, float maxMotorForce)
+    public virtual bool CalculateTransforms()
     {
         bool ret = false;
-        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
         if (m_enabled)
-            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
+        {
+            // Recompute the internal transforms
+            BulletSimAPI.CalculateTransforms2(m_constraint.Ptr);
+            ret = true;
+        }
         return ret;
     }
 
-    public bool CalculateTransforms()
+    // Reset this constraint making sure it has all its internal structures
+    //    recomputed and is enabled and ready to go.
+    public virtual bool RecomputeConstraintVariables(float mass)
     {
         bool ret = false;
         if (m_enabled)
         {
-            BulletSimAPI.CalculateTransforms2(m_constraint.Ptr);
-            ret = true;
+            ret = CalculateTransforms();
+            if (ret)
+            {
+                // m_world.scene.PhysicsLogging.Write("{0},BSConstraint.RecomputeConstraintVariables,taint,enabling,A={1},B={2}",
+                //                 BSScene.DetailLogZero, Body1.ID, Body2.ID);
+                BulletSimAPI.SetConstraintEnable2(m_constraint.Ptr, m_world.scene.NumericBool(true));
+            }
+            else
+            {
+                m_world.scene.Logger.ErrorFormat("[BULLETSIM CONSTRAINT] CalculateTransforms failed. A={0}, B={1}", Body1.ID, Body2.ID);
+            }
         }
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index c88e645..22ea367 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -56,29 +56,25 @@ public class BSConstraintCollection : IDisposable
 
     public void Clear()
     {
-        foreach (BSConstraint cons in m_constraints)
+        lock (m_constraints)
         {
-            cons.Dispose();
+            foreach (BSConstraint cons in m_constraints)
+            {
+                cons.Dispose();
+            }
+            m_constraints.Clear();
         }
-        m_constraints.Clear();
-    }
-
-    public BSConstraint CreateConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
-                    Vector3 frame1, Quaternion frame1rot,
-                    Vector3 frame2, Quaternion frame2rot)
-    {
-        BSConstraint constrain = new BSConstraint(world, obj1, obj2, frame1, frame1rot, frame2, frame2rot);
-
-        this.AddConstraint(constrain);
-        return constrain;
     }
 
     public bool AddConstraint(BSConstraint cons)
     {
-        // There is only one constraint between any bodies. Remove any old just to make sure.
-        RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
+        lock (m_constraints)
+        {
+            // There is only one constraint between any bodies. Remove any old just to make sure.
+            RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
 
-        m_constraints.Add(cons);
+            m_constraints.Add(cons);
+        }
 
         return true;
     }
@@ -92,16 +88,19 @@ public class BSConstraintCollection : IDisposable
 
         uint lookingID1 = body1.ID;
         uint lookingID2 = body2.ID;
-        ForEachConstraint(delegate(BSConstraint constrain)
+        lock (m_constraints)
         {
-            if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2)
-                || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1))
+            foreach (BSConstraint constrain in m_constraints)
             {
-                foundConstraint = constrain;
-                found = true;
+                if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2)
+                    || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1))
+                {
+                    foundConstraint = constrain;
+                    found = true;
+                    break;
+                }
             }
-            return found;
-        });
+        }
         returnConstraint = foundConstraint;
         return found;
     }
@@ -111,22 +110,33 @@ public class BSConstraintCollection : IDisposable
     // Return 'true' if a constraint was found and destroyed.
     public bool RemoveAndDestroyConstraint(BulletBody body1, BulletBody body2)
     {
-        // return BulletSimAPI.RemoveConstraint(m_world.ID, obj1.ID, obj2.ID);
-
         bool ret = false;
-        BSConstraint constrain;
+        lock (m_constraints)
+        {
+            BSConstraint constrain;
+            if (this.TryGetConstraint(body1, body2, out constrain))
+            {
+                // remove the constraint from our collection
+                RemoveAndDestroyConstraint(constrain);
+                ret = true;
+            }
+        }
 
-        if (this.TryGetConstraint(body1, body2, out constrain))
+        return ret;
+    }
+
+    // The constraint MUST exist in the collection
+    public bool RemoveAndDestroyConstraint(BSConstraint constrain)
+    {
+        lock (m_constraints)
         {
             // remove the constraint from our collection
             m_constraints.Remove(constrain);
-            // tell the engine that all its structures need to be freed
-            constrain.Dispose();
-            // we destroyed something
-            ret = true;
         }
-
-        return ret;
+        // tell the engine that all its structures need to be freed
+        constrain.Dispose();
+        // we destroyed something
+        return true;
     }
 
     // Remove all constraints that reference the passed body.
@@ -137,16 +147,15 @@ public class BSConstraintCollection : IDisposable
 
         List<BSConstraint> toRemove = new List<BSConstraint>();
         uint lookingID = body1.ID;
-        ForEachConstraint(delegate(BSConstraint constrain)
+        lock (m_constraints)
         {
-            if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID)
+            foreach (BSConstraint constrain in m_constraints)
             {
-                toRemove.Add(constrain);
+                if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID)
+                {
+                    toRemove.Add(constrain);
+                }
             }
-            return false;
-        });
-        lock (m_constraints)
-        {
             foreach (BSConstraint constrain in toRemove)
             {
                 m_constraints.Remove(constrain);
@@ -158,27 +167,16 @@ public class BSConstraintCollection : IDisposable
 
     public bool RecalculateAllConstraints()
     {
-        foreach (BSConstraint constrain in m_constraints)
-        {
-            constrain.CalculateTransforms();
-        }
-        return true;
-    }
-
-    // Lock the constraint list and loop through it.
-    // The constraint action returns 'true' if it wants the loop aborted.
-    private void ForEachConstraint(ConstraintAction action)
-    {
+        bool ret = false;
         lock (m_constraints)
         {
             foreach (BSConstraint constrain in m_constraints)
             {
-                if (action(constrain))
-                    break;
+                constrain.CalculateTransforms();
+                ret = true;
             }
         }
+        return ret;
     }
-
-
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index c197e61..5a9f135 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -613,7 +613,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             MoveAngular(pTimestep);
             LimitRotation(pTimestep);
 
-            DetailLog("{0},Dynamics,done,pos={1},force={2},velocity={3},angvel={4}", 
+            DetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 
                     m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
         }// end Step
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSHingeConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSHingeConstraint.cs
new file mode 100755
index 0000000..d68048b
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSHingeConstraint.cs
@@ -0,0 +1,55 @@
+/*

+ * 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 OpenMetaverse;

+

+namespace OpenSim.Region.Physics.BulletSPlugin

+{

+

+class BSHingeConstraint : BSConstraint

+{

+    public BSHingeConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,

+                    Vector3 pivotInA, Vector3 pivotInB,

+                    Vector3 axisInA, Vector3 axisInB,

+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)

+    {

+        m_world = world;

+        m_body1 = obj1;

+        m_body2 = obj2;

+        m_constraint = new BulletConstraint(

+                            BulletSimAPI.CreateHingeConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,

+                                pivotInA, pivotInB,

+                                axisInA, axisInB,

+                                useLinearReferenceFrameA, disableCollisionsBetweenLinkedBodies));

+        m_enabled = true;

+    }

+

+}

+

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 6f8430c..087b9bb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -37,10 +37,12 @@ public class BSLinkset
     private static string LogHeader = "[BULLETSIM LINKSET]";
 
     private BSPrim m_linksetRoot;
-    public BSPrim Root { get { return m_linksetRoot; } }
+    public BSPrim LinksetRoot { get { return m_linksetRoot; } }
 
-    private BSScene m_scene;
+    private BSScene m_physicsScene;
+    public BSScene PhysicsScene { get { return m_physicsScene; } }
 
+    // The children under the root in this linkset
     private List<BSPrim> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
@@ -72,7 +74,7 @@ public class BSLinkset
     public BSLinkset(BSScene scene, BSPrim parent)
     {
         // A simple linkset of one (no children)
-        m_scene = scene;
+        m_physicsScene = scene;
         m_linksetRoot = parent;
         m_children = new List<BSPrim>();
         m_mass = parent.MassRaw;
@@ -80,16 +82,19 @@ public class BSLinkset
 
     // Link to a linkset where the child knows the parent.
     // Parent changing should not happen so do some sanity checking.
-    // We return the parent's linkset so the child can track it's membership.
-    public BSLinkset AddMeToLinkset(BSPrim child, BSPrim parent)
+    // We return the parent's linkset so the child can track its membership.
+    public BSLinkset AddMeToLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
         {
-            parent.Linkset.AddChildToLinkset(child);
+            AddChildToLinkset(child);
         }
-        return parent.Linkset;
+        return this;
     }
 
+    // Remove a child from a linkset.
+    // Returns a new linkset for the child which is a linkset of one (just the
+    //    orphened child).
     public BSLinkset RemoveMeFromLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
@@ -101,7 +106,7 @@ public class BSLinkset
                 {
                     // Note that we don't do a foreach because the remove routine
                     //    takes it out of the list.
-                    RemoveChildFromLinkset(m_children[0]);
+                    RemoveChildFromOtherLinkset(m_children[0]);
                 }
                 m_children.Clear(); // just to make sure
             }
@@ -113,63 +118,17 @@ public class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(m_scene, child);
+        return new BSLinkset(PhysicsScene, child);
     }
 
-    // An existing linkset had one of its members rebuilt or something.
-    // Go through the linkset and rebuild the pointers to the bodies of the linkset members.
-    public BSLinkset RefreshLinkset(BSPrim requestor)
-    {
-        BSLinkset ret = requestor.Linkset;
-
-        lock (m_linksetActivityLock)
-        {
-            System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID);
-            if (aPtr == System.IntPtr.Zero)
-            {
-                // That's odd. We can't find the root of the linkset.
-                // The linkset is somehow dead.  The requestor is now a member of a linkset of one.
-                DetailLog("{0},RefreshLinkset.RemoveRoot,child={1}", m_linksetRoot.LocalID, m_linksetRoot.LocalID);
-                ret = RemoveMeFromLinkset(m_linksetRoot);
-            }
-            else
-            {
-                // Reconstruct the pointer to the body of the linkset root.
-                DetailLog("{0},RefreshLinkset.RebuildRoot,rootID={1},ptr={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, aPtr);
-                m_linksetRoot.Body = new BulletBody(m_linksetRoot.LocalID, aPtr);
-
-                List<BSPrim> toRemove = new List<BSPrim>();
-                foreach (BSPrim bsp in m_children)
-                {
-                    aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, bsp.LocalID);
-                    if (aPtr == System.IntPtr.Zero)
-                    {
-                        toRemove.Add(bsp);
-                    }
-                    else
-                    {
-                        // Reconstruct the pointer to the body of the linkset root.
-                        DetailLog("{0},RefreshLinkset.RebuildChild,rootID={1},ptr={2}", bsp.LocalID, m_linksetRoot.LocalID, aPtr);
-                        bsp.Body = new BulletBody(bsp.LocalID, aPtr);
-                    }
-                }
-                foreach (BSPrim bsp in toRemove)
-                {
-                    RemoveChildFromLinkset(bsp);
-                }
-            }
-        }
-
-        return ret;
-    }
-
-
     // Return 'true' if the passed object is the root object of this linkset
     public bool IsRoot(BSPrim requestor)
     {
         return (requestor.LocalID == m_linksetRoot.LocalID);
     }
 
+    public int NumberOfChildren { get { return m_children.Count; } }
+
     // Return 'true' if this linkset has any children (more than the root member)
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
@@ -177,12 +136,15 @@ public class BSLinkset
     public bool HasChild(BSPrim child)
     {
         bool ret = false;
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            if (child.LocalID == bp.LocalID)
+            foreach (BSPrim bp in m_children)
             {
-                ret = true;
-                break;
+                if (child.LocalID == bp.LocalID)
+                {
+                    ret = true;
+                    break;
+                }
             }
         }
         return ret;
@@ -203,12 +165,16 @@ public class BSLinkset
         OMV.Vector3 com = m_linksetRoot.Position * m_linksetRoot.MassRaw;
         float totalMass = m_linksetRoot.MassRaw;
 
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            com += bp.Position * bp.MassRaw;
-            totalMass += bp.MassRaw;
+            foreach (BSPrim bp in m_children)
+            {
+                com += bp.Position * bp.MassRaw;
+                totalMass += bp.MassRaw;
+            }
+            if (totalMass != 0f)
+                com /= totalMass;
         }
-        com /= totalMass;
 
         return com;
     }
@@ -217,135 +183,237 @@ public class BSLinkset
     {
         OMV.Vector3 com = m_linksetRoot.Position;
 
-        foreach (BSPrim bp in m_children)
+        lock (m_linksetActivityLock)
         {
-            com += bp.Position * bp.MassRaw;
+            foreach (BSPrim bp in m_children)
+            {
+                com += bp.Position * bp.MassRaw;
+            }
+            com /= (m_children.Count + 1);
         }
-        com /= m_children.Count + 1;
 
         return com;
     }
 
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    public void Refresh(BSPrim requestor)
+    {
+        // If there are no children, there aren't any constraints to recompute
+        if (!HasAnyChildren)
+            return;
+
+        // Only the root does the recomputation
+        if (IsRoot(requestor))
+        {
+            PhysicsScene.TaintedObject("BSLinkSet.Refresh", delegate()
+            {
+                RecomputeLinksetConstraintVariables();
+            });
+        }
+    }
+
+    // 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 bool RecomputeLinksetConstraintVariables()
+    {
+        float linksetMass = LinksetMass;
+        lock (m_linksetActivityLock)
+        {
+            foreach (BSPrim child in m_children)
+            {
+                BSConstraint constrain;
+                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.Body, child.Body, 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 can happen when children are being added to the linkset but
+                    //    their constraints have not been created yet.
+                    // Caused by the fact that m_children is built at run time but building constraints
+                    //    happens at taint time.
+                    // m_physicsScene.Logger.ErrorFormat("[BULLETSIM LINKSET] RecomputeLinksetConstraintVariables: constraint not found for root={0}, child={1}",
+                    //                                 m_linksetRoot.Body.ID, child.Body.ID);
+                }
+            }
+        }
+        return false;
+    }
+
     // I am the root of a linkset and a new child is being added
-    public void AddChildToLinkset(BSPrim pchild)
+    // Called while LinkActivity is locked.
+    private void AddChildToLinkset(BSPrim child)
     {
-        BSPrim child = pchild;
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            m_scene.TaintedObject(delegate()
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
-                DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
-                DetailLog("{0},AddChildToLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
-                PhysicallyLinkAChildToRoot(pchild);     // build the physical binding between me and the child
+                // DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
+                // DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the 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
+    //    has to be updated also (like pointer to prim's parent).
+    private void RemoveChildFromOtherLinkset(BSPrim pchild)
+    {
+        pchild.Linkset = new BSLinkset(m_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.
-    public void RemoveChildFromLinkset(BSPrim pchild)
+    private void RemoveChildFromLinkset(BSPrim child)
     {
-        BSPrim child = pchild;
-
         if (m_children.Remove(child))
         {
-            m_scene.TaintedObject(delegate()
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
-                DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
-                DetailLog("{0},RemoveChildFromLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
+                // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
-                if (m_children.Count == 0)
-                {
-                    // if the linkset is empty, make sure all linkages have been removed
-                    PhysicallyUnlinkAllChildrenFromRoot();
-                }
-                else
-                {
-                    PhysicallyUnlinkAChildFromRoot(pchild);
-                }
+                PhysicallyUnlinkAChildFromRoot(rootx, childx);
             });
+
+            RecomputeLinksetConstraintVariables();
         }
         else
         {
             // This will happen if we remove the root of the linkset first. Non-fatal occurance.
-            // m_scene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
+            // 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(BSPrim childPrim)
+    private void PhysicallyLinkAChildToRoot(BSPrim rootPrim, BSPrim 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);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2},rLoc={3},cLoc={4},midLoc={5}", 
+            rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID, rootPrim.Position, childPrim.Position, midPoint);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        m_physicsScene.World, rootPrim.Body, childPrim.Body,
+                        midPoint,
+                        true,
+                        true
+                        );
+        /* NOTE: attempt to build constraint with full frame computation, etc.
+         *     Using the midpoint is easier since it lets the Bullet code use the transforms
+         *     of the objects.
+         * Code left here as an example.
+        // ==================================================================================
         // relative position normalized to the root prim
-        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(m_linksetRoot.Orientation);
-        OMV.Vector3 childRelativePosition = (childPrim.Position - m_linksetRoot.Position) * invThisOrientation;
+        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
         // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
-        DetailLog("{0},LinkAChildToMe,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
-        BSConstraint constrain = m_scene.Constraints.CreateConstraint(
-                        m_scene.World, m_linksetRoot.Body, childPrim.Body,
-                        // childRelativePosition,
-                        // childRelativeRotation,
+        DetailLog("{0},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.Identity,
+                        OMV.Quaternion.Inverse(rootPrim.Orientation),
                         OMV.Vector3.Zero,
-                        OMV.Quaternion.Identity
+                        OMV.Quaternion.Inverse(childPrim.Orientation),
+                        // A point half way between the parent and child
+                        // childRelativePosition/2,
+                        // childRelativeRotation,
+                        // childRelativePosition/2,
+                        // inverseChildRelativeRotation,
+                        true,
+                        true
                         );
+        // ==================================================================================
+        */
+
+        m_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(m_scene.BoolNumeric(m_scene.Params.linkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(m_scene.BoolNumeric(m_scene.Params.linkConstraintEnableTransMotor),
-                        m_scene.Params.linkConstraintTransMotorMaxVel,
-                        m_scene.Params.linkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(m_scene.Params.linkConstraintCFM, m_scene.Params.linkConstraintERP);
+        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);
 
+        RecomputeLinksetConstraintVariables();
     }
 
     // Remove linkage between myself and a particular child
     // Called at taint time!
-    private void PhysicallyUnlinkAChildFromRoot(BSPrim childPrim)
+    private void PhysicallyUnlinkAChildFromRoot(BSPrim rootPrim, BSPrim childPrim)
     {
-        DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
-                    LogHeader, m_linksetRoot.LocalID, childPrim.LocalID);
-        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
-        // BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, childPrim.LocalID);
-        m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body, childPrim.Body);
+        // DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
+        //             LogHeader, rootPrim.LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+
+        // Find the constraint for this link and get rid of it from the overall collection and from my list
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body);
+
+        // Make the child refresh its location
+        BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
     }
 
     // Remove linkage between myself and any possible children I might have
     // Called at taint time!
-    private void PhysicallyUnlinkAllChildrenFromRoot()
+    private void PhysicallyUnlinkAllChildrenFromRoot(BSPrim rootPrim)
     {
         // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
-        DetailLog("{0},PhysicallyUnlinkAllChildren,taint", m_linksetRoot.LocalID);
-        m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body);
-        // BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
+        DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DebugLog(string msg, params Object[] args)
     {
-        m_scene.Logger.DebugFormat(msg, args);
+        if (m_physicsScene.ShouldDebugLog)
+            m_physicsScene.Logger.DebugFormat(msg, args);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DetailLog(string msg, params Object[] args)
     {
-        m_scene.PhysicsLogging.Write(msg, args);
+        m_physicsScene.PhysicsLogging.Write(msg, args);
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index ebfd85b..9c20004 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -42,7 +42,7 @@ public sealed class BSPrim : PhysicsActor
     private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS PRIM]";
 
-    private void DebugLog(string mm, params Object[] xx) { if (_scene.shouldDebugLog) m_log.DebugFormat(mm, xx); }
+    private void DebugLog(string mm, params Object[] xx) { if (_scene.ShouldDebugLog) m_log.DebugFormat(mm, xx); }
 
     private IMesh _mesh;
     private PrimitiveBaseShape _pbs;
@@ -138,14 +138,15 @@ public sealed class BSPrim : PhysicsActor
         _isPhysical = pisPhysical;
         _isVolumeDetect = false;
         _subscribedEventsMs = 0;
-        _friction = _scene.Params.defaultFriction; // TODO: compute based on object material
-        _density = _scene.Params.defaultDensity; // TODO: compute based on object material
+        _friction = _scene.Params.defaultFriction;  // TODO: compute based on object material
+        _density = _scene.Params.defaultDensity;    // TODO: compute based on object material
         _restitution = _scene.Params.defaultRestitution;
         _linkset = new BSLinkset(_scene, this);     // a linkset of one
-        _vehicle = new BSDynamics(this);    // add vehicleness
+        _vehicle = new BSDynamics(this);            // add vehicleness
         _mass = CalculateMass();
         // do the actual object creation at taint time
-        _scene.TaintedObject(delegate()
+        DetailLog("{0},BSPrim.constructor,call", LocalID);
+        _scene.TaintedObject("BSPrim.create", delegate()
         {
             RecreateGeomAndObject();
 
@@ -160,17 +161,22 @@ public sealed class BSPrim : PhysicsActor
     public void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
-        // DetailLog("{0},Destroy", LocalID);
+
+        // Undo any links between me and any other object
+        BSPrim parentBefore = _linkset.LinksetRoot;
+        int childrenBefore = _linkset.NumberOfChildren;
+
+        _linkset = _linkset.RemoveMeFromLinkset(this);
+
+        DetailLog("{0},BSPrim.Destroy,call,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}",
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
 
         // Undo any vehicle properties
-        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
-        _scene.RemoveVehiclePrim(this);     // just to make sure
+        this.VehicleType = (int)Vehicle.TYPE_NONE;
 
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.destroy", delegate()
         {
-            // Undo any links between me and any other object
-            _linkset = _linkset.RemoveMeFromLinkset(this);
-
+            DetailLog("{0},BSPrim.Destroy,taint,", LocalID);
             // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
             BulletSimAPI.DestroyObject(_scene.WorldID, LocalID);
         });
@@ -183,11 +189,11 @@ public sealed class BSPrim : PhysicsActor
         get { return _size; } 
         set {
             _size = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setSize", delegate()
             {
                 _mass = CalculateMass();   // changing size changes the mass
                 BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, (IsPhysical ? _mass : 0f), IsPhysical);
-                DetailLog("{0}: setSize: size={1}, mass={2}, physical={3}", LocalID, _size, _mass, IsPhysical);
+                // DetailLog("{0}: BSPrim.setSize: size={1}, mass={2}, physical={3}", LocalID, _size, _mass, IsPhysical);
                 RecreateGeomAndObject();
             });
         } 
@@ -195,7 +201,7 @@ public sealed class BSPrim : PhysicsActor
     public override PrimitiveBaseShape Shape { 
         set {
             _pbs = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setShape", delegate()
             {
                 _mass = CalculateMass();   // changing the shape changes the mass
                 RecreateGeomAndObject();
@@ -213,7 +219,7 @@ public sealed class BSPrim : PhysicsActor
     public override bool Selected { 
         set {
             _isSelected = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setSelected", delegate()
             {
                 SetObjectDynamic();
             });
@@ -224,10 +230,17 @@ public sealed class BSPrim : PhysicsActor
     // link me to the specified parent
     public override void link(PhysicsActor obj) {
         BSPrim parent = obj as BSPrim;
-        DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
-        DetailLog("{0},link,parent={1}", LocalID, obj.LocalID);
+        if (parent != null)
+        {
+            DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, parent.LocalID);
+            BSPrim parentBefore = _linkset.LinksetRoot;
+            int childrenBefore = _linkset.NumberOfChildren;
 
-        _linkset = _linkset.AddMeToLinkset(this, parent);
+            _linkset = parent.Linkset.AddMeToLinkset(this);
+
+            DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}", 
+                LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
+        }
         return; 
     }
 
@@ -236,10 +249,15 @@ public sealed class BSPrim : PhysicsActor
         // TODO: decide if this parent checking needs to happen at taint time
         // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
         DebugLog("{0}: delink {1}/{2}. Parent={3}", LogHeader, _avName, _localID, 
-                            _linkset.Root._avName+"/"+_linkset.Root.LocalID.ToString());
-        DetailLog("{0},delink,parent={1}", LocalID, _linkset.Root.LocalID.ToString());
+                            _linkset.LinksetRoot._avName+"/"+_linkset.LinksetRoot.LocalID.ToString());
+
+        BSPrim parentBefore = _linkset.LinksetRoot;
+        int childrenBefore = _linkset.NumberOfChildren;
+        
+        _linkset = _linkset.RemoveMeFromLinkset(this);
 
-        _linkset.RemoveMeFromLinkset(this);
+        DetailLog("{0},BSPrim.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ", 
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
         return; 
     }
 
@@ -262,7 +280,7 @@ public sealed class BSPrim : PhysicsActor
 
     public override void LockAngularMotion(OMV.Vector3 axis)
     { 
-        DetailLog("{0},LockAngularMotion,call,axis={1}", LocalID, axis);
+        // DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
         return;
     }
 
@@ -279,9 +297,9 @@ public sealed class BSPrim : PhysicsActor
         set {
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                DetailLog("{0},SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -316,9 +334,9 @@ public sealed class BSPrim : PhysicsActor
         get { return _force; } 
         set {
             _force = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setForce", delegate()
             {
-                DetailLog("{0},SetForce,taint,force={1}", LocalID, _force);
+                // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
                 // BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
                 BulletSimAPI.SetObjectForce2(Body.Ptr, _force);
             });
@@ -331,53 +349,41 @@ public sealed class BSPrim : PhysicsActor
         } 
         set {
             Vehicle type = (Vehicle)value;
-            _scene.TaintedObject(delegate()
+            BSPrim vehiclePrim = this;
+            _scene.TaintedObject("setVehicleType", delegate()
             {
-                DetailLog("{0},SetVehicleType,taint,type={1}", LocalID, type);
+                // 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.
                 _vehicle.ProcessTypeChange(type);
-                if (type == Vehicle.TYPE_NONE)
-                {
-                    _scene.RemoveVehiclePrim(this);
-                }
-                else
-                {
-                    _scene.TaintedObject(delegate()
-                    {
-                        // Tell the physics engine to clear state
-                        BulletSimAPI.ClearForces2(this.Body.Ptr);
-                    });
-
-                    // make it so the scene will call us each tick to do vehicle things
-                    _scene.AddVehiclePrim(this);
-                }
-                return;
+                // Tell the scene about the vehicle so it will get processing each frame.
+                _scene.VehicleInSceneTypeChanged(this, type);
             });
         } 
     }
     public override void VehicleFloatParam(int param, float value) 
     {
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.VehicleFloatParam", delegate()
         {
             _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
         });
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) 
     {
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.VehicleVectorParam", delegate()
         {
             _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
         });
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
     {
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.VehicleRotationParam", delegate()
         {
             _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
         });
     }
     public override void VehicleFlags(int param, bool remove) 
     {
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.VehicleFlags", delegate()
         {
             _vehicle.ProcessVehicleFlags(param, remove);
         });
@@ -395,7 +401,7 @@ public sealed class BSPrim : PhysicsActor
     public override void SetVolumeDetect(int param) {
         bool newValue = (param != 0);
         _isVolumeDetect = newValue;
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.SetVolumeDetect", delegate()
         {
             SetObjectDynamic();
         });
@@ -406,9 +412,9 @@ public sealed class BSPrim : PhysicsActor
         get { return _velocity; } 
         set {
             _velocity = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setVelocity", delegate()
             {
-                DetailLog("{0},SetVelocity,taint,vel={1}", LocalID, _velocity);
+                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
                 BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
             });
         } 
@@ -416,7 +422,7 @@ public sealed class BSPrim : PhysicsActor
     public override OMV.Vector3 Torque { 
         get { return _torque; } 
         set { _torque = value; 
-            DetailLog("{0},SetTorque,call,torque={1}", LocalID, _torque);
+            // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         } 
     }
     public override float CollisionScore { 
@@ -440,10 +446,10 @@ public sealed class BSPrim : PhysicsActor
         set {
             _orientation = value;
             // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setOrientation", delegate()
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
-                DetailLog("{0},SetOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -457,7 +463,7 @@ public sealed class BSPrim : PhysicsActor
         get { return _isPhysical; } 
         set {
             _isPhysical = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setIsPhysical", delegate()
             {
                 SetObjectDynamic();
             });
@@ -478,7 +484,6 @@ public sealed class BSPrim : PhysicsActor
 
     // Make gravity work if the object is physical and not selected
     // No locking here because only called when it is safe
-    // Only called at taint time so it is save to call into Bullet.
     private void SetObjectDynamic()
     {
         // RA: remove this for the moment.
@@ -487,13 +492,16 @@ public sealed class BSPrim : PhysicsActor
         //    Maybe a VerifyCorrectPhysicalShape() routine?
         // RecreateGeomAndObject();
 
-        float mass = _mass;
-        // Bullet wants static objects have a mass of zero
-        if (IsStatic) 
-            mass = 0f;
+        // Bullet wants static objects to have a mass of zero
+        float mass = IsStatic ? 0f : _mass;
 
-        DetailLog("{0},SetObjectDynamic,taint,static={1},solid={2},mass={3}", LocalID, IsStatic, IsSolid, mass);
         BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), mass);
+
+        // recompute any linkset parameters
+        _linkset.Refresh(this);
+
+        CollisionFlags cf = BulletSimAPI.GetCollisionFlags2(Body.Ptr);
+        // DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}, cf={4}", LocalID, IsStatic, IsSolid, mass, cf);
     }
 
     // prims don't fly
@@ -548,9 +556,9 @@ public sealed class BSPrim : PhysicsActor
         set {
             _rotationalVelocity = value;
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                DetailLog("{0},SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                // DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
             });
         } 
@@ -565,9 +573,9 @@ public sealed class BSPrim : PhysicsActor
         get { return _buoyancy; } 
         set {
             _buoyancy = value;
-            _scene.TaintedObject(delegate()
+            _scene.TaintedObject("BSPrim.setBuoyancy", delegate()
             {
-                DetailLog("{0},SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                // DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
             });
         } 
@@ -607,6 +615,7 @@ public sealed class BSPrim : PhysicsActor
 
     private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
+        // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
         {
             // _force += force;
@@ -618,40 +627,48 @@ public sealed class BSPrim : PhysicsActor
             m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
             return;
         }
-        _scene.TaintedObject(delegate()
+        _scene.TaintedObject("BSPrim.AddForce", delegate()
         {
+            OMV.Vector3 fSum = OMV.Vector3.Zero;
             lock (m_accumulatedForces)
             {
-                if (m_accumulatedForces.Count > 0)
+                foreach (OMV.Vector3 v in m_accumulatedForces)
                 {
-                    OMV.Vector3 fSum = OMV.Vector3.Zero;
-                    foreach (OMV.Vector3 v in m_accumulatedForces)
-                    {
-                        fSum += v;
-                    }
-                    m_accumulatedForces.Clear();
-
-                    DetailLog("{0},SetObjectForce,taint,force={1}", LocalID, fSum);
-                    BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, fSum);
+                    fSum += v;
                 }
+                m_accumulatedForces.Clear();
             }
+            // DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, _force);
+            BulletSimAPI.AddObjectForce2(Body.Ptr, fSum);
         });
     }
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { 
-        DetailLog("{0},AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
+        // DetailLog("{0},BSPrim.AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
         // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
     }
     public override void SetMomentum(OMV.Vector3 momentum) { 
-        DetailLog("{0},SetMomentum,call,mom={1}", LocalID, momentum);
+        // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
     }
     public override void SubscribeEvents(int ms) { 
         _subscribedEventsMs = ms;
-        // make sure first collision happens
-        _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs;
+        if (ms > 0)
+        {
+            // make sure first collision happens
+            _nextCollisionOkTime = Util.EnvironmentTickCount() - _subscribedEventsMs;
+
+            Scene.TaintedObject("BSPrim.SubscribeEvents", delegate()
+            {
+                BulletSimAPI.AddToCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+            });
+        }
     }
     public override void UnSubscribeEvents() { 
         _subscribedEventsMs = 0;
+        Scene.TaintedObject("BSPrim.UnSubscribeEvents", delegate()
+        {
+            BulletSimAPI.RemoveFromCollisionFlags2(Body.Ptr, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
+        });
     }
     public override bool SubscribedEvents() { 
         return (_subscribedEventsMs > 0);
@@ -970,26 +987,26 @@ public sealed class BSPrim : PhysicsActor
         {
             if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
             {
-                if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
-                {
+                // if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
+                // {
                     // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
                     if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
                     {
-                        DetailLog("{0},CreateGeom,sphere", LocalID);
+                        // DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
                         _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
                         // Bullet native objects are scaled by the Bullet engine so pass the size in
                         _scale = _size;
                         // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
                         ret = true;
                     }
-                }
+                // }
             }
             else
             {
                 // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, type={2}, size={3}", LogHeader, LocalID, _shapeType, _size);
                 if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX))
                 {
-                    DetailLog("{0},CreateGeom,box", LocalID);
+                    // DetailLog("{0},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                     _scale = _size;
                     // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
@@ -1032,12 +1049,12 @@ public sealed class BSPrim : PhysicsActor
         // if this new shape is the same as last time, don't recreate the mesh
         if (_meshKey == newMeshKey) return;
 
-        DetailLog("{0},CreateGeomMesh,create,key={1}", LocalID, _meshKey);
+        // DetailLog("{0},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
         // Since we're recreating new, get rid of any previously generated shape
         if (_meshKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
-            DetailLog("{0},CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
+            // DetailLog("{0},BSPrim.CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;
             _meshKey = 0;
@@ -1067,7 +1084,7 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
-        DetailLog("{0},CreateGeomMesh,done", LocalID);
+        // DetailLog("{0},BSPrim.CreateGeomMesh,done", LocalID);
         return;
     }
 
@@ -1081,28 +1098,21 @@ public sealed class BSPrim : PhysicsActor
         // if the hull hasn't changed, don't rebuild it
         if (newHullKey == _hullKey) return;
 
-        DetailLog("{0},CreateGeomHull,create,key={1}", LocalID, _meshKey);
+        // DetailLog("{0},BSPrim.CreateGeomHull,create,oldKey={1},newKey={2}", LocalID, _hullKey, newHullKey);
         
         // Since we're recreating new, get rid of any previously generated shape
         if (_hullKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey);
-            DetailLog("{0},CreateGeomHull,deleteOldHull,key={1}", LocalID, _meshKey);
+            // DetailLog("{0},BSPrim.CreateGeomHull,deleteOldHull,key={1}", LocalID, _hullKey);
             BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
             _hullKey = 0;
-            _hulls.Clear();
-            DetailLog("{0},CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
-            BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
-            _mesh = null;   // the mesh cannot match either
-            _meshKey = 0;
         }
 
         _hullKey = newHullKey;
-        if (_meshKey != _hullKey)
-        {
-            // if the underlying mesh has changed, rebuild it
-            CreateGeomMesh();
-        }
+
+        // Make sure the underlying mesh exists and is correct
+        CreateGeomMesh();
 
         int[] indices = _mesh.getIndexListAsInt();
         List<OMV.Vector3> vertices = _mesh.getVertexList();
@@ -1128,7 +1138,7 @@ public sealed class BSPrim : PhysicsActor
         // create the hull into the _hulls variable
         convexBuilder.process(dcomp);
 
-        // Convert the vertices and indices for passing to unmanaged
+        // Convert the vertices and indices for passing to unmanaged.
         // The hull information is passed as a large floating point array. 
         // The format is:
         //  convHulls[0] = number of hulls
@@ -1188,7 +1198,7 @@ public sealed class BSPrim : PhysicsActor
         _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL;
         // meshes are already scaled by the meshmerizer
         _scale = new OMV.Vector3(1f, 1f, 1f);
-        DetailLog("{0},CreateGeomHull,done", LocalID);
+        // DetailLog("{0},BSPrim.CreateGeomHull,done", LocalID);
         return;
     }
 
@@ -1214,7 +1224,7 @@ public sealed class BSPrim : PhysicsActor
         bool ret = BulletSimAPI.CreateObject(_scene.WorldID, shape);
 
         // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
-        m_body.Ptr = BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID);
+        Body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
 
         return ret;
     }
@@ -1326,8 +1336,8 @@ public sealed class BSPrim : PhysicsActor
 
             // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
             //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
-            DetailLog("{0},UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            // DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+            //         LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
 
             base.RequestPhysicsterseUpdate();
         }
@@ -1335,7 +1345,7 @@ public sealed class BSPrim : PhysicsActor
         else
         {
             // For debugging, we also report the movement of children
-            DetailLog("{0},UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+            DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                     entprop.Acceleration, entprop.RotationalVelocity);
         }
@@ -1343,7 +1353,7 @@ public sealed class BSPrim : PhysicsActor
     }
 
     // I've collided with something
-    CollisionEventUpdate collisionCollection = null;
+    CollisionEventUpdate collisionCollection;
     public void Collide(uint collidingWith, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
         // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith);
@@ -1355,6 +1365,8 @@ public sealed class BSPrim : PhysicsActor
             _collidingGroundStep = _scene.SimulationStep;
         }
 
+        // DetailLog("{0},BSPrim.Collison,call,with={1}", LocalID, collidingWith);
+
         // if someone is subscribed to collision events....
         if (_subscribedEventsMs != 0) {
             // throttle the collisions to the number of milliseconds specified in the subscription
@@ -1375,7 +1387,9 @@ public sealed class BSPrim : PhysicsActor
         if (collisionCollection != null && collisionCollection.Count > 0)
         {
             base.SendCollisionUpdate(collisionCollection);
-            collisionCollection.Clear();
+            // The collisionCollection structure is passed around in the simulator.
+            // Make sure we don't have a handle to that one and that a new one is used next time.
+            collisionCollection = null;
         }
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 011033c..a31c578 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -73,7 +73,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
     private static readonly string LogHeader = "[BULLETS SCENE]";
 
-    public void DebugLog(string mm, params Object[] xx) { if (shouldDebugLog) m_log.DebugFormat(mm, xx); }
+    public void DebugLog(string mm, params Object[] xx) { if (ShouldDebugLog) m_log.DebugFormat(mm, xx); }
 
     public string BulletSimVersion = "?";
 
@@ -162,14 +162,24 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     public delegate void TaintCallback();
-    private List<TaintCallback> _taintedObjects;
+    private struct TaintCallbackEntry
+    {
+        public String ident;
+        public TaintCallback callback;
+        public TaintCallbackEntry(string i, TaintCallback c)
+        {
+            ident = i;
+            callback = c;
+        }
+    }
+    private List<TaintCallbackEntry> _taintedObjects;
     private Object _taintLock = new Object();
 
     // A pointer to an instance if this structure is passed to the C++ code
     ConfigurationParameters[] m_params;
     GCHandle m_paramsHandle;
 
-    public bool shouldDebugLog { get; private set; }
+    public bool ShouldDebugLog { get; private set; }
 
     private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
 
@@ -232,7 +242,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             BulletSimAPI.SetDebugLogCallback(m_DebugLogCallbackHandle);
         }
 
-        _taintedObjects = new List<TaintCallback>();
+        _taintedObjects = new List<TaintCallbackEntry>();
 
         mesher = meshmerizer;
         // The bounding box for the simulated world
@@ -245,7 +255,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // Initialization to support the transition to a new API which puts most of the logic
         //   into the C# code so it is easier to modify and add to.
-        m_worldSim = new BulletSim(m_worldID, BulletSimAPI.GetSimHandle2(m_worldID));
+        m_worldSim = new BulletSim(m_worldID, this, BulletSimAPI.GetSimHandle2(m_worldID));
         m_constraintCollection = new BSConstraintCollection(World);
 
         m_initialized = true;
@@ -352,7 +362,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPrim bsprim = prim as BSPrim;
         if (bsprim != null)
         {
-            m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
+            // DetailLog("{0},RemovePrim,call", bsprim.LocalID);
+            // m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
             try
             {
                 lock (m_prims) m_prims.Remove(bsprim.LocalID);
@@ -377,6 +388,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         if (!m_initialized) return null;
 
+        // DetailLog("{0},AddPrimShape,call", localID);
+
         BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
         lock (m_prims) m_prims.Add(localID, prim);
         return prim;
@@ -400,7 +413,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // prevent simulation until we've been initialized
         if (!m_initialized) return 10.0f;
 
-        long simulateStartTime = Util.EnvironmentTickCount();
+        int simulateStartTime = Util.EnvironmentTickCount();
 
         // update the prim states while we know the physics engine is not busy
         ProcessTaints();
@@ -416,12 +429,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         {
             numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep,
                         out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
-            DetailLog("{0},Simulate,call, substeps={1}, updates={2}, colliders={3}", "0000000000", numSubSteps, updatedEntityCount, collidersCount); 
+            // DetailLog("{0},Simulate,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount); 
         }
         catch (Exception e)
         {
             m_log.WarnFormat("{0},PhysicsStep Exception: substeps={1}, updates={2}, colliders={3}, e={4}", LogHeader, numSubSteps, updatedEntityCount, collidersCount, e);
-            DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", "0000000000", numSubSteps, updatedEntityCount, collidersCount);
+            // DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount);
             // updatedEntityCount = 0;
             collidersCount = 0;
         }
@@ -498,8 +511,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // long simulateTotalTime = Util.EnvironmentTickCountSubtract(simulateStartTime);
         // return (timeStep * (float)simulateTotalTime);
 
-        // TODO: FIX THIS: fps calculation wrong. This calculation always returns about 1 in normal operation.
-        return timeStep / (numSubSteps * m_fixedTimeStep) * 1000f;
+        // TODO: FIX THIS: fps calculation possibly wrong.
+        // This calculation says 1/timeStep is the ideal frame rate. Any time added to
+        //    that by the physics simulation gives a slower frame rate.
+        long totalSimulationTime = Util.EnvironmentTickCountSubtract(simulateStartTime);
+        if (totalSimulationTime >= timeStep)
+            return 0;
+        return 1f / (timeStep + totalSimulationTime);
     }
 
     // Something has collided
@@ -535,7 +553,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public override void SetTerrain(float[] heightMap) {
         m_heightMap = heightMap;
-        this.TaintedObject(delegate()
+        this.TaintedObject("BSScene.SetTerrain", delegate()
         {
             BulletSimAPI.SetHeightmap(m_worldID, m_heightMap);
         });
@@ -577,12 +595,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         // make sure no stepping happens while we're deleting stuff
         m_initialized = false;
 
-        if (m_constraintCollection != null)
-        {
-            m_constraintCollection.Dispose();
-            m_constraintCollection = null;
-        }
-
         foreach (KeyValuePair<uint, BSCharacter> kvp in m_avatars)
         {
             kvp.Value.Destroy();
@@ -595,6 +607,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         }
         m_prims.Clear();
 
+        // Now that the prims are all cleaned up, there should be no constraints left
+        if (m_constraintCollection != null)
+        {
+            m_constraintCollection.Dispose();
+            m_constraintCollection = null;
+        }
+
         // Anything left in the unmanaged code should be cleaned out
         BulletSimAPI.Shutdown(WorldID);
 
@@ -727,12 +746,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // Calls to the PhysicsActors can't directly call into the physics engine
     // because it might be busy. We delay changes to a known time.
     // We rely on C#'s closure to save and restore the context for the delegate.
-    public void TaintedObject(TaintCallback callback)
+    public void TaintedObject(String ident, TaintCallback callback)
     {
         if (!m_initialized) return;
 
         lock (_taintLock)
-            _taintedObjects.Add(callback);
+            _taintedObjects.Add(new TaintCallbackEntry(ident, callback));
         return;
     }
 
@@ -744,22 +763,22 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (_taintedObjects.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
-            List<TaintCallback> oldList;
+            List<TaintCallbackEntry> oldList;
             lock (_taintLock)
             {
                 oldList = _taintedObjects;
-                _taintedObjects = new List<TaintCallback>();
+                _taintedObjects = new List<TaintCallbackEntry>();
             }
 
-            foreach (TaintCallback callback in oldList)
+            foreach (TaintCallbackEntry tcbe in oldList)
             {
                 try
                 {
-                    callback();
+                    tcbe.callback();
                 }
                 catch (Exception e)
                 {
-                    m_log.ErrorFormat("{0}: ProcessTaints: Exception: {1}", LogHeader, e);
+                    m_log.ErrorFormat("{0}: ProcessTaints: {1}: Exception: {2}", LogHeader, tcbe.ident, e);
                 }
             }
             oldList.Clear();
@@ -767,6 +786,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     }
 
     #region Vehicles
+
+    public void VehicleInSceneTypeChanged(BSPrim vehic, Vehicle newType)
+    {
+        if (newType == Vehicle.TYPE_NONE)
+        {
+            RemoveVehiclePrim(vehic);
+        }
+        else
+        {
+            // make it so the scene will call us each tick to do vehicle things
+           AddVehiclePrim(vehic);
+        }
+    }
+
     // Make so the scene will call this prim for vehicle actions each tick.
     // Safe to call if prim is already in the vehicle list.
     public void AddVehiclePrim(BSPrim vehicle)
@@ -812,12 +845,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     private struct ParameterDefn
     {
-        public string name;
-        public string desc;
-        public float defaultValue;
-        public ParamUser userParam;
-        public ParamGet getter;
-        public ParamSet setter;
+        public string name;         // string name of the parameter
+        public string desc;         // a short description of what the parameter means
+        public float defaultValue;  // default value if not specified anywhere else
+        public ParamUser userParam; // get the value from the configuration file
+        public ParamGet getter;     // return the current value stored for this parameter
+        public ParamSet setter;     // set the current value for this parameter
         public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s)
         {
             name = n;
@@ -834,7 +867,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // To add a new externally referencable/settable parameter, add the paramter storage
     //    location somewhere in the program and make an entry in this table with the
     //    getters and setters.
-    // To add a new variable, it is easiest to find an existing definition and copy it.
+    // It is easiest to find an existing definition and copy it.
     // Parameter values are floats. Booleans are converted to a floating value.
     // 
     // A ParameterDefn() takes the following parameters:
@@ -870,7 +903,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return (float)s.m_meshLOD; },
             (s,p,l,v) => { s.m_meshLOD = (int)v; } ),
         new ParameterDefn("SculptLOD", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
-            32,
+            32f,
             (s,cf,p,v) => { s.m_sculptLOD = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_sculptLOD; },
             (s,p,l,v) => { s.m_sculptLOD = (int)v; } ),
@@ -1027,14 +1060,19 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
 
 
-            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default)",
+            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)",
             0f,     // zero to disable
             (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].maxPersistantManifoldPoolSize; },
             (s,p,l,v) => { s.m_params[0].maxPersistantManifoldPoolSize = v; } ),
+            new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)",
+            0f,     // zero to disable
+            (s,cf,p,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].maxCollisionAlgorithmPoolSize; },
+            (s,p,l,v) => { s.m_params[0].maxCollisionAlgorithmPoolSize = v; } ),
             new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
-            ConfigurationParameters.numericTrue,
-            (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].shouldDisableContactPoolDynamicAllocation = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
             (s) => { return s.m_params[0].shouldDisableContactPoolDynamicAllocation; },
             (s,p,l,v) => { s.m_params[0].shouldDisableContactPoolDynamicAllocation = v; } ),
             new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
@@ -1101,9 +1139,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,p,l,v) => { s.m_detailedStatsStep = (int)v; } ),
         new ParameterDefn("ShouldDebugLog", "Enables detailed DEBUG log statements",
             ConfigurationParameters.numericFalse,
-            (s,cf,p,v) => { s.shouldDebugLog = cf.GetBoolean(p, s.BoolNumeric(v)); },
-            (s) => { return s.NumericBool(s.shouldDebugLog); },
-            (s,p,l,v) => { s.shouldDebugLog = s.BoolNumeric(v); } ),
+            (s,cf,p,v) => { s.ShouldDebugLog = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldDebugLog); },
+            (s,p,l,v) => { s.ShouldDebugLog = s.BoolNumeric(v); } ),
 
     };
 
@@ -1243,7 +1281,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 List<uint> objectIDs = lIDs;
                 string xparm = parm.ToLower();
                 float xval = val;
-                TaintedObject(delegate() {
+                TaintedObject("BSScene.UpdateParameterSet", delegate() {
                     foreach (uint lID in objectIDs)
                     {
                         BulletSimAPI.UpdateParameter(m_worldID, lID, xparm, xval);
@@ -1263,7 +1301,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         uint xlocalID = localID;
         string xparm = parm.ToLower();
         float xval = val;
-        TaintedObject(delegate() {
+        TaintedObject("BSScene.TaintedUpdateParameter", delegate() {
             BulletSimAPI.UpdateParameter(m_worldID, xlocalID, xparm, xval);
         });
     }
@@ -1289,10 +1327,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     #endregion Runtime settable parameters
 
     // Invoke the detailed logger and output something if it's enabled.
-    private void DetailLog(string msg, params Object[] args)
+    public void DetailLog(string msg, params Object[] args)
     {
         PhysicsLogging.Write(msg, args);
     }
+    // used to fill in the LocalID when there isn't one
+    public const string DetailLogZero = "0000000000";
 
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 0ffbc94..504bd3c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -35,9 +35,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin {
 // Classes to allow some type checking for the API
 public struct BulletSim
 {
-    public BulletSim(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
-    public IntPtr Ptr;
+    public BulletSim(uint id, BSScene bss, IntPtr xx) { ID = id; scene = bss;  Ptr = xx; }
     public uint ID;
+    // The scene is only in here so very low level routines have a handle to print debug/error messages
+    public BSScene scene;
+    public IntPtr Ptr;
 }
 
 public struct BulletBody
@@ -158,6 +160,7 @@ public struct ConfigurationParameters
         public float avatarContactProcessingThreshold;
 
         public float maxPersistantManifoldPoolSize;
+        public float maxCollisionAlgorithmPoolSize;
         public float shouldDisableContactPoolDynamicAllocation;
         public float shouldForceUpdateAllAabbs;
         public float shouldRandomizeSolverOrder;
@@ -179,17 +182,18 @@ public struct ConfigurationParameters
 // Values used by Bullet and BulletSim to control collisions
 public enum CollisionFlags : uint
 {
-    STATIC_OBJECT                 = 1 << 0,
-    KINEMATIC_OBJECT              = 1 << 1,
-    NO_CONTACT_RESPONSE           = 1 << 2,
-    CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
-    CHARACTER_OBJECT              = 1 << 4,
-    DISABLE_VISUALIZE_OBJECT      = 1 << 5,
-    DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
+    CF_STATIC_OBJECT                 = 1 << 0,
+    CF_KINEMATIC_OBJECT              = 1 << 1,
+    CF_NO_CONTACT_RESPONSE           = 1 << 2,
+    CF_CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
+    CF_CHARACTER_OBJECT              = 1 << 4,
+    CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
+    CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
     // Following used by BulletSim to control collisions
-    VOLUME_DETECT_OBJECT          = 1 << 10,
-    PHANTOM_OBJECT                = 1 << 11,
-    PHYSICAL_OBJECT               = 1 << 12,
+    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
+    BS_VOLUME_DETECT_OBJECT          = 1 << 11,
+    BS_PHANTOM_OBJECT                = 1 << 12,
+    BS_PHYSICAL_OBJECT               = 1 << 13,
 };
 
 // CFM controls the 'hardness' of the constraint. 0=fixed, 0..1=violatable. Default=0
@@ -361,7 +365,7 @@ public static extern IntPtr GetSimHandle2(uint worldID);
 public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr GetBodyHandle2(IntPtr sim, uint id);
+public static extern IntPtr GetBodyHandle2(IntPtr world, uint id);
 
 // ===============================================================================
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -370,45 +374,69 @@ public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
                                                                                         int maxUpdates, IntPtr updateArray);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateParameter2(IntPtr sim, uint localID, String parm, float value);
+public static extern bool UpdateParameter2(IntPtr world, uint localID, String parm, float value);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern void SetHeightmap2(IntPtr sim, float[] heightmap);
+public static extern void SetHeightmap2(IntPtr world, float[] heightmap);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void Shutdown2(IntPtr sim);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern int PhysicsStep2(IntPtr sim, float timeStep, int maxSubSteps, float fixedTimeStep,
+public static extern int PhysicsStep2(IntPtr world, 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 PushUpdate2(IntPtr obj);
+
 /*
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateMesh2(IntPtr sim, int indicesCount, int* indices, int verticesCount, float* vertices );
+public static extern IntPtr CreateMesh2(IntPtr world, int indicesCount, int* indices, int verticesCount, float* vertices );
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool BuildHull2(IntPtr sim, IntPtr mesh);
+public static extern bool BuildHull2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool ReleaseHull2(IntPtr sim, IntPtr mesh);
+public static extern bool ReleaseHull2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyMesh2(IntPtr sim, IntPtr mesh);
+public static extern bool DestroyMesh2(IntPtr world, IntPtr mesh);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateObject2(IntPtr sim, ShapeData shapeData);
+public static extern IntPtr CreateObject2(IntPtr world, ShapeData shapeData);
 */
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr CreateConstraint2(IntPtr sim, IntPtr obj1, IntPtr obj2,
+public static extern IntPtr Create6DofConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
                     Vector3 frame1loc, Quaternion frame1rot,
                     Vector3 frame2loc, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Create6DofConstraintToPoint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 joinPoint,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateHingeConstraint2(IntPtr world, IntPtr obj1, IntPtr obj2,
+                    Vector3 pivotinA, Vector3 pivotinB,
+                    Vector3 axisInA, Vector3 axisInB,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetConstraintEnable2(IntPtr constrain, float numericTrueFalse);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetConstraintNumSolverIterations2(IntPtr constrain, float iterations);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetFrames2(IntPtr constrain, 
+                Vector3 frameA, Quaternion frameArot, Vector3 frameB, Quaternion frameBrot);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -421,13 +449,22 @@ public static extern bool UseFrameOffset2(IntPtr constrain, float enable);
 public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enable, float targetVel, float maxMotorForce);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetBreakingImpulseThreshold2(IntPtr constrain, float threshold);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool CalculateTransforms2(IntPtr constrain);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetConstraintParam2(IntPtr constrain, ConstraintParams paramIndex, float value, ConstraintParamAxis axis);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool DestroyConstraint2(IntPtr sim, IntPtr constrain);
+public static extern bool DestroyConstraint2(IntPtr world, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 AddObjectToWorld2(IntPtr world, IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetPosition2(IntPtr obj);
@@ -448,6 +485,9 @@ public static extern bool SetAngularVelocity2(IntPtr obj, Vector3 angularVelocit
 public static extern bool SetObjectForce2(IntPtr obj, Vector3 force);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddObjectForce2(IntPtr obj, Vector3 force);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetCcdMotionThreshold2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -478,13 +518,16 @@ public static extern bool SetLinearVelocity2(IntPtr obj, Vector3 val);
 public static extern bool SetInterpolation2(IntPtr obj, Vector3 lin, Vector3 ang);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr SetCollisionFlags2(IntPtr obj, uint flags);
+public static extern CollisionFlags GetCollisionFlags2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr AddToCollisionFlags2(IntPtr obj, uint flags);
+public static extern IntPtr SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, uint flags);
+public static extern IntPtr AddToCollisionFlags2(IntPtr obj, CollisionFlags flags);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
@@ -499,16 +542,13 @@ public static extern bool SetGravity2(IntPtr obj, Vector3 val);
 public static extern IntPtr ClearForces2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool SetMargin2(IntPtr obj, float val);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
+public static extern IntPtr ClearAllForces2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
+public static extern bool SetMargin2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
+public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyObject2(IntPtr world, uint id);