Merge branch 'master' of melanie@opensimulator.org:/var/git/opensim

12 files changed, 439 insertions, 217 deletions

diff --git a/OpenSim/Region/Application/OpenSimBase.cs b/OpenSim/Region/Application/OpenSimBase.cs
index 37cfe1d..f784398 100644
--- a/OpenSim/Region/Application/OpenSimBase.cs
+++ b/OpenSim/Region/Application/OpenSimBase.cs
@@ -305,8 +305,13 @@ namespace OpenSim
             m_httpServerPort = m_networkServersInfo.HttpListenerPort;
             SceneManager.OnRestartSim += handleRestartRegion;
 
-            // Only start the memory watchdog once all regions are ready
-            SceneManager.OnRegionsReadyStatusChange += sm => MemoryWatchdog.Enabled = sm.AllRegionsReady;
+            // Only enable the watchdogs when all regions are ready.  Otherwise we get false positives when cpu is
+            // heavily used during initial startup.
+            //
+            // FIXME: It's also possible that region ready status should be flipped during an OAR load since this
+            // also makes heavy use of the CPU.
+            SceneManager.OnRegionsReadyStatusChange
+                += sm => { MemoryWatchdog.Enabled = sm.AllRegionsReady; Watchdog.Enabled = sm.AllRegionsReady; };
         }
 
         /// <summary>
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
index 72df6b9..683bc51 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BS6DofConstraint.cs
@@ -37,7 +37,8 @@ public class BS6DofConstraint : BSConstraint
     // Create a btGeneric6DofConstraint
     public BS6DofConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
                     Vector3 frame1, Quaternion frame1rot,
-                    Vector3 frame2, Quaternion frame2rot )
+                    Vector3 frame2, Quaternion frame2rot,
+                    bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     {
         m_world = world;
         m_body1 = obj1;
@@ -46,16 +47,45 @@ public class BS6DofConstraint : BSConstraint
                             BulletSimAPI.Create6DofConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
                                 frame1, frame1rot,
                                 frame2, frame2rot,
-                                true /*useLinearReferenceFrameA*/, true /*disableCollisionsBetweenLinkedBodies*/));
+                                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 = 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);
+        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;
     }
 
@@ -76,5 +106,13 @@ public class BS6DofConstraint : BSConstraint
             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 f164afe..e2f7af9 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -137,7 +137,7 @@ public class BSCharacter : PhysicsActor
     // called when this character is being destroyed and the resources should be released
     public void Destroy()
     {
-        // DetailLog("{0},Destroy", LocalID);
+        // DetailLog("{0},BSCharacter.Destroy", LocalID);
         _scene.TaintedObject("BSCharacter.destroy", delegate()
         {
             BulletSimAPI.DestroyObject(_scene.WorldID, _localID);
@@ -209,7 +209,7 @@ public class BSCharacter : PhysicsActor
 
             _scene.TaintedObject("BSCharacter.setPosition", delegate()
             {
-                DetailLog("{0},SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -226,7 +226,7 @@ public class BSCharacter : PhysicsActor
         float terrainHeight = Scene.GetTerrainHeightAtXYZ(_position);
         if (_position.Z < terrainHeight)
         {
-            DetailLog("{0},PositionAdjustUnderGround,call,pos={1},orient={2}", LocalID, _position, _orientation);
+            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},orient={2}", LocalID, _position, _orientation);
             _position.Z = terrainHeight + 2.0f;
             ret = true;
         }
@@ -368,7 +368,7 @@ public class BSCharacter : PhysicsActor
         set { _buoyancy = value; 
             _scene.TaintedObject("BSCharacter.setBuoyancy", delegate()
             {
-                DetailLog("{0},setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                DetailLog("{0},BSCharacter.setBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _buoyancy);
             });
         } 
@@ -415,7 +415,7 @@ public class BSCharacter : PhysicsActor
             // m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
             _scene.TaintedObject("BSCharacter.AddForce", delegate()
             {
-                DetailLog("{0},setAddForce,taint,addedForce={1}", LocalID, _force);
+                DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
                 BulletSimAPI.AddObjectForce2(Body.Ptr, _force);
             });
         }
@@ -488,9 +488,11 @@ public class BSCharacter : PhysicsActor
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
+        /*
         DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                 LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                 entprop.Acceleration, entprop.RotationalVelocity);
+         */
     }
 
     // Called by the scene when a collision with this object is reported
@@ -507,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) {
@@ -535,7 +538,10 @@ 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
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index da26b72..25084d8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -80,10 +80,33 @@ public abstract class BSConstraint : IDisposable
         bool ret = false;
         if (m_enabled)
         {
+            // Recompute the internal transforms
             BulletSimAPI.CalculateTransforms2(m_constraint.Ptr);
             ret = true;
         }
         return ret;
     }
+
+    // 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)
+        {
+            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 3df2ddc..22ea367 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -56,21 +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 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);
+        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;
     }
@@ -84,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;
     }
@@ -103,23 +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;
+            }
+        }
+
+        return ret;
+    }
 
-        if (this.TryGetConstraint(body1, body2, out constrain))
+    // The constraint MUST exist in the collection
+    public bool RemoveAndDestroyConstraint(BSConstraint constrain)
+    {
+        lock (m_constraints)
         {
-            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
-            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.
@@ -130,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);
@@ -151,28 +167,16 @@ public class BSConstraintCollection : IDisposable
 
     public bool RecalculateAllConstraints()
     {
-        ForEachConstraint(delegate(BSConstraint constrain)
-        {
-            constrain.CalculateTransforms();
-            return false;
-        });
-        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 4a71612..087b9bb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -37,11 +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;
-    public BSScene Scene { get { return 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.
@@ -73,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;
@@ -91,6 +92,9 @@ public class BSLinkset
         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)
@@ -114,60 +118,9 @@ public class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(Scene, child);
+        return new BSLinkset(PhysicsScene, 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)
-    {
-        BSLinkset ret = requestor.Linkset;
-
-        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)
-            {
-                // 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)
-                {
-                    RemoveChildFromOtherLinkset(bsp);
-                }
-            }
-        }
-
-        return ret;
-    }
-     */
-
-
     // Return 'true' if the passed object is the root object of this linkset
     public bool IsRoot(BSPrim requestor)
     {
@@ -183,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;
@@ -209,13 +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;
         }
-        if (totalMass != 0f)
-            com /= totalMass;
 
         return com;
     }
@@ -224,29 +183,84 @@ 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
     // Called while LinkActivity is locked.
-    public void AddChildToLinkset(BSPrim child)
+    private void AddChildToLinkset(BSPrim child)
     {
         if (!HasChild(child))
         {
             m_children.Add(child);
 
-            BSPrim root = Root; // capture the root as of now
-            m_scene.TaintedObject("AddChildToLinkset", 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,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
-                PhysicallyLinkAChildToRoot(root, child);     // 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;
@@ -257,31 +271,34 @@ public class BSLinkset
     //    it's still connected to the linkset.
     // Normal OpenSimulator operation will never do this because other SceneObjectPart information
     //    has to be updated also (like pointer to prim's parent).
-    public void RemoveChildFromOtherLinkset(BSPrim pchild)
+    private void RemoveChildFromOtherLinkset(BSPrim pchild)
     {
-        pchild.Linkset = new BSLinkset(m_scene, 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 child)
+    private void RemoveChildFromLinkset(BSPrim child)
     {
         if (m_children.Remove(child))
         {
-            BSPrim root = Root; // capture the root as of now
-            m_scene.TaintedObject("RemoveChildFromLinkset", 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,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
 
-                PhysicallyUnlinkAChildFromRoot(root, child);
+                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;
     }
@@ -293,37 +310,72 @@ public class BSLinkset
         // 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(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},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,
+                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
                         OMV.Vector3.Zero,
-                        -childRelativeRotation
+                        OMV.Quaternion.Inverse(rootPrim.Orientation),
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(childPrim.Orientation),
+                        // A point half way between the parent and child
+                        // childRelativePosition/2,
+                        // childRelativeRotation,
+                        // childRelativePosition/2,
+                        // inverseChildRelativeRotation,
+                        true,
+                        true
                         );
-        m_scene.Constraints.AddConstraint(constrain);
+        // ==================================================================================
+        */
+
+        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
@@ -334,7 +386,9 @@ public class BSLinkset
         //             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);
+        // 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);
     }
@@ -346,20 +400,20 @@ public class BSLinkset
         // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
         DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
 
-        m_scene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
     private void DebugLog(string msg, params Object[] args)
     {
-        if (m_scene.ShouldDebugLog)
-            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 05cc822..9c20004 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -163,13 +163,13 @@ public sealed class BSPrim : PhysicsActor
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
 
         // Undo any links between me and any other object
-        BSPrim parentBefore = _linkset.Root;
+        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.Root.LocalID, _linkset.NumberOfChildren);
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
 
         // Undo any vehicle properties
         this.VehicleType = (int)Vehicle.TYPE_NONE;
@@ -233,13 +233,13 @@ public sealed class BSPrim : PhysicsActor
         if (parent != null)
         {
             DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, parent.LocalID);
-            BSPrim parentBefore = _linkset.Root;
+            BSPrim parentBefore = _linkset.LinksetRoot;
             int childrenBefore = _linkset.NumberOfChildren;
 
             _linkset = parent.Linkset.AddMeToLinkset(this);
 
             DetailLog("{0},BSPrim.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}", 
-                LocalID, parentBefore.LocalID, childrenBefore, _linkset.Root.LocalID, _linkset.NumberOfChildren);
+                LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
         }
         return; 
     }
@@ -249,15 +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());
+                            _linkset.LinksetRoot._avName+"/"+_linkset.LinksetRoot.LocalID.ToString());
 
-        BSPrim parentBefore = _linkset.Root;
+        BSPrim parentBefore = _linkset.LinksetRoot;
         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);
+            LocalID, parentBefore.LocalID, childrenBefore, _linkset.LinksetRoot.LocalID, _linkset.NumberOfChildren);
         return; 
     }
 
@@ -280,7 +280,7 @@ public sealed class BSPrim : PhysicsActor
 
     public override void LockAngularMotion(OMV.Vector3 axis)
     { 
-        DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
+        // DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
         return;
     }
 
@@ -299,7 +299,7 @@ public sealed class BSPrim : PhysicsActor
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             _scene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -336,7 +336,7 @@ public sealed class BSPrim : PhysicsActor
             _force = value;
             _scene.TaintedObject("BSPrim.setForce", delegate()
             {
-                DetailLog("{0},BSPrim.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);
             });
@@ -414,7 +414,7 @@ public sealed class BSPrim : PhysicsActor
             _velocity = value;
             _scene.TaintedObject("BSPrim.setVelocity", delegate()
             {
-                DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
+                // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, _velocity);
                 BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
             });
         } 
@@ -422,7 +422,7 @@ public sealed class BSPrim : PhysicsActor
     public override OMV.Vector3 Torque { 
         get { return _torque; } 
         set { _torque = value; 
-            DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
+            // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         } 
     }
     public override float CollisionScore { 
@@ -449,7 +449,7 @@ public sealed class BSPrim : PhysicsActor
             _scene.TaintedObject("BSPrim.setOrientation", delegate()
             {
                 // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
-                DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
             });
         } 
@@ -497,8 +497,11 @@ public sealed class BSPrim : PhysicsActor
 
         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);
+        // DetailLog("{0},BSPrim.SetObjectDynamic,taint,static={1},solid={2},mass={3}, cf={4}", LocalID, IsStatic, IsSolid, mass, cf);
     }
 
     // prims don't fly
@@ -555,7 +558,7 @@ public sealed class BSPrim : PhysicsActor
             // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
             _scene.TaintedObject("BSPrim.setRotationalVelocity", delegate()
             {
-                DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
+                // DetailLog("{0},BSPrim.SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                 BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
             });
         } 
@@ -572,7 +575,7 @@ public sealed class BSPrim : PhysicsActor
             _buoyancy = value;
             _scene.TaintedObject("BSPrim.setBuoyancy", delegate()
             {
-                DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                // DetailLog("{0},BSPrim.SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
                 BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
             });
         } 
@@ -635,17 +638,17 @@ public sealed class BSPrim : PhysicsActor
                 }
                 m_accumulatedForces.Clear();
             }
-            DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, _force);
+            // 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},BSPrim.AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
+        // DetailLog("{0},BSPrim.AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
         // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
     }
     public override void SetMomentum(OMV.Vector3 momentum) { 
-        DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
+        // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum);
     }
     public override void SubscribeEvents(int ms) { 
         _subscribedEventsMs = ms;
@@ -989,7 +992,7 @@ public sealed class BSPrim : PhysicsActor
                     // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
                     if (forceRebuild || (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE))
                     {
-                        DetailLog("{0},BSPrim.CreateGeom,sphere (force={1}", LocalID, forceRebuild);
+                        // 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;
@@ -1003,7 +1006,7 @@ public sealed class BSPrim : PhysicsActor
                 // 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},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
+                    // DetailLog("{0},BSPrim.CreateGeom,box (force={1})", LocalID, forceRebuild);
                     _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                     _scale = _size;
                     // TODO: do we need to check for and destroy a mesh or hull that might have been left from before?
@@ -1046,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},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
+        // DetailLog("{0},BSPrim.CreateGeomMesh,create,key={1}", LocalID, newMeshKey);
         // Since we're recreating new, get rid of any previously generated shape
         if (_meshKey != 0)
         {
             // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
-            DetailLog("{0},BSPrim.CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
+            // DetailLog("{0},BSPrim.CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
             BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
             _mesh = null;
             _meshKey = 0;
@@ -1081,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},BSPrim.CreateGeomMesh,done", LocalID);
+        // DetailLog("{0},BSPrim.CreateGeomMesh,done", LocalID);
         return;
     }
 
@@ -1095,28 +1098,21 @@ public sealed class BSPrim : PhysicsActor
         // if the hull hasn't changed, don't rebuild it
         if (newHullKey == _hullKey) return;
 
-        DetailLog("{0},BSPrim.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},BSPrim.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},BSPrim.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();
@@ -1142,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
@@ -1202,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},BSPrim.CreateGeomHull,done", LocalID);
+        // DetailLog("{0},BSPrim.CreateGeomHull,done", LocalID);
         return;
     }
 
@@ -1340,11 +1336,12 @@ 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},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            // DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+            //         LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
 
             base.RequestPhysicsterseUpdate();
         }
+            /*
         else
         {
             // For debugging, we also report the movement of children
@@ -1352,10 +1349,11 @@ public sealed class BSPrim : PhysicsActor
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity, 
                     entprop.Acceleration, entprop.RotationalVelocity);
         }
+             */
     }
 
     // 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);
@@ -1367,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
@@ -1387,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 beaea1f..a31c578 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -362,7 +362,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPrim bsprim = prim as BSPrim;
         if (bsprim != null)
         {
-            DetailLog("{0},RemovePrim,call", bsprim.LocalID);
+            // DetailLog("{0},RemovePrim,call", bsprim.LocalID);
             // m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
             try
             {
@@ -388,7 +388,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         if (!m_initialized) return null;
 
-        DetailLog("{0},AddPrimShape,call", localID);
+        // DetailLog("{0},AddPrimShape,call", localID);
 
         BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical);
         lock (m_prims) m_prims.Add(localID, prim);
@@ -413,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();
@@ -429,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}", DetailLogZero, 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}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount);
+            // DetailLog("{0},PhysicsStepException,call, substeps={1}, updates={2}, colliders={3}", DetailLogZero, numSubSteps, updatedEntityCount, collidersCount);
             // updatedEntityCount = 0;
             collidersCount = 0;
         }
@@ -511,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
@@ -590,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();
@@ -608,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);
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 6800b96..504bd3c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -416,6 +416,27 @@ public static extern IntPtr Create6DofConstraint2(IntPtr world, IntPtr obj1, Int
                     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]
@@ -428,6 +449,9 @@ 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]
@@ -518,6 +542,9 @@ 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 IntPtr ClearAllForces2(IntPtr obj);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetMargin2(IntPtr obj, float val);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
index c3eada0..e1c054d 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
@@ -226,6 +226,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
         public const int ATTACH_BELLY = 28;
         public const int ATTACH_RPEC = 29;
         public const int ATTACH_LPEC = 30;
+        public const int ATTACH_LEFT_PEC = 29; // Same value as ATTACH_RPEC, see https://jira.secondlife.com/browse/SVC-580
+        public const int ATTACH_RIGHT_PEC = 30; // Same value as ATTACH_LPEC, see https://jira.secondlife.com/browse/SVC-580
         public const int ATTACH_HUD_CENTER_2 = 31;
         public const int ATTACH_HUD_TOP_RIGHT = 32;
         public const int ATTACH_HUD_TOP_CENTER = 33;