12 files changed, 335 insertions, 207 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 2a52e01..2e6b2da 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -263,7 +263,7 @@ public class BSCharacter : BSPhysObject
     // A version of the sanity check that also makes sure a new position value is
     //    pushed back to the physics engine. This routine would be used by anyone
     //    who is not already pushing the value.
-    private bool PositionSanityCheck2(bool atTaintTime)
+    private bool PositionSanityCheck2(bool inTaintTime)
     {
         bool ret = false;
         if (PositionSanityCheck())
@@ -275,7 +275,7 @@ public class BSCharacter : BSPhysObject
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
             };
-            if (atTaintTime)
+            if (inTaintTime)
                 sanityOperation();
             else
                 PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", sanityOperation);
@@ -332,6 +332,13 @@ public class BSCharacter : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceVelocity {
+        get { return _velocity; }
+        set {
+            _velocity = value;
+            BulletSimAPI.SetObjectVelocity(PhysicsScene.WorldID, LocalID, _velocity);
+        }
+    }
     public override OMV.Vector3 Torque {
         get { return _torque; }
         set { _torque = value;
@@ -432,6 +439,10 @@ public class BSCharacter : BSPhysObject
         get { return _rotationalVelocity; }
         set { _rotationalVelocity = value; }
     }
+    public override OMV.Vector3 ForceRotationalVelocity {
+        get { return _rotationalVelocity; }
+        set { _rotationalVelocity = value; }
+    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
index 63a4127..a20be3a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -49,9 +49,16 @@ public abstract class BSConstraint : IDisposable
         if (m_enabled)
         {
             m_enabled = false;
-            bool success = BulletSimAPI.DestroyConstraint2(m_world.ptr, m_constraint.ptr);
-            m_world.physicsScene.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;
+            if (m_constraint.ptr != IntPtr.Zero)
+            {
+                bool success = BulletSimAPI.DestroyConstraint2(m_world.ptr, m_constraint.ptr);
+                m_world.physicsScene.DetailLog("{0},BSConstraint.Dispose,taint,id1={1},body1={2},id2={3},body2={4},success={5}",
+                                    BSScene.DetailLogZero, 
+                                    m_body1.ID, m_body1.ptr.ToString("X"),
+                                    m_body2.ID, m_body2.ptr.ToString("X"),
+                                    success);
+                m_constraint.ptr = System.IntPtr.Zero;
+            }
         }
     }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 4ba2f62..56342b8 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -462,7 +462,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 return;
 
             // Set the prim's inertia to zero. The vehicle code handles that and this
-            //    removes the torque action introduced by Bullet.
+            //    removes the motion and torque actions introduced by Bullet.
             Vector3 inertia = Vector3.Zero;
             BulletSimAPI.SetMassProps2(Prim.BSBody.ptr, Prim.MassRaw, inertia);
             BulletSimAPI.UpdateInertiaTensor2(Prim.BSBody.ptr);
@@ -481,7 +481,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastPositionVector = Prim.ForcePosition;
 
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
+                    Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }// end Step
 
         // Apply the effect of the linear motor.
@@ -540,7 +540,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // add Gravity and Buoyancy
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Mass * (1f - m_VehicleBuoyancy));
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Linkset.LinksetMass * (1f - m_VehicleBuoyancy));
 
             /*
              * RA: Not sure why one would do this
@@ -678,10 +678,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_newVelocity.Z = 0;
 
             // Apply velocity
-            Prim.Velocity = m_newVelocity;
+            Prim.ForceVelocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
-            // m_prim.AddForce(grav, false);
+            Prim.AddForce(grav, false, true);
 
             // Apply friction
             Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
@@ -704,7 +704,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastAngularVelocity           // what was last applied to body
 
             // Get what the body is doing, this includes 'external' influences
-            Vector3 angularVelocity = Prim.RotationalVelocity;
+            Vector3 angularVelocity = Prim.ForceRotationalVelocity;
 
             if (m_angularMotorApply > 0)
             {
@@ -810,7 +810,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
 
             // Apply to the body
-            Prim.RotationalVelocity = m_lastAngularVelocity;
+            Prim.ForceRotationalVelocity = m_lastAngularVelocity;
 
             VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
@@ -862,7 +862,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private void VDetailLog(string msg, params Object[] args)
         {
             if (Prim.PhysicsScene.VehicleLoggingEnabled)
-                Prim.PhysicsScene.PhysicsLogging.Write(msg, args);
+                Prim.PhysicsScene.DetailLog(msg, args);
         }
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 3e82642..43b1262 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -52,8 +52,8 @@ public class BSLinkset
     //    the physical 'taint' children separately.
     // After taint processing and before the simulation step, these
     //    two lists must be the same.
-    private List<BSPhysObject> m_children;
-    private List<BSPhysObject> m_taintChildren;
+    private HashSet<BSPhysObject> m_children;
+    private HashSet<BSPhysObject> m_taintChildren;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -90,8 +90,8 @@ public class BSLinkset
             m_nextLinksetID = 1;
         PhysicsScene = scene;
         LinksetRoot = parent;
-        m_children = new List<BSPhysObject>();
-        m_taintChildren = new List<BSPhysObject>();
+        m_children = new HashSet<BSPhysObject>();
+        m_taintChildren = new HashSet<BSPhysObject>();
         m_mass = parent.MassRaw;
     }
 
@@ -160,6 +160,28 @@ public class BSLinkset
         return ret;
     }
 
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    // May be called at runtime or taint-time (just pass the appropriate flag).
+    public void Refresh(BSPhysObject requestor, bool inTaintTime)
+    {
+        // If there are no children, not physical or not root, I am not the one that recomputes the constraints
+        // (For the moment, static linksets do create constraints so remove the test for physical.)
+        if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
+            return;
+
+        BSScene.TaintCallback refreshOperation = delegate()
+            {
+                RecomputeLinksetConstraintVariables();
+                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}", 
+                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
+            };
+        if (inTaintTime)
+            refreshOperation();
+        else
+            PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
+    }
+
     // The object is going dynamic (physical). Do any setup necessary
     //     for a dynamic linkset.
     // Only the state of the passed object can be modified. The rest of the linkset
@@ -182,22 +204,19 @@ public class BSLinkset
         return false;
     }
 
-    // When physical properties are changed the linkset needs to recalculate
-    //   its internal properties.
-    // Called at runtime.
-    public void Refresh(BSPhysObject requestor)
+    // If the software is handling the movement of all the objects in a linkset
+    // (like if one doesn't use constraints for static linksets), this is called
+    // when an update for the root of the linkset is received.
+    // Called at taint-time!!
+    public void UpdateProperties(BSPhysObject physObject)
     {
-        // If there are no children, there can't be any constraints to recompute
-        if (!HasAnyChildren)
-            return;
-
-        // Only the root does the recomputation
-        if (IsRoot(requestor))
+        // The root local properties have been updated. Apply to the children if appropriate.
+        if (IsRoot(physObject) && HasAnyChildren)
         {
-            PhysicsScene.TaintedObject("BSLinkSet.Refresh", delegate()
+            if (!physObject.IsPhysical)
             {
-                RecomputeLinksetConstraintVariables();
-            });
+                // TODO: implement software linkset update for static object linksets
+            }
         }
     }
 
@@ -215,13 +234,10 @@ public class BSLinkset
             if (IsRoot(child))
             {
                 // If the one with the dependency is root, must undo all children
-                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},numChild={2}",
-                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
-                foreach (BSPhysObject bpo in m_taintChildren)
-                {
-                    PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
-                    ret = true;
-                }
+                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
+                                                child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
+
+                ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
             }
             else
             {
@@ -229,20 +245,16 @@ public class BSLinkset
                                                 child.LocalID,
                                                 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                 child.LocalID, child.BSBody.ptr.ToString("X"));
-                // Remove the dependency on the body of this one 
-                if (m_taintChildren.Contains(child))
-                {
-                    PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
-                    ret = true;
-                }
+                // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
+                // Despite the function name, this removes any link to the specified object.
+                ret = PhysicallyUnlinkAllChildrenFromRoot(child);
             }
         }
         return ret;
     }
 
-    // Routine used when rebuilding the body of the root of the linkset
-    // This is called after RemoveAllLinksToRoot() to restore all the constraints.
-    // This is called when the root body has been changed.
+    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
+    // this routine will restore the removed constraints.
     // Called at taint-time!!
     public void RestoreBodyDependencies(BSPrim child)
     {
@@ -254,7 +266,7 @@ public class BSLinkset
                                                 child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
                 foreach (BSPhysObject bpo in m_taintChildren)
                 {
-                    PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
+                    PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
                 }
             }
             else
@@ -263,7 +275,7 @@ public class BSLinkset
                                                 LinksetRoot.LocalID,
                                                 LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
                                                 child.LocalID, child.BSBody.ptr.ToString("X"));
-                PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
+                PhysicallyLinkAChildToRoot(LinksetRoot, child);
             }
         }
     }
@@ -330,22 +342,22 @@ public class BSLinkset
         {
             m_children.Add(child);
 
-            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
             BSPhysObject childx = child;
 
-            DetailLog("{0},AddChildToLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
-                            rootx.LocalID,
-                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
-                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
+            DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
             PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
-                DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
-                // build the physical binding between me and the child
-                m_taintChildren.Add(childx);
-
+                DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}", 
+                                rootx.LocalID,
+                                rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
+                                childx.LocalID, childx.BSBody.ptr.ToString("X"));
                 // Since this is taint-time, the body and shape could have changed for the child
-                PhysicallyLinkAChildToRoot(rootx, rootx.BSBody, childx, childx.BSBody);
+                rootx.ForcePosition = rootx.Position;   // DEBUG
+                childx.ForcePosition = childx.Position;   // DEBUG
+                PhysicallyLinkAChildToRoot(rootx, childx);
+                m_taintChildren.Add(child);
             });
         }
         return;
@@ -378,10 +390,8 @@ public class BSLinkset
 
             PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
-                if (m_taintChildren.Contains(childx))
-                    m_taintChildren.Remove(childx);
-
-                PhysicallyUnlinkAChildFromRoot(rootx, rootx.BSBody, childx, childx.BSBody);
+                m_taintChildren.Remove(child);
+                PhysicallyUnlinkAChildFromRoot(rootx, childx);
                 RecomputeLinksetConstraintVariables();
             });
 
@@ -396,8 +406,7 @@ public class BSLinkset
 
     // Create a constraint between me (root of linkset) and the passed prim (the child).
     // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BulletBody rootBody,
-                                    BSPhysObject childPrim, BulletBody childBody)
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
         // Zero motion for children so they don't interpolate
         childPrim.ZeroMotion();
@@ -409,33 +418,17 @@ public class BSLinkset
         // real world coordinate of midpoint between the two objects
         OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
 
-        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
                                         rootPrim.LocalID,
-                                        rootPrim.LocalID, rootBody.ptr.ToString("X"),
-                                        childPrim.LocalID, childBody.ptr.ToString("X"),
+                                        rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
+                                        childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
                                         rootPrim.Position, childPrim.Position, midPoint);
 
         // create a constraint that allows no freedom of movement between the two objects
         // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
 
-        // There is great subtlty in these paramters. Notice the check for a ptr of zero.
-        // We pass the BulletBody structure into the taint in order to capture the pointer
-        //     of the body at the time of constraint creation. This doesn't work for the very first
-        //     construction because there is no body yet. The body
-        //     is constructed later at taint time. Thus we use the body address at time of the
-        //     taint creation but, if it is zero, use what's in the prim at the moment.
-        //     There is a possible race condition since shape can change without a taint call
-        //     (like changing to a mesh that is already constructed). The fix for that would be
-        //     to only change BSShape at taint time thus syncronizing these operations at
-        //     the cost of efficiency and lag.
         BS6DofConstraint constrain = new BS6DofConstraint(
-                        PhysicsScene.World,
-                        rootBody.ptr == IntPtr.Zero ? rootPrim.BSBody : rootBody,
-                        childBody.ptr == IntPtr.Zero ? childPrim.BSBody : childBody,
-                        midPoint,
-                        true,
-                        true
-                        );
+                            PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
 
         /* NOTE: below is an attempt to build constraint with full frame computation, etc.
          *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
@@ -452,7 +445,7 @@ 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
-        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
         BS6DofConstraint constrain = new BS6DofConstraint(
                         PhysicsScene.World, rootPrim.Body, childPrim.Body,
                         OMV.Vector3.Zero,
@@ -486,39 +479,44 @@ public class BSLinkset
         {
             constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
         }
-
-        RecomputeLinksetConstraintVariables();
     }
 
     // Remove linkage between myself and a particular child
     // The root and child bodies are passed in because we need to remove the constraint between
     //      the bodies that were at unlink time.
     // Called at taint time!
-    private void PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BulletBody rootBody,
-                                                    BSPhysObject childPrim, BulletBody childBody)
+    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
-        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
+        bool ret = false;
+        DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
                             rootPrim.LocalID,
-                            rootPrim.LocalID, rootBody.ptr.ToString("X"),
-                            childPrim.LocalID, childBody.ptr.ToString("X"));
+                            rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
+                            childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
 
         // Find the constraint for this link and get rid of it from the overall collection and from my list
-        PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootBody, childBody);
+        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
+        {
+            // Make the child refresh its location
+            BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
+            ret = true;
+        }
 
-        // Make the child refresh its location
-        BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
+        return ret;
     }
 
-    /*
     // Remove linkage between myself and any possible children I might have.
     // Called at taint time!
-    private void PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
+    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
     {
-        DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+        DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+        bool ret = false;
 
-        PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody);
+        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
+        {
+            ret = true;
+        }
+        return ret;
     }
-     */
 
     // Call each of the constraints that make up this linkset and recompute the
     //    various transforms and variables. Used when objects are added or removed
@@ -550,11 +548,17 @@ public class BSLinkset
         {
             // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
             OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
-            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
+            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, 
+                                centerOfMass, OMV.Quaternion.Identity);
+            DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
+                                LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
             foreach (BSPhysObject child in m_taintChildren)
             {
-                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
+                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, 
+                                centerOfMass, OMV.Quaternion.Identity);
             }
+
+            // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr);  // DEBUG DEBUG DEBUG
         }
         return;
     }
@@ -563,7 +567,8 @@ public class BSLinkset
     // Invoke the detailed logger and output something if it's enabled.
     private void DetailLog(string msg, params Object[] args)
     {
-        PhysicsScene.PhysicsLogging.Write(msg, args);
+        if (PhysicsScene.PhysicsLogging.Enabled)
+            PhysicsScene.DetailLog(msg, args);
     }
 
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 1ac8c59..cae599c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -85,6 +85,10 @@ public abstract class BSPhysObject : PhysicsActor
 
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
+    public abstract OMV.Vector3 ForceVelocity { get; set; }
+
+    public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
@@ -207,7 +211,8 @@ public abstract class BSPhysObject : PhysicsActor
     // High performance detailed logging routine used by the physical objects.
     protected void DetailLog(string msg, params Object[] args)
     {
-        PhysicsScene.PhysicsLogging.Write(msg, args);
+        if (PhysicsScene.PhysicsLogging.Enabled)
+            PhysicsScene.DetailLog(msg, args);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index f7b68ba..6a4365c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -196,7 +196,7 @@ public sealed class BSPrim : BSPhysObject
             _isSelected = value;
             PhysicsScene.TaintedObject("BSPrim.setSelected", delegate()
             {
-                // DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
+                DetailLog("{0},BSPrim.selected,taint,selected={1}", LocalID, _isSelected);
                 SetObjectDynamic(false);
             });
         }
@@ -265,6 +265,11 @@ public sealed class BSPrim : BSPhysObject
             return _position;
         }
         set {
+            // If you must push the position into the physics engine, use ForcePosition.
+            if (_position == value)
+            {
+                return;
+            }
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             PositionSanityCheck();
@@ -322,7 +327,7 @@ public sealed class BSPrim : BSPhysObject
     // A version of the sanity check that also makes sure a new position value is
     //    pushed back to the physics engine. This routine would be used by anyone
     //    who is not already pushing the value.
-    private bool PositionSanityCheck2(bool atTaintTime)
+    private bool PositionSanityCheck2(bool inTaintTime)
     {
         bool ret = false;
         if (PositionSanityCheck())
@@ -334,7 +339,7 @@ public sealed class BSPrim : BSPhysObject
                 DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
             };
-            if (atTaintTime)
+            if (inTaintTime)
                 sanityOperation();
             else
                 PhysicsScene.TaintedObject("BSPrim.PositionSanityCheck", sanityOperation);
@@ -453,7 +458,6 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
-
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
@@ -465,6 +469,13 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceVelocity {
+        get { return _velocity; }
+        set {
+            _velocity = value;
+            BulletSimAPI.SetLinearVelocity2(BSBody.ptr, _velocity);
+        }
+    }
     public override OMV.Vector3 Torque {
         get { return _torque; }
         set { _torque = value;
@@ -490,6 +501,8 @@ public sealed class BSPrim : BSPhysObject
             return _orientation;
         }
         set {
+            if (_orientation == value)
+                return;
             _orientation = value;
             // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
             PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
@@ -570,7 +583,7 @@ public sealed class BSPrim : BSPhysObject
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
 
-        // Update vehicle specific parameters
+        // Update vehicle specific parameters (after MakeDynamic() so can change physical parameters)
         _vehicle.Refresh();
 
         // Arrange for collision events if the simulator wants them
@@ -593,7 +606,7 @@ public sealed class BSPrim : BSPhysObject
         // Recompute any linkset parameters.
         // When going from non-physical to physical, this re-enables the constraints that
         //     had been automatically disabled when the mass was set to zero.
-        Linkset.Refresh(this);
+        Linkset.Refresh(this, true);
 
         DetailLog("{0},BSPrim.UpdatePhysicalParameters,exit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
                         LocalID, IsStatic, IsSolid, _mass, SubscribedEvents(), CurrentCollisionFlags, BSBody, BSShape);
@@ -620,8 +633,10 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
             // There can be special things needed for implementing linksets
             Linkset.MakeStatic(this);
-            // The activation state is 'disabled' so Bullet will not try to act on it
+            // The activation state is 'disabled' so Bullet will not try to act on it.
             BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
+            // Start it out sleeping and physical actions could wake it up.
+            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
 
             BSBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
             BSBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
@@ -638,6 +653,9 @@ public sealed class BSPrim : BSPhysObject
             // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
             BulletSimAPI.ClearAllForces2(BSBody.ptr);
 
+            // For good measure, make sure the transform is set through to the motion state
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+
             // A dynamic object has mass
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
             OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
@@ -655,8 +673,8 @@ public sealed class BSPrim : BSPhysObject
 
             // Force activation of the object so Bullet will act on it.
             // Must do the ForceActivationState2() to overcome the DISABLE_SIMULATION from static objects.
-            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
-            BulletSimAPI.Activate2(BSBody.ptr, true);
+            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
+            // BulletSimAPI.Activate2(BSBody.ptr, true);
 
             BSBody.collisionFilter = CollisionFilterGroups.ObjectFilter;
             BSBody.collisionMask = CollisionFilterGroups.ObjectMask;
@@ -774,6 +792,15 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceRotationalVelocity {
+        get {
+            return _rotationalVelocity;
+        }
+        set {
+            _rotationalVelocity = value;
+            BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity);
+        }
+    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value;
@@ -828,6 +855,9 @@ public sealed class BSPrim : BSPhysObject
 
     private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
+        AddForce(force, pushforce, false);
+    }
+    public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
         {
@@ -840,11 +870,12 @@ public sealed class BSPrim : BSPhysObject
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        PhysicsScene.TaintedObject("BSPrim.AddForce", delegate()
+        BSScene.TaintCallback addForceOperation = delegate()
         {
             OMV.Vector3 fSum = OMV.Vector3.Zero;
             lock (m_accumulatedForces)
             {
+                // Sum the accumulated additional forces for one big force to apply once.
                 foreach (OMV.Vector3 v in m_accumulatedForces)
                 {
                     fSum += v;
@@ -854,7 +885,11 @@ public sealed class BSPrim : BSPhysObject
             // DetailLog("{0},BSPrim.AddObjectForce,taint,force={1}", LocalID, fSum);
             // For unknown reasons, "ApplyCentralForce" adds this force to the total force on the object.
             BulletSimAPI.ApplyCentralForce2(BSBody.ptr, fSum);
-        });
+        };
+        if (inTaintTime)
+            addForceOperation();
+        else
+            PhysicsScene.TaintedObject("BSPrim.AddForce", addForceOperation);
     }
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
@@ -1204,6 +1239,7 @@ public sealed class BSPrim : BSPhysObject
         {
             // Called if the current prim body is about to be destroyed.
             // Remove all the physical dependencies on the old body.
+            // (Maybe someday make the changing of BSShape an event handled by BSLinkset.)
             needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
         });
 
@@ -1294,6 +1330,8 @@ public sealed class BSPrim : BSPhysObject
 
             PositionSanityCheck2(true);
 
+            Linkset.UpdateProperties(this);
+
             DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
 
@@ -1304,7 +1342,7 @@ public sealed class BSPrim : BSPhysObject
             /*
         else
         {
-            // For debugging, we can also report the movement of children
+            // For debugging, report the movement of children
             DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity,
                     entprop.Acceleration, entprop.RotationalVelocity);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index aaed7de..2c3c481 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -39,7 +39,6 @@ using log4net;
 using OpenMetaverse;
 
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
-// Move all logic out of the C++ code and into the C# code for easier future modifications.
 // Test sculpties (verified that they don't work)
 // Compute physics FPS reasonably
 // Based on material, set density and friction
@@ -90,10 +89,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // let my minuions use my logger
     public ILog Logger { get { return m_log; } }
 
-    // If non-zero, the number of simulation steps between calls to the physics
-    //    engine to output detailed physics stats. Debug logging level must be on also.
-    private int m_detailedStatsStep = 0;
-
     public IMesher mesher;
     // Level of Detail values kept as float because that's what the Meshmerizer wants
     public float MeshLOD { get; private set; }
@@ -112,6 +107,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private float m_fixedTimeStep;
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
+    private int m_taintsToProcessPerStep;
 
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
@@ -131,6 +127,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
     public bool ShouldForceSimplePrimMeshing { get; private set; }   // if a cube or sphere, let Bullet do internal shapes
+    public bool ShouldUseHullsForPhysicalObjects { get; private set; }   // 'true' if should create hulls for physical objects
 
     public float PID_D { get; private set; }    // derivative
     public float PID_P { get; private set; }    // proportional
@@ -254,7 +251,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // The bounding box for the simulated world. The origin is 0,0,0 unless we're
         //    a child in a mega-region.
-        // Turns out that Bullet really doesn't care about the extents of the simulated
+        // Bullet actually doesn't care about the extents of the simulated
         //    area. It tracks active objects no matter where they are.
         Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight);
 
@@ -331,7 +328,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // Called directly from unmanaged code so don't do much
     private void BulletLoggerPhysLog(string msg)
     {
-        PhysicsLogging.Write("[BULLETS UNMANAGED]:" + msg);
+        DetailLog("[BULLETS UNMANAGED]:" + msg);
     }
 
     public override void Dispose()
@@ -494,8 +491,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_simulationStep++;
         int numSubSteps = 0;
 
-        // Sometimes needed for debugging to find out what happened before the step
-        // PhysicsLogging.Flush();
+        // DEBUG
+        // DetailLog("{0},BSScene.Simulate,beforeStep,ntaimts={1},step={2}", DetailLogZero, numTaints, m_simulationStep);
 
         try
         {
@@ -505,8 +502,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                         out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
 
             if (PhysicsLogging.Enabled) simTime = Util.EnvironmentTickCountSubtract(beforeTime);
-            DetailLog("{0},Simulate,call, nTaints={1}, simTime={2}, substeps={3}, updates={4}, colliders={5}",
-                        DetailLogZero, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
+            DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}",
+                        DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
         }
         catch (Exception e)
         {
@@ -582,19 +579,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        // If enabled, call into the physics engine to dump statistics
-        if (m_detailedStatsStep > 0)
-        {
-            if ((m_simulationStep % m_detailedStatsStep) == 0)
-            {
-                BulletSimAPI.DumpBulletStatistics();
-            }
-        }
-
         // The physics engine returns the number of milliseconds it simulated this call.
         // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
-        // Since Bullet normally does 5 or 6 substeps, this will normally sum to about 60 FPS.
-        return numSubSteps * m_fixedTimeStep * 1000;
+        // We multiply by 45 to give a recognizable running rate (45 or less).
+        return numSubSteps * m_fixedTimeStep * 1000 * 45;
+        // return timeStep * 1000 * 45;
     }
 
     // Something has collided
@@ -617,7 +606,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPhysObject collidee = null;
         PhysObjects.TryGetValue(collidingWith, out collidee);
 
-        DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
+        // DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
 
         if (collider.Collide(collidingWith, collidee, collidePoint, collideNormal, penetration))
         {
@@ -704,6 +693,35 @@ 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
+            int taintCount = m_taintsToProcessPerStep;
+            TaintCallbackEntry oneCallback = new TaintCallbackEntry();
+            while (_taintedObjects.Count > 0 && taintCount-- > 0)
+            {
+                bool gotOne = false;
+                lock (_taintLock)
+                {
+                    if (_taintedObjects.Count > 0)
+                    {
+                        oneCallback = _taintedObjects[0];
+                        _taintedObjects.RemoveAt(0);
+                        gotOne = true;
+                    }
+                }
+                if (gotOne)
+                {
+                    try
+                    {
+                        DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, oneCallback.ident); // DEBUG DEBUG DEBUG
+                        oneCallback.callback();
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("{0}: ProcessTaints: {1}: Exception: {2}", LogHeader, oneCallback.ident, e);
+                    }
+                }
+            }
+            /*
+            // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
             {
@@ -715,6 +733,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             {
                 try
                 {
+                    DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, tcbe.ident); // DEBUG DEBUG DEBUG
                     tcbe.callback();
                 }
                 catch (Exception e)
@@ -723,6 +742,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 }
             }
             oldList.Clear();
+             */
         }
     }
 
@@ -834,6 +854,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.ShouldForceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
             (s) => { return s.NumericBool(s.ShouldForceSimplePrimMeshing); },
             (s,p,l,v) => { s.ShouldForceSimplePrimMeshing = s.BoolNumeric(v); } ),
+        new ParameterDefn("UseHullsForPhysicalObjects", "If true, create hulls for physical objects",
+            ConfigurationParameters.numericTrue,
+            (s,cf,p,v) => { s.ShouldUseHullsForPhysicalObjects = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldUseHullsForPhysicalObjects); },
+            (s,p,l,v) => { s.ShouldUseHullsForPhysicalObjects = s.BoolNumeric(v); } ),
 
         new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
             8f,
@@ -876,6 +901,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_maxUpdatesPerFrame; },
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
+        new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
+            100f,
+            (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_taintsToProcessPerStep; },
+            (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
         new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)",
             10000.01f,
             (s,cf,p,v) => { s.MaximumObjectMass = cf.GetFloat(p, v); },
@@ -1070,12 +1100,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
             (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
             new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1",
-            0.1f,
+            0.001f,
             (s,cf,p,v) => { s.m_params[0].linkConstraintCFM = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintCFM; },
             (s,p,l,v) => { s.m_params[0].linkConstraintCFM = v; } ),
             new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2",
-            0.2f,
+            0.8f,
             (s,cf,p,v) => { s.m_params[0].linkConstraintERP = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linkConstraintERP; },
             (s,p,l,v) => { s.m_params[0].linkConstraintERP = v; } ),
@@ -1085,11 +1115,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].linkConstraintSolverIterations; },
             (s,p,l,v) => { s.m_params[0].linkConstraintSolverIterations = v; } ),
 
-        new ParameterDefn("DetailedStats", "Frames between outputting detailed phys stats. (0 is off)",
+        new ParameterDefn("LogPhysicsStatisticsFrames", "Frames between outputting detailed phys stats. (0 is off)",
             0f,
-            (s,cf,p,v) => { s.m_detailedStatsStep = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_detailedStatsStep; },
-            (s,p,l,v) => { s.m_detailedStatsStep = (int)v; } ),
+            (s,cf,p,v) => { s.m_params[0].physicsLoggingFrames = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_params[0].physicsLoggingFrames; },
+            (s,p,l,v) => { s.m_params[0].physicsLoggingFrames = (int)v; } ),
     };
 
     // Convert a boolean to our numeric true and false values
@@ -1270,6 +1300,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     public void DetailLog(string msg, params Object[] args)
     {
         PhysicsLogging.Write(msg, args);
+        // Add the Flush() if debugging crashes to get all the messages written out.
+        // PhysicsLogging.Flush();
     }
     // used to fill in the LocalID when there isn't one
     public const string DetailLogZero = "0000000000";
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 399a133..d189f1d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -67,8 +67,8 @@ public class BSShapeCollection : IDisposable
         public DateTime lastReferenced;
     }
 
-    private Dictionary<ulong, MeshDesc> Meshes = new Dictionary<ulong, MeshDesc>();
-    private Dictionary<ulong, HullDesc> Hulls = new Dictionary<ulong, HullDesc>();
+    private Dictionary<System.UInt64, MeshDesc> Meshes = new Dictionary<System.UInt64, MeshDesc>();
+    private Dictionary<System.UInt64, HullDesc> Hulls = new Dictionary<System.UInt64, HullDesc>();
     private Dictionary<uint, BodyDesc> Bodies = new Dictionary<uint, BodyDesc>();
 
     public BSShapeCollection(BSScene physScene)
@@ -121,7 +121,7 @@ public class BSShapeCollection : IDisposable
     // Track another user of a body
     // We presume the caller has allocated the body.
     // Bodies only have one user so the reference count is either 1 or 0.
-    public void ReferenceBody(BulletBody body, bool atTaintTime)
+    public void ReferenceBody(BulletBody body, bool inTaintTime)
     {
         lock (m_collectionActivityLock)
         {
@@ -136,7 +136,21 @@ public class BSShapeCollection : IDisposable
                 // New entry
                 bodyDesc.ptr = body.ptr;
                 bodyDesc.referenceCount = 1;
-                DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,ref={1}", body.ID, body, bodyDesc.referenceCount);
+                DetailLog("{0},BSShapeCollection.ReferenceBody,newBody,ref={2}",
+                                    body.ID, body, bodyDesc.referenceCount);
+                BSScene.TaintCallback createOperation = delegate()
+                {
+                    if (!BulletSimAPI.IsInWorld2(body.ptr))
+                    {
+                        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, body.ptr);
+                        DetailLog("{0},BSShapeCollection.ReferenceBody,addedToWorld,ref={1}", 
+                                        body.ID, body);
+                    }
+                };
+                if (inTaintTime)
+                    createOperation();
+                else
+                    PhysicsScene.TaintedObject("BSShapeCollection.ReferenceBody", createOperation);
             }
             bodyDesc.lastReferenced = System.DateTime.Now;
             Bodies[body.ID] = bodyDesc;
@@ -160,21 +174,22 @@ public class BSShapeCollection : IDisposable
                 Bodies[body.ID] = bodyDesc;
                 DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", body.ID, bodyDesc.referenceCount);
 
-                // If body is no longer being used, free it -- bodies are never shared.
+                // If body is no longer being used, free it -- bodies can never be shared.
                 if (bodyDesc.referenceCount == 0)
                 {
                     Bodies.Remove(body.ID);
                     BSScene.TaintCallback removeOperation = delegate()
                     {
-                        DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}",
-                                                    body.ID, body.ptr.ToString("X"));
+                        DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}, inTaintTime={2}",
+                                                    body.ID, body.ptr.ToString("X"), inTaintTime);
                         // If the caller needs to know the old body is going away, pass the event up.
                         if (bodyCallback != null) bodyCallback(body);
 
-                        // Zero any reference to the shape so it is not freed when the body is deleted.
-                        BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
                         // It may have already been removed from the world in which case the next is a NOOP.
                         BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
+
+                        // Zero any reference to the shape so it is not freed when the body is deleted.
+                        BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
                         BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
                     };
                     // If already in taint-time, do the operations now. Otherwise queue for later.
@@ -208,7 +223,7 @@ public class BSShapeCollection : IDisposable
                 {
                     // There is an existing instance of this mesh.
                     meshDesc.referenceCount++;
-                    DetailLog("{0},BSShapeColliction.ReferenceShape,existingMesh,key={1},cnt={2}",
+                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingMesh,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
                 }
                 else
@@ -217,7 +232,7 @@ public class BSShapeCollection : IDisposable
                     meshDesc.ptr = shape.ptr;
                     // We keep a reference to the underlying IMesh data so a hull can be built
                     meshDesc.referenceCount = 1;
-                    DetailLog("{0},BSShapeColliction.ReferenceShape,newMesh,key={1},cnt={2}",
+                    DetailLog("{0},BSShapeCollection.ReferenceShape,newMesh,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), meshDesc.referenceCount);
                     ret = true;
                 }
@@ -230,7 +245,7 @@ public class BSShapeCollection : IDisposable
                 {
                     // There is an existing instance of this hull.
                     hullDesc.referenceCount++;
-                    DetailLog("{0},BSShapeColliction.ReferenceShape,existingHull,key={1},cnt={2}",
+                    DetailLog("{0},BSShapeCollection.ReferenceShape,existingHull,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
                 }
                 else
@@ -238,7 +253,7 @@ public class BSShapeCollection : IDisposable
                     // This is a new reference to a hull
                     hullDesc.ptr = shape.ptr;
                     hullDesc.referenceCount = 1;
-                    DetailLog("{0},BSShapeColliction.ReferenceShape,newHull,key={1},cnt={2}",
+                    DetailLog("{0},BSShapeCollection.ReferenceShape,newHull,key={1},cnt={2}",
                                 BSScene.DetailLogZero, shape.shapeKey.ToString("X"), hullDesc.referenceCount);
                     ret = true;
 
@@ -257,7 +272,7 @@ public class BSShapeCollection : IDisposable
 
     // Release the usage of a shape.
     // The collisionObject is released since it is a copy of the real collision shape.
-    public void DereferenceShape(BulletShape shape, bool atTaintTime, ShapeDestructionCallback shapeCallback)
+    public void DereferenceShape(BulletShape shape, bool inTaintTime, ShapeDestructionCallback shapeCallback)
     {
         if (shape.ptr == IntPtr.Zero)
             return;
@@ -279,14 +294,14 @@ public class BSShapeCollection : IDisposable
                     if (shape.ptr != IntPtr.Zero & shape.isNativeShape)
                     {
                         DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,ptr={1},taintTime={2}",
-                                        BSScene.DetailLogZero, shape.ptr.ToString("X"), atTaintTime);
+                                        BSScene.DetailLogZero, shape.ptr.ToString("X"), inTaintTime);
                         if (shapeCallback != null) shapeCallback(shape);
                         BulletSimAPI.DeleteCollisionShape2(PhysicsScene.World.ptr, shape.ptr);
                     }
                     break;
             }
         };
-        if (atTaintTime)
+        if (inTaintTime)
         {
             lock (m_collectionActivityLock)
             {
@@ -358,7 +373,8 @@ public class BSShapeCollection : IDisposable
                         && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
                         && pbs.PathShearX == 0 && pbs.PathShearY == 0) ) )
         {
-            if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
+            if ((pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1)
+                                && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)
             {
                 haveShape = true;
                 if (forceRebuild
@@ -372,7 +388,7 @@ public class BSShapeCollection : IDisposable
                                         prim.LocalID, forceRebuild, prim.BSShape);
                 }
             }
-            else
+            if (pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
             {
                 haveShape = true;
                 if (forceRebuild
@@ -392,7 +408,7 @@ public class BSShapeCollection : IDisposable
         //     made. Native shapes are best used in either case.
         if (!haveShape)
         {
-            if (prim.IsPhysical)
+            if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
             {
                 // Update prim.BSShape to reference a hull of this shape.
                 ret = GetReferenceToHull(prim, shapeData, pbs, shapeCallback);
@@ -426,7 +442,7 @@ public class BSShapeCollection : IDisposable
 
         // Native shapes are always built independently.
         newShape = new BulletShape(BulletSimAPI.BuildNativeShape2(PhysicsScene.World.ptr, shapeData), shapeType);
-        newShape.shapeKey = (ulong)shapeKey;
+        newShape.shapeKey = (System.UInt64)shapeKey;
         newShape.isNativeShape = true;
 
         // Don't need to do a 'ReferenceShape()' here because native shapes are not tracked.
@@ -446,7 +462,7 @@ public class BSShapeCollection : IDisposable
         BulletShape newShape = new BulletShape(IntPtr.Zero);
 
         float lod;
-        ulong newMeshKey = ComputeShapeKey(shapeData, pbs, out lod);
+        System.UInt64 newMeshKey = ComputeShapeKey(shapeData, pbs, out lod);
 
         // if this new shape is the same as last time, don't recreate the mesh
         if (newMeshKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_MESH)
@@ -469,7 +485,7 @@ public class BSShapeCollection : IDisposable
         return true;        // 'true' means a new shape has been added to this prim
     }
 
-    private BulletShape CreatePhysicalMesh(string objName, ulong newMeshKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    private BulletShape CreatePhysicalMesh(string objName, System.UInt64 newMeshKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
     {
         IMesh meshData = null;
         IntPtr meshPtr;
@@ -516,7 +532,7 @@ public class BSShapeCollection : IDisposable
         BulletShape newShape;
 
         float lod;
-        ulong newHullKey = ComputeShapeKey(shapeData, pbs, out lod);
+        System.UInt64 newHullKey = ComputeShapeKey(shapeData, pbs, out lod);
 
         // if the hull hasn't changed, don't rebuild it
         if (newHullKey == prim.BSShape.shapeKey && prim.BSShape.type == ShapeData.PhysicsShapeType.SHAPE_HULL)
@@ -525,7 +541,7 @@ public class BSShapeCollection : IDisposable
         DetailLog("{0},BSShapeCollection.CreateGeomHull,create,oldKey={1},newKey={2}",
                         prim.LocalID, prim.BSShape.shapeKey.ToString("X"), newHullKey.ToString("X"));
 
-        // Remove usage of the previous shape. Also removes reference to underlying mesh if it is a hull.
+        // Remove usage of the previous shape.
         DereferenceShape(prim.BSShape, true, shapeCallback);
 
         newShape = CreatePhysicalHull(prim.PhysObjectName, newHullKey, pbs, shapeData.Size, lod);
@@ -539,7 +555,7 @@ public class BSShapeCollection : IDisposable
     }
 
     List<ConvexResult> m_hulls;
-    private BulletShape CreatePhysicalHull(string objName, ulong newHullKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    private BulletShape CreatePhysicalHull(string objName, System.UInt64 newHullKey, PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
     {
 
         IntPtr hullPtr;
@@ -652,22 +668,23 @@ public class BSShapeCollection : IDisposable
 
     // Create a hash of all the shape parameters to be used as a key
     //    for this particular shape.
-    private ulong ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs, out float retLod)
+    private System.UInt64 ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs, out float retLod)
     {
         // level of detail based on size and type of the object
         float lod = PhysicsScene.MeshLOD;
         if (pbs.SculptEntry)
             lod = PhysicsScene.SculptLOD;
 
+        // Mega prims usually get more detail because one can interact with shape approximations at this size.
         float maxAxis = Math.Max(shapeData.Size.X, Math.Max(shapeData.Size.Y, shapeData.Size.Z));
         if (maxAxis > PhysicsScene.MeshMegaPrimThreshold)
             lod = PhysicsScene.MeshMegaPrimLOD;
 
         retLod = lod;
-        return (ulong)pbs.GetMeshKey(shapeData.Size, lod);
+        return pbs.GetMeshKey(shapeData.Size, lod);
     }
     // For those who don't want the LOD
-    private ulong ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs)
+    private System.UInt64 ComputeShapeKey(ShapeData shapeData, PrimitiveBaseShape pbs)
     {
         float lod;
         return ComputeShapeKey(shapeData, pbs, out lod);
@@ -701,6 +718,7 @@ public class BSShapeCollection : IDisposable
 
         if (mustRebuild || forceRebuild)
         {
+            // Free any old body
             DereferenceBody(prim.BSBody, true, bodyCallback);
 
             BulletBody aBody;
@@ -709,13 +727,13 @@ public class BSShapeCollection : IDisposable
             {
                 bodyPtr = BulletSimAPI.CreateBodyFromShape2(sim.ptr, shape.ptr,
                                         shapeData.ID, shapeData.Position, shapeData.Rotation);
-                // DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                DetailLog("{0},BSShapeCollection.CreateBody,mesh,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
             }
             else
             {
                 bodyPtr = BulletSimAPI.CreateGhostFromShape2(sim.ptr, shape.ptr,
                                         shapeData.ID, shapeData.Position, shapeData.Rotation);
-                // DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
+                DetailLog("{0},BSShapeCollection.CreateBody,ghost,ptr={1}", prim.LocalID, bodyPtr.ToString("X"));
             }
             aBody = new BulletBody(shapeData.ID, bodyPtr);
 
@@ -731,7 +749,8 @@ public class BSShapeCollection : IDisposable
 
     private void DetailLog(string msg, params Object[] args)
     {
-        PhysicsScene.PhysicsLogging.Write(msg, args);
+        if (PhysicsScene.PhysicsLogging.Enabled)
+            PhysicsScene.DetailLog(msg, args);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 70aa429..caf411e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -114,6 +114,8 @@ public class BSTerrainManager
                         BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
                                                             Vector3.Zero, Quaternion.Identity));
         BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr);
+        // Ground plane does not move
+        BulletSimAPI.ForceActivationState2(m_groundPlane.ptr, ActivationState.DISABLE_SIMULATION);
         // Everything collides with the ground plane.
         BulletSimAPI.SetCollisionFilterMask2(m_groundPlane.ptr,
                         (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask);
@@ -201,10 +203,10 @@ public class BSTerrainManager
     // The 'doNow' boolean says whether to do all the unmanaged activities right now (like when
     //     calling this routine from initialization or taint-time routines) or whether to delay
     //     all the unmanaged activities to taint-time.
-    private void UpdateOrCreateTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords, bool atTaintTime)
+    private void UpdateOrCreateTerrain(uint id, float[] heightMap, Vector3 minCoords, Vector3 maxCoords, bool inTaintTime)
     {
-        DetailLog("{0},BSTerrainManager.UpdateOrCreateTerrain,call,minC={1},maxC={2},atTaintTime={3}",
-                            BSScene.DetailLogZero, minCoords, maxCoords, atTaintTime);
+        DetailLog("{0},BSTerrainManager.UpdateOrCreateTerrain,call,minC={1},maxC={2},inTaintTime={3}",
+                            BSScene.DetailLogZero, minCoords, maxCoords, inTaintTime);
 
         float minZ = float.MaxValue;
         float maxZ = float.MinValue;
@@ -296,16 +298,16 @@ public class BSTerrainManager
                     mapInfo.Ptr = BulletSimAPI.CreateHeightMapInfo2(PhysicsScene.World.ptr, mapInfo.ID,
                         mapInfo.minCoords, mapInfo.maxCoords, mapInfo.heightMap, TERRAIN_COLLISION_MARGIN);
 
+                    // Create the terrain shape from the mapInfo
+                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr),
+                                ShapeData.PhysicsShapeType.SHAPE_TERRAIN);
+
                     // The terrain object initial position is at the center of the object
                     Vector3 centerPos;
                     centerPos.X = minCoords.X + (mapInfo.sizeX / 2f);
                     centerPos.Y = minCoords.Y + (mapInfo.sizeY / 2f);
                     centerPos.Z = minZ + ((maxZ - minZ) / 2f);
 
-                    // Create the terrain shape from the mapInfo
-                    mapInfo.terrainShape = new BulletShape(BulletSimAPI.CreateTerrainShape2(mapInfo.Ptr),
-                                ShapeData.PhysicsShapeType.SHAPE_TERRAIN);
-
                     mapInfo.terrainBody = new BulletBody(mapInfo.ID,
                             BulletSimAPI.CreateBodyWithDefaultMotionState2(mapInfo.terrainShape.ptr,
                                                         id, centerPos, Quaternion.Identity));
@@ -320,9 +322,6 @@ public class BSTerrainManager
                 BulletSimAPI.SetRestitution2(mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainRestitution);
                 BulletSimAPI.SetCollisionFlags2(mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
 
-                BulletSimAPI.SetMassProps2(mapInfo.terrainBody.ptr, 0f, Vector3.Zero);
-                BulletSimAPI.UpdateInertiaTensor2(mapInfo.terrainBody.ptr);
-
                 // Return the new terrain to the world of physical objects
                 BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, mapInfo.terrainBody.ptr);
 
@@ -342,7 +341,7 @@ public class BSTerrainManager
 
             // There is the option to do the changes now (we're already in 'taint time'), or
             //     to do the Bullet operations later.
-            if (atTaintTime)
+            if (inTaintTime)
                 rebuildOperation();
             else
                 PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:UpdateExisting", rebuildOperation);
@@ -381,7 +380,7 @@ public class BSTerrainManager
             };
 
             // If already in taint-time, just call Bullet. Otherwise queue the operations for the safe time.
-            if (atTaintTime)
+            if (inTaintTime)
                 createOperation();
             else
                 PhysicsScene.TaintedObject("BSScene.UpdateOrCreateTerrain:NewTerrain", createOperation);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index a43880d..276111c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -101,9 +101,8 @@ public struct BulletShape
     }
     public IntPtr ptr;
     public ShapeData.PhysicsShapeType type;
-    public ulong shapeKey;
+    public System.UInt64 shapeKey;
     public bool isNativeShape;
-    // Hulls have an underlying mesh. A pointer to it is hidden here.
     public override string ToString()
     {
         StringBuilder buff = new StringBuilder();
@@ -192,8 +191,9 @@ public struct ShapeData
                 SHAPE_SPHERE    = 5,
                 SHAPE_MESH      = 6,
                 SHAPE_HULL      = 7,
-                SHAPE_GROUNDPLANE  = 8,
-                SHAPE_TERRAIN   = 9,
+        // following defined by BulletSim
+                SHAPE_GROUNDPLANE  = 20,
+                SHAPE_TERRAIN   = 21,
     };
     public uint ID;
     public PhysicsShapeType Type;
@@ -305,6 +305,8 @@ public struct ConfigurationParameters
     public float linkConstraintCFM;
     public float linkConstraintSolverIterations;
 
+    public float physicsLoggingFrames;
+
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
 }
@@ -1037,18 +1039,6 @@ public static extern IntPtr GetConstraintRef2(IntPtr obj, int index);
 public static extern int GetNumConstraintRefs2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetDeltaLinearVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetDeltaAngularVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetPushVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern Vector3 GetTurnVelocity2(IntPtr obj);
-
-[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetCollisionFilterMask2(IntPtr body, uint filter, uint mask);
 
 // =====================================================================================
@@ -1108,6 +1098,15 @@ public static extern float GetMargin2(IntPtr shape);
 public static extern void DumpRigidBody2(IntPtr sim, IntPtr collisionObject);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpCollisionShape2(IntPtr sim, IntPtr collisionShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpConstraint2(IntPtr sim, IntPtr constrain);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpAllInfo2(IntPtr sim);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void DumpMapInfo2(IntPtr sim, IntPtr manInfo);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index f3b0630..c736557 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -100,7 +100,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private bool m_hackSentFly = false;
         private int m_requestedUpdateFrequency = 0;
         private Vector3 m_taintPosition;
-
+        internal bool m_avatarplanted = false;
         /// <summary>
         /// Hold set forces so we can process them outside physics calculations.  This prevents race conditions if we set force
         /// while calculatios are going on
@@ -413,7 +413,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 m_iscollidingObj = value;
-                if (value)
+                if (value && !m_avatarplanted)
                     m_pidControllerActive = false;
                 else
                     m_pidControllerActive = true;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 7a50c4c..cbe21e2 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -501,6 +501,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int physics_logging_interval = 0;
         public bool physics_logging_append_existing_logfile = false;
 
+        private bool avplanted = false;
+        private bool av_av_collisions_off = false;
 
         public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
         public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
@@ -644,6 +646,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
                     avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
                     avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
+                    avplanted = physicsconfig.GetBoolean("av_planted", false);
+                    av_av_collisions_off = physicsconfig.GetBoolean("av_av_collisions_off", false);
+
                     IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
 
                     contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
@@ -663,6 +668,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
                     MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
                     m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
+                    
+                    
 
                     if (Environment.OSVersion.Platform == PlatformID.Unix)
                     {
@@ -1309,6 +1316,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
                     skipThisContact = true;   // No collision on volume detect prims
 
+                if (av_av_collisions_off)
+                    if ((p1 is OdeCharacter) && (p2 is OdeCharacter))
+                        skipThisContact = true;
+
                 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
                     skipThisContact = true;   // No collision on volume detect prims
 
@@ -1972,7 +1983,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             newAv.Flying = isFlying;
             newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
-            
+            newAv.m_avatarplanted = avplanted;
+
             return newAv;
         }
 
@@ -1987,6 +1999,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         internal void AddCharacter(OdeCharacter chr)
         {
+            chr.m_avatarplanted = avplanted;
             if (!_characters.Contains(chr))
             {
                 _characters.Add(chr);