8 files changed, 501 insertions, 326 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
new file mode 100755
index 0000000..72df6b9
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
@@ -0,0 +1,80 @@
+/*
+ * 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 )
+    {
+        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,
+                                true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
+        m_enabled = true;
+    }
+
+    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)
+    {
+        bool ret = false;
+        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 09e1f0c..f164afe 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},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},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},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},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},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,17 @@ 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
@@ -480,5 +539,10 @@ public class BSCharacter : PhysicsActor
         // 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..da26b72 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,34 +75,7 @@ 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)
-    {
-        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 CalculateTransforms()
+    public virtual bool CalculateTransforms()
     {
         bool ret = false;
         if (m_enabled)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
index c88e645..3df2ddc 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -63,21 +63,13 @@ public class BSConstraintCollection : IDisposable
         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);
 
+        m_world.scene.DetailLog("{0},BSConstraintCollection.AddConstraint,call,body1={1},body2={2}", BSScene.DetailLogZero, cons.Body1.ID, cons.Body2.ID);
+
         m_constraints.Add(cons);
 
         return true;
@@ -118,6 +110,7 @@ public class BSConstraintCollection : IDisposable
 
         if (this.TryGetConstraint(body1, body2, out constrain))
         {
+            m_world.scene.DetailLog("{0},BSConstraintCollection.RemoveAndDestroyConstraint,taint,body1={1},body2={2}", BSScene.DetailLogZero, body1.ID, body2.ID);
             // remove the constraint from our collection
             m_constraints.Remove(constrain);
             // tell the engine that all its structures need to be freed
@@ -158,10 +151,11 @@ public class BSConstraintCollection : IDisposable
 
     public bool RecalculateAllConstraints()
     {
-        foreach (BSConstraint constrain in m_constraints)
+        ForEachConstraint(delegate(BSConstraint constrain)
         {
             constrain.CalculateTransforms();
-        }
+            return false;
+        });
         return true;
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 6f8430c..4a71612 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -40,6 +40,7 @@ public class BSLinkset
     public BSPrim Root { get { return m_linksetRoot; } }
 
     private BSScene m_scene;
+    public BSScene Scene { get { return m_scene; } }
 
     private List<BSPrim> m_children;
 
@@ -80,14 +81,14 @@ 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;
     }
 
     public BSLinkset RemoveMeFromLinkset(BSPrim child)
@@ -101,7 +102,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,9 +114,10 @@ public class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(m_scene, child);
+        return new BSLinkset(Scene, child);
     }
 
+    /* DEPRECATED: this is really bad in that it trys to unlink other prims.
     // 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)
@@ -124,6 +126,7 @@ public class BSLinkset
 
         lock (m_linksetActivityLock)
         {
+            // The body pointer is refetched in case anything has moved.
             System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID);
             if (aPtr == System.IntPtr.Zero)
             {
@@ -155,13 +158,14 @@ public class BSLinkset
                 }
                 foreach (BSPrim bsp in toRemove)
                 {
-                    RemoveChildFromLinkset(bsp);
+                    RemoveChildFromOtherLinkset(bsp);
                 }
             }
         }
 
         return ret;
     }
+     */
 
 
     // Return 'true' if the passed object is the root object of this linkset
@@ -170,6 +174,8 @@ public class BSLinkset
         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); } }
 
@@ -208,7 +214,8 @@ public class BSLinkset
             com += bp.Position * bp.MassRaw;
             totalMass += bp.MassRaw;
         }
-        com /= totalMass;
+        if (totalMass != 0f)
+            com /= totalMass;
 
         return com;
     }
@@ -221,51 +228,54 @@ public class BSLinkset
         {
             com += bp.Position * bp.MassRaw;
         }
-        com /= m_children.Count + 1;
+        com /= (m_children.Count + 1);
 
         return com;
     }
 
     // I am the root of a linkset and a new child is being added
-    public void AddChildToLinkset(BSPrim pchild)
+    // Called while LinkActivity is locked.
+    public void AddChildToLinkset(BSPrim child)
     {
-        BSPrim child = pchild;
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            m_scene.TaintedObject(delegate()
+            BSPrim root = Root; // capture the root as of now
+            m_scene.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
+                DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                PhysicallyLinkAChildToRoot(root, child);     // 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).
+    public void RemoveChildFromOtherLinkset(BSPrim pchild)
+    {
+        pchild.Linkset = new BSLinkset(m_scene, 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)
+    public void RemoveChildFromLinkset(BSPrim child)
     {
-        BSPrim child = pchild;
-
         if (m_children.Remove(child))
         {
-            m_scene.TaintedObject(delegate()
+            BSPrim root = Root; // capture the root as of now
+            m_scene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
                 DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
-                DetailLog("{0},RemoveChildFromLinkset,child={1}", m_linksetRoot.LocalID, pchild.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(root, child);
             });
         }
         else
@@ -278,14 +288,14 @@ public class BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPrim 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
-        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;
@@ -293,16 +303,17 @@ public class BSLinkset
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
         // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
-        DetailLog("{0},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,
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Identity,
+        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        m_scene.World, rootPrim.Body, childPrim.Body,
+                        childRelativePosition,
+                        childRelativeRotation,
                         OMV.Vector3.Zero,
-                        OMV.Quaternion.Identity
+                        -childRelativeRotation
                         );
+        m_scene.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);
 
@@ -317,29 +328,32 @@ public class BSLinkset
 
     // 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);
+
+        m_scene.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_scene.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_scene.ShouldDebugLog)
+            m_scene.Logger.DebugFormat(msg, args);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index ebfd85b..05cc822 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.Root;
+        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.Root.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.Root;
+            int childrenBefore = _linkset.NumberOfChildren;
+
+            _linkset = parent.Linkset.AddMeToLinkset(this);
 
-        _linkset = _linkset.AddMeToLinkset(this, parent);
+            DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}", 
+                LocalID, parentBefore.LocalID, childrenBefore, _linkset.Root.LocalID, _linkset.NumberOfChildren);
+        }
         return; 
     }
 
@@ -237,9 +250,14 @@ public sealed class BSPrim : PhysicsActor
         // 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.RemoveMeFromLinkset(this);
+        BSPrim parentBefore = _linkset.Root;
+        int childrenBefore = _linkset.NumberOfChildren;
+        
+        _linkset = _linkset.RemoveMeFromLinkset(this);
+
+        DetailLog("{0},BSPrim.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ", 
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.Root.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,13 @@ 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);
+
+        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 +553,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 +570,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 +612,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 +624,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 +984,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 +1046,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 +1081,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,17 +1095,17 @@ 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,key={1}", LocalID, _meshKey);
         
         // 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, _meshKey);
             BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
             _hullKey = 0;
             _hulls.Clear();
-            DetailLog("{0},CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
+            DetailLog("{0},BSPrim.CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;   // the mesh cannot match either
             _meshKey = 0;
@@ -1188,7 +1202,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 +1228,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,20 +1340,18 @@ 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}",
+            DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
 
             base.RequestPhysicsterseUpdate();
         }
-            /*
         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);
         }
-             */
     }
 
     // I've collided with something
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 011033c..beaea1f 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;
@@ -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;
         }
@@ -535,7 +548,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);
         });
@@ -727,12 +740,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 +757,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 +780,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 +839,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 +861,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 +897,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 +1054,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 +1133,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 +1275,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 +1295,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 +1321,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..6800b96 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,40 +374,43 @@ 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);
@@ -427,7 +434,13 @@ public static extern bool CalculateTransforms2(IntPtr constrain);
 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 +461,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 +494,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 SetCollisionFlags2(IntPtr obj, CollisionFlags flags);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr AddToCollisionFlags2(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, uint flags);
+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);
@@ -505,12 +524,6 @@ public static extern bool SetMargin2(IntPtr obj, float val);
 public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool DestroyObject2(IntPtr world, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]