9 files changed, 1809 insertions, 666 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index dc0c008..09e1f0c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -41,7 +41,7 @@ public class BSCharacter : PhysicsActor
    private BSScene _scene;
    private String _avName;
-    private bool _stopped;
+    // private bool _stopped;
    private Vector3 _size;
    private Vector3 _scale;
    private PrimitiveBaseShape _pbs;
@@ -134,9 +134,9 @@ public class BSCharacter : PhysicsActor
    {
        base.RequestPhysicsterseUpdate();
    }
+    // No one calls this method so I don't know what it could possibly mean
    public override bool Stopped { 
-        get { return _stopped; } 
+        get { return false; } 
    }
    public override Vector3 Size { 
        get { return _size; } 
@@ -391,52 +391,47 @@ public class BSCharacter : PhysicsActor
        _mass = _density * _avatarVolume;
    }
-    // Set to 'true' if the individual changed items should be checked
-    //   (someday RequestPhysicsTerseUpdate() will take a bitmap of changed properties)
-    const bool SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES = false;
    // The physics engine says that properties have updated. Update same and inform
    // the world that things have changed.
    public void UpdateProperties(EntityProperties entprop)
    {
+        /*
        bool changed = false;
-        if (SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES) {
+        // we assign to the local variables so the normal set action does not happen
-            // we assign to the local variables so the normal set action does not happen
+        if (_position != entprop.Position) {
-            if (_position != entprop.Position) {
-                _position = entprop.Position;
-                changed = true;
-            }
-            if (_orientation != entprop.Rotation) {
-                _orientation = entprop.Rotation;
-                changed = true;
-            }
-            if (_velocity != entprop.Velocity) {
-                _velocity = entprop.Velocity;
-                changed = true;
-            }
-            if (_acceleration != entprop.Acceleration) {
-                _acceleration = entprop.Acceleration;
-                changed = true;
-            }
-            if (_rotationalVelocity != entprop.RotationalVelocity) {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                changed = true;
-            }
-            if (changed) {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Avatar movement is not done by generating this event. There is code in the heartbeat
-                //   loop that updates avatars.
-                // base.RequestPhysicsterseUpdate();
-            }
-        }
-        else {
            _position = entprop.Position;
+            changed = true;
+        }
+        if (_orientation != entprop.Rotation) {
            _orientation = entprop.Rotation;
+            changed = true;
+        }
+        if (_velocity != entprop.Velocity) {
            _velocity = entprop.Velocity;
+            changed = true;
+        }
+        if (_acceleration != entprop.Acceleration) {
            _acceleration = entprop.Acceleration;
+            changed = true;
+        }
+        if (_rotationalVelocity != entprop.RotationalVelocity) {
            _rotationalVelocity = entprop.RotationalVelocity;
+            changed = true;
+        }
+        if (changed) {
+            // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
+            // Avatar movement is not done by generating this event. There is code in the heartbeat
+            //   loop that updates avatars.
            // base.RequestPhysicsterseUpdate();
        }
+        */
+        _position = entprop.Position;
+        _orientation = entprop.Rotation;
+        _velocity = entprop.Velocity;
+        _acceleration = entprop.Acceleration;
+        _rotationalVelocity = entprop.RotationalVelocity;
+        // Avatars don't report theirr changes the usual way. Changes are checked for in the heartbeat loop.
+        // base.RequestPhysicsterseUpdate();
    }
    // Called by the scene when a collision with this object is reported
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
new file mode 100755
index 0000000..fbb9e21
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraint.cs
@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenMetaverse;
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSConstraint : IDisposable
+{
+    private BulletSim m_world;
+    private BulletBody m_body1;
+    private BulletBody m_body2;
+    private BulletConstraint m_constraint;
+    private bool m_enabled = false;
+    public BSConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1, Quaternion frame1rot,
+                    Vector3 frame2, Quaternion frame2rot
+        )
+    {
+        m_world = world;
+        m_body1 = obj1;
+        m_body2 = obj2;
+        m_constraint = new BulletConstraint(BulletSimAPI.CreateConstraint2(m_world.Ptr, m_body1.Ptr, m_body2.Ptr,
+                            frame1, frame1rot,
+                            frame2, frame2rot));
+        m_enabled = true;
+    }
+    public void Dispose()
+    {
+        if (m_enabled)
+        {
+            // BulletSimAPI.RemoveConstraint(m_world.ID, m_body1.ID, m_body2.ID);
+            BulletSimAPI.DestroyConstraint2(m_world.Ptr, m_constraint.Ptr);
+            m_enabled = false;
+        }
+    }
+    public BulletBody Body1 { get { return m_body1; } }
+    public BulletBody Body2 { get { return m_body2; } }
+    public bool SetLinearLimits(Vector3 low, Vector3 high)
+    {
+        bool ret = false;
+        if (m_enabled)
+            ret = BulletSimAPI.SetLinearLimits2(m_constraint.Ptr, low, high);
+        return ret;
+    }
+    public bool SetAngularLimits(Vector3 low, Vector3 high)
+    {
+        bool ret = false;
+        if (m_enabled)
+            ret = BulletSimAPI.SetAngularLimits2(m_constraint.Ptr, low, high);
+        return ret;
+    }
+    public bool UseFrameOffset(bool useOffset)
+    {
+        bool ret = false;
+        float onOff = useOffset ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.UseFrameOffset2(m_constraint.Ptr, onOff);
+        return ret;
+    }
+    public bool TranslationalLimitMotor(bool enable, float targetVelocity, float maxMotorForce)
+    {
+        bool ret = false;
+        float onOff = enable ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse;
+        if (m_enabled)
+            ret = BulletSimAPI.TranslationalLimitMotor2(m_constraint.Ptr, onOff, targetVelocity, maxMotorForce);
+        return ret;
+    }
+    public bool CalculateTransforms()
+    {
+        bool ret = false;
+        if (m_enabled)
+        {
+            BulletSimAPI.CalculateTransforms2(m_constraint.Ptr);
+            ret = true;
+        }
+        return ret;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
new file mode 100755
index 0000000..a2650fb
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
@@ -0,0 +1,178 @@
+/*
+ * 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 log4net;
+using OpenMetaverse;
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSConstraintCollection : IDisposable
+{
+    // private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
+    // private static readonly string LogHeader = "[CONSTRAINT COLLECTION]";
+    delegate bool ConstraintAction(BSConstraint constrain);
+    private List<BSConstraint> m_constraints;
+    private BulletSim m_world;
+    public BSConstraintCollection(BulletSim world)
+    {
+        m_world = world;
+        m_constraints = new List<BSConstraint>();
+    }
+    public void Dispose()
+    {
+        this.Clear();
+    }
+    public void Clear()
+    {
+        foreach (BSConstraint cons in m_constraints)
+        {
+            cons.Dispose();
+        }
+        m_constraints.Clear();
+    }
+    public BSConstraint CreateConstraint(BulletSim world, BulletBody obj1, BulletBody obj2,
+                    Vector3 frame1, Quaternion frame1rot,
+                    Vector3 frame2, Quaternion frame2rot)
+    {
+        BSConstraint constrain = new BSConstraint(world, obj1, obj2, frame1, frame1rot, frame2, frame2rot);
+        this.AddConstraint(constrain);
+        return constrain;
+    }
+    public bool AddConstraint(BSConstraint cons)
+    {
+        // There is only one constraint between any bodies. Remove any old just to make sure.
+        RemoveAndDestroyConstraint(cons.Body1, cons.Body2);
+        m_constraints.Add(cons);
+        return true;
+    }
+    // Get the constraint between two bodies. There can be only one the way we're using them.
+    public bool TryGetConstraint(BulletBody body1, BulletBody body2, out BSConstraint returnConstraint)
+    {
+        bool found = false;
+        BSConstraint foundConstraint = null;
+        uint lookingID1 = body1.ID;
+        uint lookingID2 = body2.ID;
+        ForEachConstraint(delegate(BSConstraint constrain)
+        {
+            if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2)
+                || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1))
+            {
+                foundConstraint = constrain;
+                found = true;
+            }
+            return found;
+        });
+        returnConstraint = foundConstraint;
+        return found;
+    }
+    public bool RemoveAndDestroyConstraint(BulletBody body1, BulletBody body2)
+    {
+        // return BulletSimAPI.RemoveConstraint(m_world.ID, obj1.ID, obj2.ID);
+        bool ret = false;
+        BSConstraint constrain;
+        if (this.TryGetConstraint(body1, body2, out constrain))
+        {
+            // 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;
+    }
+    public bool RemoveAndDestroyConstraint(BulletBody body1)
+    {
+        // return BulletSimAPI.RemoveConstraintByID(m_world.ID, obj.ID);
+        List<BSConstraint> toRemove = new List<BSConstraint>();
+        uint lookingID = body1.ID;
+        ForEachConstraint(delegate(BSConstraint 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);
+                constrain.Dispose();
+            }
+        }
+        return (toRemove.Count > 0);
+    }
+    public bool RecalculateAllConstraints()
+    {
+        foreach (BSConstraint constrain in m_constraints)
+        {
+            constrain.CalculateTransforms();
+        }
+        return true;
+    }
+    // Lock the constraint list and loop through it.
+    // The constraint action returns 'true' if it wants the loop aborted.
+    private void ForEachConstraint(ConstraintAction action)
+    {
+        lock (m_constraints)
+        {
+            foreach (BSConstraint constrain in m_constraints)
+            {
+                if (action(constrain))
+                    break;
+            }
+        }
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index eb20eb3..c197e61 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -57,7 +57,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        private int frcount = 0;                                        // Used to limit dynamics debug output to
                                                                        // every 100th frame
-        // private BSScene m_parentScene = null;
        private BSPrim m_prim;      // the prim this dynamic controller belongs to
        // Vehicle properties
@@ -131,8 +130,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            m_type = Vehicle.TYPE_NONE;
        }
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
        {
+            DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -229,8 +229,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            }
        }//end ProcessFloatVehicleParam
-        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
        {
+            DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -265,6 +266,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
        {
+            DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.REFERENCE_FRAME:
@@ -278,6 +280,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessVehicleFlags(int pParam, bool remove)
        {
+            DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
            if (remove)
            {
                if (pParam == -1)
@@ -434,6 +437,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessTypeChange(Vehicle pType)
        {
+            DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
            // Set Defaults For Type
            m_type = pType;
            switch (pType)
@@ -594,11 +598,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    break;
            }
        }//end SetDefaultsForType
-        internal void Step(float pTimestep,  BSScene pParentScene)
+        internal void Step(float pTimestep)
        {
            if (m_type == Vehicle.TYPE_NONE) return;
@@ -606,21 +609,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            if (frcount > 100)
                frcount = 0;
-            MoveLinear(pTimestep, pParentScene);
+            MoveLinear(pTimestep);
            MoveAngular(pTimestep);
            LimitRotation(pTimestep);
+            DetailLog("{0},Dynamics,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
-        private void MoveLinear(float pTimestep, BSScene _pParentScene)
+        private void MoveLinear(float pTimestep)
        {
-            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
+            // requested m_linearMotorDirection is significant
+            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
+            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
            {
+                Vector3 origDir = m_linearMotorDirection;
+                Vector3 origVel = m_lastLinearVelocityVector;
                // add drive to body
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                m_lastLinearVelocityVector += (addAmount*10);  // lastLinearVelocityVector is the current body velocity vector?
+                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
+                // lastLinearVelocityVector is the current body velocity vector?
+                // RA: Not sure what the *10 is for. A correction for pTimestep?
+                // m_lastLinearVelocityVector += (addAmount*10);  
+                m_lastLinearVelocityVector += addAmount;  
                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
+                // Limit the velocity vector to less than the last set linear motor direction
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
@@ -630,76 +646,93 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
-                //Console.WriteLine("decay: " + decayfraction);
                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-                //Console.WriteLine("actual: " + m_linearMotorDirection);
+                /*
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
+                m_lastLinearVelocityVector += addAmount;
+                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                m_linearMotorDirection *= decayfraction;
+                 */
+                DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
+                    m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
            }
            else
-            {        // requested is not significant
+            {
-                    // if what remains of applied is small, zero it.
+                // if what remains of applied is small, zero it.
-                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
+                // if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
-                    m_lastLinearVelocityVector = Vector3.Zero;
+                //     m_lastLinearVelocityVector = Vector3.Zero;
+                m_linearMotorDirection = Vector3.Zero;
+                m_lastLinearVelocityVector = Vector3.Zero;
            }
            // convert requested object velocity to world-referenced vector
-            m_dir = m_lastLinearVelocityVector;
+            Quaternion rotq = m_prim.Orientation;
-            Quaternion rot = m_prim.Orientation;
+            m_dir = m_lastLinearVelocityVector * rotq;
-            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
-            m_dir *= rotq;                            // apply obj rotation to velocity vector
+            // Add the various forces into m_dir which will be our new direction vector (velocity)
-            // add Gravity andBuoyancy
+            // add Gravity and Buoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;
-            // There is some gravity, make a gravity force vector
+            // There is some gravity, make a gravity force vector that is applied after object velocity.
-            // that is applied after object velocity.
-            float objMass = m_prim.Mass;
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy);
+            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
            // Preserve the current Z velocity
            Vector3 vel_now = m_prim.Velocity;
            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
            Vector3 pos = m_prim.Position;
+            Vector3 posChange = pos;
 //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
-            Vector3 posChange = new Vector3();
-            posChange.X = pos.X - m_lastPositionVector.X;
-            posChange.Y = pos.Y - m_lastPositionVector.Y;
-            posChange.Z = pos.Z - m_lastPositionVector.Z;
            double Zchange = Math.Abs(posChange.Z);
            if (m_BlockingEndPoint != Vector3.Zero)
            {
+                bool changed = false;
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= posChange.Y + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= posChange.Z + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.X <= 0)
                {
                    pos.X += posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Y <= 0)
                {
                    pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
                    m_prim.Position = pos;
+                    DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
                }
            }
-            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
+            // If below the terrain, move us above the ground a little.
+            if (pos.Z < m_prim.Scene.GetTerrainHeightAtXYZ(pos))
            {
-                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
+                pos.Z = m_prim.Scene.GetTerrainHeightAtXYZ(pos) + 2;
                m_prim.Position = pos;
+                DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
            }
            // Check if hovering
@@ -708,11 +741,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
-                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
-                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
@@ -746,6 +779,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    }
                }
+                DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
 //                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
 //                m_VhoverTimescale = 0f;        // time to acheive height
 //                pTimestep  is time since last frame,in secs
@@ -774,12 +809,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                {
                    grav.Z = (float)(grav.Z * 1.125);
                }
-                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float terraintemp = m_prim.Scene.GetTerrainHeightAtXYZ(pos);
                float postemp = (pos.Z - terraintemp);
                if (postemp > 2.5f)
                {
                    grav.Z = (float)(grav.Z * 1.037125);
                }
+                DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
                //End Experimental Values
            }
            if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -800,32 +836,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            // Apply velocity
            m_prim.Velocity = m_dir;
            // apply gravity force
-            m_prim.Force = grav;
+            // Why is this set here? The physics engine already does gravity.
+            // m_prim.AddForce(grav, false);
+            // m_prim.Force = grav;
-            // apply friction
+            // Apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
+                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
        } // end MoveLinear()
        private void MoveAngular(float pTimestep)
        {
-            /*
+            // m_angularMotorDirection         // angular velocity requested by LSL motor
-            private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
+            // m_angularMotorApply             // application frame counter
-            private int m_angularMotorApply = 0;                            // application frame counter
+            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-             private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
+            // m_angularMotorTimescale         // motor angular velocity ramp up rate
-            private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
+            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
-            private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
+            // m_angularFrictionTimescale      // body angular velocity  decay rate
-            private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
+            // m_lastAngularVelocity           // what was last applied to body
-            private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
-            */
            // Get what the body is doing, this includes 'external' influences
            Vector3 angularVelocity = m_prim.RotationalVelocity;
-   //         Vector3 angularVelocity = Vector3.Zero;
            if (m_angularMotorApply > 0)
            {
+                // Rather than snapping the angular motor velocity from the old value to
+                //    a newly set velocity, this routine steps the value from the previous
+                //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
+                // There are m_angularMotorApply steps.
+                Vector3 origAngularVelocity = m_angularMotorVelocity;
                // ramp up to new value
                //   current velocity  +=                         error                       /    (time to get there / step interval)
                //                               requested speed            -  last motor speed
@@ -833,23 +876,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
+                DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}", 
+                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                                            // velocity may still be acheived.
            }
            else
            {
-                // no motor recently applied, keep the body velocity
+                // No motor recently applied, keep the body velocity
-        /*        m_angularMotorVelocity.X = angularVelocity.X;
-                m_angularMotorVelocity.Y = angularVelocity.Y;
-                m_angularMotorVelocity.Z = angularVelocity.Z; */
                // and decay the velocity
                m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
            } // end motor section
            // Vertical attractor section
            Vector3 vertattr = Vector3.Zero;
            if (m_verticalAttractionTimescale < 300)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
@@ -871,7 +912,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // Error is 0 (no error) to +/- 2 (max error)
                // scale it by VAservo
                verterr = verterr * VAservo;
-//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
@@ -884,11 +924,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                vertattr.X += bounce * angularVelocity.X;
                vertattr.Y += bounce * angularVelocity.Y;
+                DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
+                            m_prim.LocalID, verterr, bounce, vertattr);
            } // else vertical attractor is off
-    //        m_lastVertAttractor = vertattr;
+            // m_lastVertAttractor = vertattr;
            // Bank section tba
            // Deflection section tba
            // Sum velocities
@@ -898,11 +942,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
+                DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
            }
            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
            }
             // apply friction
@@ -912,10 +958,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            // Apply to the body
            m_prim.RotationalVelocity = m_lastAngularVelocity;
+            DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
        } //end MoveAngular
        internal void LimitRotation(float timestep)
        {
-            Quaternion rotq = m_prim.Orientation;    // rotq = rotation of object
+            Quaternion rotq = m_prim.Orientation;
            Quaternion m_rot = rotq;
            bool changed = false;
            if (m_RollreferenceFrame != Quaternion.Identity)
@@ -923,18 +971,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                if (rotq.X >= m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
+                    changed = true;
                }
                if (rotq.Y >= m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                }
                if (rotq.X <= -m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
+                    changed = true;
                }
                if (rotq.Y <= -m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                }
                changed = true;
            }
@@ -944,8 +996,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                m_rot.Y = 0;
                changed = true;
            }
+            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
+            {
+                m_rot.X = 0;
+                m_rot.Y = 0;
+                changed = true;
+            }
            if (changed)
                m_prim.Orientation = m_rot;
+            DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
+        }
+        // Invoke the detailed logger and output something if it's enabled.
+        private void DetailLog(string msg, params Object[] args)
+        {
+            if (m_prim.Scene.VehicleLoggingEnabled)
+                m_prim.Scene.PhysicsLogging.Write(msg, args);
        }
    }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
new file mode 100755
index 0000000..3bc2100
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -0,0 +1,308 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OMV = OpenMetaverse;
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSLinkset
+{
+    private static string LogHeader = "[BULLETSIM LINKSET]";
+    private BSPrim m_linksetRoot;
+    public BSPrim Root { get { return m_linksetRoot; } }
+    private BSScene m_scene;
+    private List<BSPrim> m_children;
+    // We lock the diddling of linkset classes to prevent any badness.
+    // This locks the modification of the instances of this class. Changes
+    //    to the physical representation is done via the tainting mechenism.
+    private object m_linksetActivityLock = new Object();
+    // We keep the prim's mass in the linkset structure since it could be dependent on other prims
+    private float m_mass;
+    public float LinksetMass 
+    { 
+        get 
+        {
+            m_mass = ComputeLinksetMass();
+            return m_mass;
+        }
+    }
+    public OMV.Vector3 CenterOfMass
+    {
+        get { return ComputeLinksetCenterOfMass(); }
+    }
+    public OMV.Vector3 GeometricCenter
+    {
+        get { return ComputeLinksetGeometricCenter(); }
+    }
+    public BSLinkset(BSScene scene, BSPrim parent)
+    {
+        // A simple linkset of one (no children)
+        m_scene = scene;
+        m_linksetRoot = parent;
+        m_children = new List<BSPrim>();
+        m_mass = parent.MassRaw;
+    }
+    // Link to a linkset where the child knows the parent.
+    // Parent changing should not happen so do some sanity checking.
+    // We return the parent's linkset so the child can track it's membership.
+    public BSLinkset AddMeToLinkset(BSPrim child, BSPrim parent)
+    {
+        lock (m_linksetActivityLock)
+        {
+            parent.Linkset.AddChildToLinkset(child);
+        }
+        return parent.Linkset;
+    }
+    public BSLinkset RemoveMeFromLinkset(BSPrim child)
+    {
+        lock (m_linksetActivityLock)
+        {
+            if (IsRoot(child))
+            {
+                // if root of linkset, take the linkset apart
+                while (m_children.Count > 0)
+                {
+                    // Note that we don't do a foreach because the remove routine
+                    //    takes it out of the list.
+                    RemoveChildFromLinkset(m_children[0]);
+                }
+                m_children.Clear(); // just to make sure
+            }
+            else
+            {
+                // Just removing a child from an existing linkset
+                RemoveChildFromLinkset(child);
+            }
+        }
+        // The child is down to a linkset of just itself
+        return new BSLinkset(m_scene, child);
+    }
+    // An existing linkset had one of its members rebuilt or something.
+    // Undo all the physical linking and rebuild the physical linkset.
+    public bool RefreshLinkset(BSPrim requestor)
+    {
+        return true;
+    }
+    // Return 'true' if the passed object is the root object of this linkset
+    public bool IsRoot(BSPrim requestor)
+    {
+        return (requestor.LocalID == m_linksetRoot.LocalID);
+    }
+    // Return 'true' if this linkset has any children (more than the root member)
+    public bool HasAnyChildren { get { return (m_children.Count > 0); } }
+    // Return 'true' if this child is in this linkset
+    public bool HasChild(BSPrim child)
+    {
+        bool ret = false;
+        foreach (BSPrim bp in m_children)
+        {
+            if (child.LocalID == bp.LocalID)
+            {
+                ret = true;
+                break;
+            }
+        }
+        return ret;
+    }
+    private float ComputeLinksetMass()
+    {
+        float mass = m_linksetRoot.MassRaw;
+        foreach (BSPrim bp in m_children)
+        {
+            mass += bp.MassRaw;
+        }
+        return mass;
+    }
+    private OMV.Vector3 ComputeLinksetCenterOfMass()
+    {
+        OMV.Vector3 com = m_linksetRoot.Position * m_linksetRoot.MassRaw;
+        float totalMass = m_linksetRoot.MassRaw;
+        foreach (BSPrim bp in m_children)
+        {
+            com += bp.Position * bp.MassRaw;
+            totalMass += bp.MassRaw;
+        }
+        com /= totalMass;
+        return com;
+    }
+    private OMV.Vector3 ComputeLinksetGeometricCenter()
+    {
+        OMV.Vector3 com = m_linksetRoot.Position;
+        foreach (BSPrim bp in m_children)
+        {
+            com += bp.Position * bp.MassRaw;
+        }
+        com /= m_children.Count + 1;
+        return com;
+    }
+    // I am the root of a linkset and a new child is being added
+    public void AddChildToLinkset(BSPrim pchild)
+    {
+        BSPrim child = pchild;
+        if (!HasChild(child))
+        {
+            m_children.Add(child);
+            m_scene.TaintedObject(delegate()
+            {
+                DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
+                DetailLog("{0},AddChildToLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
+                PhysicallyLinkAChildToRoot(pchild);     // build the physical binding between me and the child
+            });
+        }
+        return;
+    }
+    // I am the root of a linkset and one of my children is being removed.
+    // Safe to call even if the child is not really in my linkset.
+    public void RemoveChildFromLinkset(BSPrim pchild)
+    {
+        BSPrim child = pchild;
+        if (m_children.Remove(child))
+        {
+            m_scene.TaintedObject(delegate()
+            {
+                DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                DetailLog("{0},RemoveChildFromLinkset,child={1}", m_linksetRoot.LocalID, pchild.LocalID);
+                if (m_children.Count == 0)
+                {
+                    // if the linkset is empty, make sure all linkages have been removed
+                    PhysicallyUnlinkAllChildrenFromRoot();
+                }
+                else
+                {
+                    PhysicallyUnlinkAChildFromRoot(pchild);
+                }
+            });
+        }
+        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);
+        }
+        return;
+    }
+    // Create a constraint between me (root of linkset) and the passed prim (the child).
+    // Called at taint time!
+    private void PhysicallyLinkAChildToRoot(BSPrim childPrim)
+    {
+        // Zero motion for children so they don't interpolate
+        childPrim.ZeroMotion();
+        // relative position normalized to the root prim
+        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(m_linksetRoot.Orientation);
+        OMV.Vector3 childRelativePosition = (childPrim.Position - m_linksetRoot.Position) * invThisOrientation;
+        // relative rotation of the child to the parent
+        OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
+        DetailLog("{0},LinkAChildToMe,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
+        BSConstraint constrain = m_scene.Constraints.CreateConstraint(
+                        m_scene.World, m_linksetRoot.Body, childPrim.Body,
+                        childRelativePosition,
+                        childRelativeRotation,
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Identity);
+        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);
+    }
+    // Remove linkage between myself and a particular child
+    // Called at taint time!
+    private void PhysicallyUnlinkAChildFromRoot(BSPrim childPrim)
+    {
+        DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
+                    LogHeader, m_linksetRoot.LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, childPrim.LocalID);
+        // BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, childPrim.LocalID);
+        m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body, childPrim.Body);
+    }
+    // Remove linkage between myself and any possible children I might have
+    // Called at taint time!
+    private void PhysicallyUnlinkAllChildrenFromRoot()
+    {
+        // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
+        DetailLog("{0},PhysicallyUnlinkAllChildren,taint", m_linksetRoot.LocalID);
+        m_scene.Constraints.RemoveAndDestroyConstraint(m_linksetRoot.Body);
+        // BulletSimAPI.RemoveConstraintByID(_scene.WorldID, LocalID);
+    }
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DebugLog(string msg, params Object[] args)
+    {
+        m_scene.Logger.DebugFormat(msg, args);
+    }
+    // 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);
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 130f1ca..7590d93 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -42,6 +42,8 @@ public sealed class BSPrim : PhysicsActor
    private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
    private static readonly string LogHeader = "[BULLETS PRIM]";
+    private void DebugLog(string mm, params Object[] xx) { if (_scene.shouldDebugLog) m_log.DebugFormat(mm, xx); }
    private IMesh _mesh;
    private PrimitiveBaseShape _pbs;
    private ShapeData.PhysicsShapeType _shapeType;
@@ -50,6 +52,7 @@ public sealed class BSPrim : PhysicsActor
    private List<ConvexResult> _hulls;
    private BSScene _scene;
+    public BSScene Scene { get { return _scene; } }
    private String _avName;
    private uint _localID = 0;
@@ -63,7 +66,7 @@ public sealed class BSPrim : PhysicsActor
    private bool _isSelected;
    private bool _isVolumeDetect;
    private OMV.Vector3 _position;
-    private float _mass;
+    private float _mass;    // the mass of this object
    private float _density;
    private OMV.Vector3 _force;
    private OMV.Vector3 _velocity;
@@ -86,14 +89,22 @@ public sealed class BSPrim : PhysicsActor
    private bool _kinematic;
    private float _buoyancy;
-    private List<BSPrim> _childrenPrims;
+    // Membership in a linkset is controlled by this class.
-    private BSPrim _parentPrim;
+    private BSLinkset _linkset;
+    public BSLinkset Linkset
+    {
+        get { return _linkset; }
+        set { _linkset = value; }
+    }
    private int _subscribedEventsMs = 0;
    private int _nextCollisionOkTime = 0;
    long _collidingStep;
    long _collidingGroundStep;
+    private BulletBody m_body;
+    public BulletBody Body { get { return m_body; } }
    private BSDynamics _vehicle;
    private OMV.Vector3 _PIDTarget;
@@ -127,17 +138,18 @@ public sealed class BSPrim : PhysicsActor
        _friction = _scene.Params.defaultFriction; // TODO: compute based on object material
        _density = _scene.Params.defaultDensity; // TODO: compute based on object material
        _restitution = _scene.Params.defaultRestitution;
-        _parentPrim = null;     // not a child or a parent
+        _linkset = new BSLinkset(_scene, this);     // a linkset of one
        _vehicle = new BSDynamics(this);    // add vehicleness
-        _childrenPrims = new List<BSPrim>();
+        _mass = CalculateMass();
-        if (_isPhysical)
-            _mass = CalculateMass();
-        else
-            _mass = 0f;
        // do the actual object creation at taint time
        _scene.TaintedObject(delegate()
        {
            RecreateGeomAndObject();
+            // Get the pointer to the physical body for this object.
+            // At the moment, we're still letting BulletSim manage the creation and destruction
+            //    of the object. Someday we'll move that into the C# code.
+            m_body = new BulletBody(LocalID, BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID));
        });
    }
@@ -145,13 +157,19 @@ public sealed class BSPrim : PhysicsActor
    public void Destroy()
    {
        // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
+        // DetailLog("{0},Destroy", LocalID);
        // Undo any vehicle properties
        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
        _scene.RemoveVehiclePrim(this);     // just to make sure
        _scene.TaintedObject(delegate()
        {
+            // Undo any links between me and any other object
+            _linkset = _linkset.RemoveMeFromLinkset(this);
            // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
-            BulletSimAPI.DestroyObject(_scene.WorldID, _localID);
+            BulletSimAPI.DestroyObject(_scene.WorldID, LocalID);
        });
    }
    
@@ -164,8 +182,8 @@ public sealed class BSPrim : PhysicsActor
            _size = value;
            _scene.TaintedObject(delegate()
            {
-                if (_isPhysical) _mass = CalculateMass();   // changing size changes the mass
+                _mass = CalculateMass();   // changing size changes the mass
-                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, _mass, _isPhysical);
+                BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, (IsPhysical ? _mass : 0f), IsPhysical);
                RecreateGeomAndObject();
            });
        } 
@@ -175,7 +193,7 @@ public sealed class BSPrim : PhysicsActor
            _pbs = value;
            _scene.TaintedObject(delegate()
            {
-                if (_isPhysical) _mass = CalculateMass();   // changing the shape changes the mass
+                _mass = CalculateMass();   // changing the shape changes the mass
                RecreateGeomAndObject();
            });
        } 
@@ -202,33 +220,10 @@ public sealed class BSPrim : PhysicsActor
    // link me to the specified parent
    public override void link(PhysicsActor obj) {
        BSPrim parent = obj as BSPrim;
-        // m_log.DebugFormat("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
+        DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
-        // TODO: decide if this parent checking needs to happen at taint time
+        DetailLog("{0},link,parent={1}", LocalID, obj.LocalID);
-        if (_parentPrim == null)
-        {
+        _linkset = _linkset.AddMeToLinkset(this, parent);
-            if (parent != null)
-            {
-                // I don't have a parent so I am joining a linkset
-                parent.AddChildToLinkset(this);
-            }
-        }
-        else
-        {
-            // I already have a parent, is parenting changing?
-            if (parent != _parentPrim)
-            {
-                if (parent == null)
-                {
-                    // we are being removed from a linkset
-                    _parentPrim.RemoveChildFromLinkset(this);
-                }
-                else
-                {
-                    // asking to reparent a prim should not happen
-                    m_log.ErrorFormat("{0}: Reparenting a prim. ", LogHeader);
-                }
-            }
-        }
        return; 
    }
@@ -236,101 +231,92 @@ public sealed class BSPrim : PhysicsActor
    public override void delink() {
        // 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
-        // m_log.DebugFormat("{0}: delink {1}/{2}", LogHeader, _avName, _localID);
+        DebugLog("{0}: delink {1}/{2}. Parent={3}", LogHeader, _avName, _localID, 
-        if (_parentPrim != null)
+                            _linkset.Root._avName+"/"+_linkset.Root.LocalID.ToString());
-        {
+        DetailLog("{0},delink,parent={1}", LocalID, _linkset.Root.LocalID.ToString());
-            _parentPrim.RemoveChildFromLinkset(this);
-        }
-        return; 
-    }
-    // I am the root of a linkset and a new child is being added
-    public void AddChildToLinkset(BSPrim pchild)
-    {
-        BSPrim child = pchild;
-        _scene.TaintedObject(delegate()
-        {
-            if (!_childrenPrims.Contains(child))
-            {
-                _childrenPrims.Add(child);
-                child.ParentPrim = this;    // the child has gained a parent
-                RecreateGeomAndObject();    // rebuild my shape with the new child added
-            }
-        });
-        return;
-    }
-    // I am the root of a linkset and one of my children is being removed.
-    // Safe to call even if the child is not really in my linkset.
-    public void RemoveChildFromLinkset(BSPrim pchild)
-    {
-        BSPrim child = pchild;
-        _scene.TaintedObject(delegate()
-        {
-            if (_childrenPrims.Contains(child))
-            {
-                BulletSimAPI.RemoveConstraint(_scene.WorldID, child.LocalID, this.LocalID);
-                _childrenPrims.Remove(child);
-                child.ParentPrim = null;    // the child has lost its parent
-                RecreateGeomAndObject();    // rebuild my shape with the child removed
-            }
-            else
-            {
-                m_log.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset");
-            }
-        });
-        return;
-    }
-    public BSPrim ParentPrim
-    {
-        set { _parentPrim = value; }
-    }
-    // return true if we are the root of a linkset (there are children to manage)
+        _linkset.RemoveMeFromLinkset(this);
-    public bool IsRootOfLinkset
+        return; 
-    {
-        get { return (_parentPrim == null && _childrenPrims.Count != 0); }
    }
    // Set motion values to zero.
    // Do it to the properties so the values get set in the physics engine.
    // Push the setting of the values to the viewer.
-    private void ZeroMotion()
+    // Called at taint time!
+    public void ZeroMotion()
    {
-        Velocity = OMV.Vector3.Zero;
+        _velocity = OMV.Vector3.Zero;
        _acceleration = OMV.Vector3.Zero;
-        RotationalVelocity = OMV.Vector3.Zero;
+        _rotationalVelocity = OMV.Vector3.Zero;
-        base.RequestPhysicsterseUpdate();
+        // Zero some other properties directly into the physics engine
+        BulletSimAPI.SetVelocity2(Body.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetAngularVelocity2(Body.Ptr, OMV.Vector3.Zero);
+        BulletSimAPI.SetInterpolation2(Body.Ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
+        BulletSimAPI.ClearForces2(Body.Ptr);
    }
-    public override void LockAngularMotion(OMV.Vector3 axis) { return; }
+    public override void LockAngularMotion(OMV.Vector3 axis)
+    { 
+        DetailLog("{0},LockAngularMotion,call,axis={1}", LocalID, axis);
+        return;
+    }
    public override OMV.Vector3 Position { 
        get { 
-            // don't do the following GetObjectPosition because this function is called a zillion times
+            if (!_linkset.IsRoot(this))
+                // child prims move around based on their parent. Need to get the latest location
+                _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+            // don't do the GetObjectPosition for root elements because this function is called a zillion times
            // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
            return _position; 
        } 
        set {
            _position = value;
+            // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
-                // m_log.DebugFormat("{0}: setPosition: id={1}, position={2}", LogHeader, _localID, _position);
            });
        } 
    }
-    public override float Mass { 
-        get { return _mass; } 
+    // Return the effective mass of the object.
+    // If there are multiple items in the linkset, add them together for the root
+    public override float Mass
+    { 
+        get
+        {
+            return _linkset.LinksetMass;
+        }
+    }
+    // used when we only want this prim's mass and not the linkset thing
+    public float MassRaw { get { return _mass; } }
+    // Is this used?
+    public override OMV.Vector3 CenterOfMass
+    {
+        get { return _linkset.CenterOfMass; }
    }
+    // Is this used?
+    public override OMV.Vector3 GeometricCenter
+    {
+        get { return _linkset.GeometricCenter; }
+    }
    public override OMV.Vector3 Force { 
        get { return _force; } 
        set {
            _force = value;
            _scene.TaintedObject(delegate()
            {
-                BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                DetailLog("{0},SetForce,taint,force={1}", LocalID, _force);
+                // BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+                BulletSimAPI.SetObjectForce2(Body.Ptr, _force);
            });
        } 
    }
@@ -341,15 +327,22 @@ public sealed class BSPrim : PhysicsActor
        } 
        set {
            Vehicle type = (Vehicle)value;
-            _vehicle.ProcessTypeChange(type);
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetVehicleType,taint,type={1}", LocalID, type);
+                _vehicle.ProcessTypeChange(type);
                if (type == Vehicle.TYPE_NONE)
                {
                    _scene.RemoveVehiclePrim(this);
                }
                else
                {
+                    _scene.TaintedObject(delegate()
+                    {
+                        // Tell the physics engine to clear state
+                        BulletSimAPI.ClearForces2(this.Body.Ptr);
+                    });
                    // make it so the scene will call us each tick to do vehicle things
                    _scene.AddVehiclePrim(this);
                }
@@ -359,47 +352,59 @@ public sealed class BSPrim : PhysicsActor
    }
    public override void VehicleFloatParam(int param, float value) 
    {
-        _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
    }
    public override void VehicleVectorParam(int param, OMV.Vector3 value) 
    {
-        _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
    }
    public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
    {
-        _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        });
    }
    public override void VehicleFlags(int param, bool remove) 
    {
-        _vehicle.ProcessVehicleFlags(param, remove);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVehicleFlags(param, remove);
+        });
    }
-    // Called each simulation step to advance vehicle characteristics
+    // Called each simulation step to advance vehicle characteristics.
+    // Called from Scene when doing simulation step so we're in taint processing time.
    public void StepVehicle(float timeStep)
    {
-        _vehicle.Step(timeStep, _scene);
+        if (IsPhysical)
+            _vehicle.Step(timeStep);
    }
    // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
    public override void SetVolumeDetect(int param) {
        bool newValue = (param != 0);
-        if (_isVolumeDetect != newValue)
+        _isVolumeDetect = newValue;
+        _scene.TaintedObject(delegate()
        {
-            _isVolumeDetect = newValue;
+            SetObjectDynamic();
-            _scene.TaintedObject(delegate()
+        });
-            {
-                SetObjectDynamic();
-            });
-        }
        return; 
    }
-    public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } }
-    public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
    public override OMV.Vector3 Velocity { 
        get { return _velocity; } 
-        set { _velocity = value; 
+        set {
+            _velocity = value;
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetVelocity,taint,vel={1}", LocalID, _velocity);
                BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
            });
        } 
@@ -407,6 +412,7 @@ public sealed class BSPrim : PhysicsActor
    public override OMV.Vector3 Torque { 
        get { return _torque; } 
        set { _torque = value; 
+            DetailLog("{0},SetTorque,call,torque={1}", LocalID, _torque);
        } 
    }
    public override float CollisionScore { 
@@ -419,13 +425,21 @@ public sealed class BSPrim : PhysicsActor
        set { _acceleration = value; }
    }
    public override OMV.Quaternion Orientation { 
-        get { return _orientation; } 
+        get {
+            if (!_linkset.IsRoot(this))
+            {
+                // Children move around because tied to parent. Get a fresh value.
+                _orientation = BulletSimAPI.GetObjectOrientation(_scene.WorldID, LocalID);
+            }
+            return _orientation;
+        } 
        set {
            _orientation = value;
-            // m_log.DebugFormat("{0}: set orientation: id={1}, ori={2}", LogHeader, LocalID, _orientation);
+            // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
            _scene.TaintedObject(delegate()
            {
                // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+                DetailLog("{0},SetOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
            });
        } 
@@ -458,25 +472,22 @@ public sealed class BSPrim : PhysicsActor
        get { return !IsPhantom && !_isVolumeDetect; }
    }
-    // make gravity work if the object is physical and not selected
+    // Make gravity work if the object is physical and not selected
-    // no locking here because only called when it is safe
+    // No locking here because only called when it is safe
+    // Only called at taint time so it is save to call into Bullet.
    private void SetObjectDynamic()
    {
        // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}", LogHeader, _localID, IsStatic, IsSolid);
-        // non-physical things work best with a mass of zero
-        if (IsStatic)
-        {
-            _mass = 0f;
-        }
-        else
-        {
-            _mass = CalculateMass();
-            // If it's dynamic, make sure the hull has been created for it
-            // This shouldn't do much work if the object had previously been built
-            RecreateGeomAndObject();
-        }
+        RecreateGeomAndObject();
-        BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), _mass);
+        float mass = _mass;
+        // Bullet wants static objects have a mass of zero
+        if (IsStatic) 
+            mass = 0f;
+        DetailLog("{0},SetObjectDynamic,taint,static={1},solid={2},mass={3}", LocalID, IsStatic, IsSolid, mass);
+        BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), mass);
    }
    // prims don't fly
@@ -516,11 +527,24 @@ public sealed class BSPrim : PhysicsActor
        set { _floatOnWater = value; } 
    }
    public override OMV.Vector3 RotationalVelocity { 
-        get { return _rotationalVelocity; } 
+        get {
-        set { _rotationalVelocity = value; 
+            /*
+            OMV.Vector3 pv = OMV.Vector3.Zero;
+            // if close to zero, report zero
+            // This is copied from ODE but I'm not sure why it returns zero but doesn't
+            //    zero the property in the physics engine.
+            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
+                return pv;
+             */
+            return _rotationalVelocity;
+        } 
+        set {
+            _rotationalVelocity = value;
            // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
            });
        } 
@@ -533,11 +557,13 @@ public sealed class BSPrim : PhysicsActor
    }
    public override float Buoyancy { 
        get { return _buoyancy; } 
-        set { _buoyancy = value;
+        set {
-        _scene.TaintedObject(delegate()
+            _buoyancy = value;
-        {
+            _scene.TaintedObject(delegate()
-            BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+            {
-        });
+                DetailLog("{0},SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+            });
        } 
    }
@@ -573,27 +599,45 @@ public sealed class BSPrim : PhysicsActor
    public override float APIDStrength { set { return; } }
    public override float APIDDamping { set { return; } }
+    private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
    public override void AddForce(OMV.Vector3 force, bool pushforce) {
        if (force.IsFinite())
        {
-            _force.X += force.X;
+            // _force += force;
-            _force.Y += force.Y;
+            lock (m_accumulatedForces)
-            _force.Z += force.Z;
+                m_accumulatedForces.Add(new OMV.Vector3(force));
        }
        else
        {
            m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
+            return;
        }
        _scene.TaintedObject(delegate()
        {
-            BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+            lock (m_accumulatedForces)
+            {
+                if (m_accumulatedForces.Count > 0)
+                {
+                    OMV.Vector3 fSum = OMV.Vector3.Zero;
+                    foreach (OMV.Vector3 v in m_accumulatedForces)
+                    {
+                        fSum += v;
+                    }
+                    m_accumulatedForces.Clear();
+                    DetailLog("{0},SetObjectForce,taint,force={1}", LocalID, fSum);
+                    BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, fSum);
+                }
+            }
        });
    }
    public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { 
+        DetailLog("{0},AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
        // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
    }
    public override void SetMomentum(OMV.Vector3 momentum) { 
+        DetailLog("{0},SetMomentum,call,mom={1}", LocalID, momentum);
    }
    public override void SubscribeEvents(int ms) { 
        _subscribedEventsMs = ms;
@@ -885,6 +929,9 @@ public sealed class BSPrim : PhysicsActor
        returnMass = _density * volume;
+        /*
+         * This change means each object keeps its own mass and the Mass property
+         * will return the sum if we're part of a linkset.
        if (IsRootOfLinkset)
        {
            foreach (BSPrim prim in _childrenPrims)
@@ -892,6 +939,7 @@ public sealed class BSPrim : PhysicsActor
                returnMass += prim.CalculateMass();
            }
        }
+         */
        if (returnMass <= 0)
            returnMass = 0.0001f;
@@ -907,9 +955,11 @@ public sealed class BSPrim : PhysicsActor
    // The objects needs a hull if it's physical otherwise a mesh is enough
    // No locking here because this is done when we know physics is not simulating
    // if 'forceRebuild' is true, the geometry is rebuilt. Otherwise a previously built version is used
-    private void CreateGeom(bool forceRebuild)
+    // Returns 'true' if the geometry was rebuilt
+    private bool CreateGeom(bool forceRebuild)
    {
        // the mesher thought this was too simple to mesh. Use a native Bullet collision shape.
+        bool ret = false;
        if (!_scene.NeedsMeshing(_pbs))
        {
            if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
@@ -917,16 +967,26 @@ public sealed class BSPrim : PhysicsActor
                if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
                {
                    // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
-                    _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                    if (_shapeType != ShapeData.PhysicsShapeType.SHAPE_SPHERE)
-                    // Bullet native objects are scaled by the Bullet engine so pass the size in
+                    {
-                    _scale = _size;
+                        DetailLog("{0},CreateGeom,sphere", LocalID);
+                        _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                        ret = true;
+                        // Bullet native objects are scaled by the Bullet engine so pass the size in
+                        _scale = _size;
+                    }
                }
            }
            else
            {
                // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, size={2}", LogHeader, LocalID, _size);
-                _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
+                if (_shapeType != ShapeData.PhysicsShapeType.SHAPE_BOX)
-                _scale = _size;
+                {
+                    DetailLog("{0},CreateGeom,box", LocalID);
+                    _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
+                    ret = true;
+                    _scale = _size;
+                }
            }
        }
        else
@@ -938,6 +998,7 @@ public sealed class BSPrim : PhysicsActor
                    // physical objects require a hull for interaction.
                    // This will create the mesh if it doesn't already exist
                    CreateGeomHull();
+                    ret = true;
                }
            }
            else
@@ -946,9 +1007,11 @@ public sealed class BSPrim : PhysicsActor
                {
                    // Static (non-physical) objects only need a mesh for bumping into
                    CreateGeomMesh();
+                    ret = true;
                }
            }
        }
+        return ret;
    }
    // No locking here because this is done when we know physics is not simulating
@@ -961,10 +1024,12 @@ public sealed class BSPrim : PhysicsActor
        // if this new shape is the same as last time, don't recreate the mesh
        if (_meshKey == newMeshKey) return;
+        DetailLog("{0},CreateGeomMesh,create,key={1}", LocalID, _meshKey);
        // Since we're recreating new, get rid of any previously generated shape
        if (_meshKey != 0)
        {
            // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
+            DetailLog("{0},CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
            _mesh = null;
            _meshKey = 0;
@@ -981,7 +1046,6 @@ public sealed class BSPrim : PhysicsActor
        int vi = 0;
        foreach (OMV.Vector3 vv in vertices)
        {
-            // m_log.DebugFormat("{0}:  {1}: <{2:0.00}, {3:0.00}, {4:0.00}>", LogHeader, vi / 3, vv.X, vv.Y, vv.Z);
            verticesAsFloats[vi++] = vv.X;
            verticesAsFloats[vi++] = vv.Y;
            verticesAsFloats[vi++] = vv.Z;
@@ -995,6 +1059,7 @@ public sealed class BSPrim : PhysicsActor
        _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
        // meshes are already scaled by the meshmerizer
        _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomMesh,done", LocalID);
        return;
    }
@@ -1008,13 +1073,17 @@ public sealed class BSPrim : PhysicsActor
        // if the hull hasn't changed, don't rebuild it
        if (newHullKey == _hullKey) return;
+        DetailLog("{0},CreateGeomHull,create,key={1}", LocalID, _meshKey);
+        
        // Since we're recreating new, get rid of any previously generated shape
        if (_hullKey != 0)
        {
            // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey);
+            DetailLog("{0},CreateGeomHull,deleteOldHull,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
            _hullKey = 0;
            _hulls.Clear();
+            DetailLog("{0},CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
            _mesh = null;   // the mesh cannot match either
            _meshKey = 0;
@@ -1111,6 +1180,7 @@ public sealed class BSPrim : PhysicsActor
        _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL;
        // meshes are already scaled by the meshmerizer
        _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomHull,done", LocalID);
        return;
    }
@@ -1126,45 +1196,18 @@ public sealed class BSPrim : PhysicsActor
    // No locking here because this is done when the physics engine is not simulating
    private void CreateObject()
    {
-        if (IsRootOfLinkset)
+        // this routine is called when objects are rebuilt. 
-        {
-            // Create a linkset around this object
-            // CreateLinksetWithCompoundHull();
-            CreateLinksetWithConstraints();
-        }
-        else
-        {
-            // simple object
-            // the mesh or hull must have already been created in Bullet
-            ShapeData shape;
-            FillShapeInfo(out shape);
-            // m_log.DebugFormat("{0}: CreateObject: lID={1}, shape={2}", LogHeader, _localID, shape.Type);
-            BulletSimAPI.CreateObject(_scene.WorldID, shape);
-        }
-    }
-    // Create a linkset by creating a compound hull at the root prim that consists of all
+        // the mesh or hull must have already been created in Bullet
-    // the children.
+        ShapeData shape;
-    // NOTE: This does not allow proper collisions with the children prims so it is not a workable solution
+        FillShapeInfo(out shape);
-    void CreateLinksetWithCompoundHull()
+        // m_log.DebugFormat("{0}: CreateObject: lID={1}, shape={2}", LogHeader, _localID, shape.Type);
-    {
+        BulletSimAPI.CreateObject(_scene.WorldID, shape);
-        // If I am the root prim of a linkset, replace my physical shape with all the
+        // the CreateObject() may have recreated the rigid body. Make sure we have the latest.
-        // pieces of the children.
+        m_body.Ptr = BulletSimAPI.GetBodyHandle2(_scene.World.Ptr, LocalID);
-        // All of the children should have called CreateGeom so they have a hull
-        // in the physics engine already. Here we pull together all of those hulls
+        // The root object could have been recreated. Make sure everything linksety is up to date.
-        // into one shape.
+        _linkset.RefreshLinkset(this);
-        int totalPrimsInLinkset = _childrenPrims.Count + 1;
-        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, totalPrimsInLinkset);
-        ShapeData[] shapes = new ShapeData[totalPrimsInLinkset];
-        FillShapeInfo(out shapes[0]);
-        int ii = 1;
-        foreach (BSPrim prim in _childrenPrims)
-        {
-            // m_log.DebugFormat("{0}: CreateLinkset: adding prim {1}", LogHeader, prim.LocalID);
-            prim.FillShapeInfo(out shapes[ii]);
-            ii++;
-        }
-        BulletSimAPI.CreateLinkset(_scene.WorldID, totalPrimsInLinkset, shapes);
    }
    // Copy prim's info into the BulletSim shape description structure
@@ -1186,44 +1229,6 @@ public sealed class BSPrim : PhysicsActor
        shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue;
    }
-    // Create the linkset by putting constraints between the objects of the set so they cannot move
-    // relative to each other.
-    // TODO: make this more effeicient: a large linkset gets rebuilt over and over and prims are added
-    void CreateLinksetWithConstraints()
-    {
-        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
-        // remove any constraints that might be in place
-        foreach (BSPrim prim in _childrenPrims)
-        {
-            // m_log.DebugFormat("{0}: CreateLinkset: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
-            BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID);
-        }
-        // create constraints between the root prim and each of the children
-        foreach (BSPrim prim in _childrenPrims)
-        {
-            // m_log.DebugFormat("{0}: CreateLinkset: AddConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
-            // Zero motion for children so they don't interpolate
-            prim.ZeroMotion();
-            // relative position normalized to the root prim
-            OMV.Vector3 childRelativePosition = (prim._position - this._position) * OMV.Quaternion.Inverse(this._orientation);
-            // relative rotation of the child to the parent
-            OMV.Quaternion relativeRotation = OMV.Quaternion.Inverse(prim._orientation) * this._orientation;
-            // this is a constraint that allows no freedom of movement between the two objects
-            // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-            BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, 
-                childRelativePosition,
-                relativeRotation,
-                OMV.Vector3.Zero,
-                OMV.Quaternion.Identity,
-                OMV.Vector3.Zero, OMV.Vector3.Zero,
-                OMV.Vector3.Zero, OMV.Vector3.Zero);
-        }
-    }
    // Rebuild the geometry and object.
    // This is called when the shape changes so we need to recreate the mesh/hull.
@@ -1252,78 +1257,71 @@ public sealed class BSPrim : PhysicsActor
    const float POSITION_TOLERANCE = 0.05f;
    const float ACCELERATION_TOLERANCE = 0.01f;
    const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f;
-    const bool SHOULD_DAMP_UPDATES = false;
    public void UpdateProperties(EntityProperties entprop)
    {
+        /*
        UpdatedProperties changed = 0;
-        if (SHOULD_DAMP_UPDATES)
+        // assign to the local variables so the normal set action does not happen
+        // if (_position != entprop.Position)
+        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
        {
-            // assign to the local variables so the normal set action does not happen
+            _position = entprop.Position;
-            // if (_position != entprop.Position)
+            changed |= UpdatedProperties.Position;
-            if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-            {
-                _position = entprop.Position;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, pos = {2}", LogHeader, LocalID, _position);
-                changed |= UpdatedProperties.Position;
-            }
-            // if (_orientation != entprop.Rotation)
-            if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-            {
-                _orientation = entprop.Rotation;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, rot = {2}", LogHeader, LocalID, _orientation);
-                changed |= UpdatedProperties.Rotation;
-            }
-            // if (_velocity != entprop.Velocity)
-            if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-            {
-                _velocity = entprop.Velocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity);
-                changed |= UpdatedProperties.Velocity;
-            }
-            // if (_acceleration != entprop.Acceleration)
-            if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-            {
-                _acceleration = entprop.Acceleration;
-                // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration);
-                changed |= UpdatedProperties.Acceleration;
-            }
-            // if (_rotationalVelocity != entprop.RotationalVelocity)
-            if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-            {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity);
-                changed |= UpdatedProperties.RotationalVel;
-            }
-            if (changed != 0)
-            {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Only update the position of single objects and linkset roots
-                if (this._parentPrim == null)
-                {
-                    // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                    base.RequestPhysicsterseUpdate();
-                }
-            }
        }
-        else
+        // if (_orientation != entprop.Rotation)
+        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
        {
-            // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+            _orientation = entprop.Rotation;
+            changed |= UpdatedProperties.Rotation;
-            // Only updates only for individual prims and for the root object of a linkset.
+        }
+        // if (_velocity != entprop.Velocity)
+        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
+        {
+            _velocity = entprop.Velocity;
+            changed |= UpdatedProperties.Velocity;
+        }
+        // if (_acceleration != entprop.Acceleration)
+        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
+        {
+            _acceleration = entprop.Acceleration;
+            changed |= UpdatedProperties.Acceleration;
+        }
+        // if (_rotationalVelocity != entprop.RotationalVelocity)
+        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
+        {
+            _rotationalVelocity = entprop.RotationalVelocity;
+            changed |= UpdatedProperties.RotationalVel;
+        }
+        if (changed != 0)
+        {
+            // Only update the position of single objects and linkset roots
            if (this._parentPrim == null)
            {
-                // Assign to the local variables so the normal set action does not happen
-                _position = entprop.Position;
-                _orientation = entprop.Rotation;
-                _velocity = entprop.Velocity;
-                _acceleration = entprop.Acceleration;
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // 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);
                base.RequestPhysicsterseUpdate();
            }
        }
+        */
+        // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+        // Updates only for individual prims and for the root object of a linkset.
+        if (_linkset.IsRoot(this))
+        {
+            // Assign to the local variables so the normal set action does not happen
+            _position = entprop.Position;
+            _orientation = entprop.Rotation;
+            _velocity = entprop.Velocity;
+            _acceleration = entprop.Acceleration;
+            _rotationalVelocity = entprop.RotationalVelocity;
+            // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
+            //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            DetailLog("{0},UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            base.RequestPhysicsterseUpdate();
+        }
    }
    // I've collided with something
@@ -1362,5 +1360,11 @@ public sealed class BSPrim : PhysicsActor
            collisionCollection.Clear();
        }
    }
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DetailLog(string msg, params Object[] args)
+    {
+        Scene.PhysicsLogging.Write(msg, args);
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 417cb5f..c6d622b 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -29,12 +29,14 @@ using System.Collections.Generic;
 using System.Runtime.InteropServices;
 using System.Text;
 using System.Threading;
-using Nini.Config;
-using log4net;
 using OpenSim.Framework;
+using OpenSim.Region.Framework;
+using OpenSim.Region.CoreModules;
+using Logging = OpenSim.Region.CoreModules.Framework.Statistics.Logging;
 using OpenSim.Region.Physics.Manager;
+using Nini.Config;
+using log4net;
 using OpenMetaverse;
-using OpenSim.Region.Framework;
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
 // Debug linkset 
@@ -44,15 +46,17 @@ using OpenSim.Region.Framework;
 // Compute physics FPS reasonably
 // Based on material, set density and friction
 // More efficient memory usage when passing hull information from BSPrim to BulletSim
+// Move all logic out of the C++ code and into the C# code for easier future modifications.
 // Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly?
 //     In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground)
 //     At the moment, physical and phantom causes object to drop through the terrain
 // Physical phantom objects and related typing (collision options )
+// Use collision masks for collision with terrain and phantom objects 
 // Check out llVolumeDetect. Must do something for that.
 // Should prim.link() and prim.delink() membership checking happen at taint time?
+// changing the position and orientation of a linked prim must rebuild the constraint with the root.
 // Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once
 // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
-// Use collision masks for collision with terrain and phantom objects 
 // Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions)
 // Implement LockAngularMotion
 // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
@@ -60,9 +64,6 @@ using OpenSim.Region.Framework;
 // Remove mesh and Hull stuff. Use mesh passed to bullet and use convexdecom from bullet.
 // Add PID movement operations. What does ScenePresence.MoveToTarget do?
 // Check terrain size. 128 or 127?
-// Multiple contact points on collision?
-//    See code in ode::near... calls to collision_accounting_events()
-//    (This might not be a problem. ODE collects all the collisions with one object in one tick.)
 // Raycast
 // 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -72,6 +73,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
    private static readonly string LogHeader = "[BULLETS SCENE]";
+    public void DebugLog(string mm, params Object[] xx) { if (shouldDebugLog) m_log.DebugFormat(mm, xx); }
    public string BulletSimVersion = "?";
    private Dictionary<uint, BSCharacter> m_avatars = new Dictionary<uint, BSCharacter>();
@@ -84,6 +87,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    private uint m_worldID;
    public uint WorldID { get { return m_worldID; } }
+    // let my minuions use my logger
+    public ILog Logger { get { return m_log; } }
    private bool m_initialized = false;
    private int m_detailedStatsStep = 0;
@@ -100,11 +106,24 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        get { return m_sculptLOD; }
    }
+    private BulletSim m_worldSim;
+    public BulletSim World
+    {
+        get { return m_worldSim; }
+    }
+    private BSConstraintCollection m_constraintCollection;
+    public BSConstraintCollection Constraints
+    { 
+        get { return m_constraintCollection; }
+    }
    private int m_maxSubSteps;
    private float m_fixedTimeStep;
    private long m_simulationStep = 0;
    public long SimulationStep { get { return m_simulationStep; } }
+    public float LastSimulatedTimestep { get; private set; }
    // 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
    private int m_simulationNowTime;
@@ -121,6 +140,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    private bool _meshSculptedPrim = true;         // cause scuplted prims to get meshed
    private bool _forceSimplePrimMeshing = false;   // if a cube or sphere, let Bullet do internal shapes
+    public float PID_D { get; private set; }    // derivative
+    public float PID_P { get; private set; }    // proportional
    public const uint TERRAIN_ID = 0;       // OpenSim senses terrain with a localID of zero
    public const uint GROUNDPLANE_ID = 1;
@@ -147,8 +169,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    ConfigurationParameters[] m_params;
    GCHandle m_paramsHandle;
+    public bool shouldDebugLog { get; private set; }
    private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
+    // Sometimes you just have to log everything.
+    public Logging.LogWriter PhysicsLogging;
+    private bool m_physicsLoggingEnabled;
+    private string m_physicsLoggingDir;
+    private string m_physicsLoggingPrefix;
+    private int m_physicsLoggingFileMinutes;
+    private bool m_vehicleLoggingEnabled;
+    public bool VehicleLoggingEnabled { get { return m_vehicleLoggingEnabled; } }
    public BSScene(string identifier)
    {
        m_initialized = false;
@@ -169,17 +203,32 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
        m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
+        // Enable very detailed logging.
+        // By creating an empty logger when not logging, the log message invocation code
+        // can be left in and every call doesn't have to check for null.
+        if (m_physicsLoggingEnabled)
+        {
+            PhysicsLogging = new Logging.LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
+        }
+        else
+        {
+            PhysicsLogging = new Logging.LogWriter();
+        }
        // Get the version of the DLL
        // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
        // BulletSimVersion = BulletSimAPI.GetVersion();
        // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
        // if Debug, enable logging from the unmanaged code
-        if (m_log.IsDebugEnabled)
+        if (m_log.IsDebugEnabled || PhysicsLogging.Enabled)
        {
            m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader);
-            // the handle is saved to it doesn't get freed after this call
+            if (PhysicsLogging.Enabled)
-            m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLoggerPhysLog);
+            else
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+            // the handle is saved in a variable to make sure it doesn't get freed after this call
            BulletSimAPI.SetDebugLogCallback(m_DebugLogCallbackHandle);
        }
@@ -194,6 +243,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                                        m_maxCollisionsPerFrame, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
                                        m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject());
+        // Initialization to support the transition to a new API which puts most of the logic
+        //   into the C# code so it is easier to modify and add to.
+        m_worldSim = new BulletSim(m_worldID, BulletSimAPI.GetSimHandle2(m_worldID));
+        m_constraintCollection = new BSConstraintCollection(World);
        m_initialized = true;
    }
@@ -202,110 +256,27 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    private void GetInitialParameterValues(IConfigSource config)
    {
        ConfigurationParameters parms = new ConfigurationParameters();
+        m_params[0] = parms;
-        _meshSculptedPrim = true;           // mesh sculpted prims
+        SetParameterDefaultValues();
-        _forceSimplePrimMeshing = false;    // use complex meshing if called for
-        m_meshLOD = 8f;
-        m_sculptLOD = 32f;
-        m_detailedStatsStep = 0;            // disabled
-        m_maxSubSteps = 10;
-        m_fixedTimeStep = 1f / 60f;
-        m_maxCollisionsPerFrame = 2048;
-        m_maxUpdatesPerFrame = 2048;
-        m_maximumObjectMass = 10000.01f;
-        parms.defaultFriction = 0.5f;
-        parms.defaultDensity = 10.000006836f; // Aluminum g/cm3
-        parms.defaultRestitution = 0f;
-        parms.collisionMargin = 0.0f;
-        parms.gravity = -9.80665f;
-        parms.linearDamping = 0.0f;
-        parms.angularDamping = 0.0f;
-        parms.deactivationTime = 0.2f;
-        parms.linearSleepingThreshold = 0.8f;
-        parms.angularSleepingThreshold = 1.0f;
-        parms.ccdMotionThreshold = 0.0f;    // set to zero to disable
-        parms.ccdSweptSphereRadius = 0.0f;
-        parms.contactProcessingThreshold = 0.1f;
-        parms.terrainFriction = 0.5f;
-        parms.terrainHitFraction = 0.8f;
-        parms.terrainRestitution = 0f;
-        parms.avatarFriction = 0.5f;
-        parms.avatarRestitution = 0.0f;
-        parms.avatarDensity = 60f;
-        parms.avatarCapsuleRadius = 0.37f;
-        parms.avatarCapsuleHeight = 1.5f; // 2.140599f
-        parms.avatarContactProcessingThreshold = 0.1f;
-        parms.maxPersistantManifoldPoolSize = 0f;
-        parms.shouldDisableContactPoolDynamicAllocation = ConfigurationParameters.numericTrue;
-        parms.shouldForceUpdateAllAabbs = ConfigurationParameters.numericFalse;
-        parms.shouldRandomizeSolverOrder = ConfigurationParameters.numericFalse;
-        parms.shouldSplitSimulationIslands = ConfigurationParameters.numericFalse;
-        parms.shouldEnableFrictionCaching = ConfigurationParameters.numericFalse;
-        parms.numberOfSolverIterations = 0f;    // means use default
        if (config != null)
        {
            // If there are specifications in the ini file, use those values
-            // WHEN ADDING OR UPDATING THIS SECTION, BE SURE TO UPDATE OpenSimDefaults.ini
-            // ALSO REMEMBER TO UPDATE THE RUNTIME SETTING OF THE PARAMETERS.
            IConfig pConfig = config.Configs["BulletSim"];
            if (pConfig != null)
            {
-                _meshSculptedPrim = pConfig.GetBoolean("MeshSculptedPrim", _meshSculptedPrim);
+                SetParameterConfigurationValues(pConfig);
-                _forceSimplePrimMeshing = pConfig.GetBoolean("ForceSimplePrimMeshing", _forceSimplePrimMeshing);
+                // Very detailed logging for physics debugging
-                m_detailedStatsStep = pConfig.GetInt("DetailedStatsStep", m_detailedStatsStep);
+                m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
-                m_meshLOD = pConfig.GetFloat("MeshLevelOfDetail", m_meshLOD);
+                m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
-                m_sculptLOD = pConfig.GetFloat("SculptLevelOfDetail", m_sculptLOD);
+                m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-");
+                m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
-                m_maxSubSteps = pConfig.GetInt("MaxSubSteps", m_maxSubSteps);
+                // Very detailed logging for vehicle debugging
-                m_fixedTimeStep = pConfig.GetFloat("FixedTimeStep", m_fixedTimeStep);
+                m_vehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
-                m_maxCollisionsPerFrame = pConfig.GetInt("MaxCollisionsPerFrame", m_maxCollisionsPerFrame);
-                m_maxUpdatesPerFrame = pConfig.GetInt("MaxUpdatesPerFrame", m_maxUpdatesPerFrame);
-                m_maximumObjectMass = pConfig.GetFloat("MaxObjectMass", m_maximumObjectMass);
-                parms.defaultFriction = pConfig.GetFloat("DefaultFriction", parms.defaultFriction);
-                parms.defaultDensity = pConfig.GetFloat("DefaultDensity", parms.defaultDensity);
-                parms.defaultRestitution = pConfig.GetFloat("DefaultRestitution", parms.defaultRestitution);
-                parms.collisionMargin = pConfig.GetFloat("CollisionMargin", parms.collisionMargin);
-                parms.gravity = pConfig.GetFloat("Gravity", parms.gravity);
-                parms.linearDamping = pConfig.GetFloat("LinearDamping", parms.linearDamping);
-                parms.angularDamping = pConfig.GetFloat("AngularDamping", parms.angularDamping);
-                parms.deactivationTime = pConfig.GetFloat("DeactivationTime", parms.deactivationTime);
-                parms.linearSleepingThreshold = pConfig.GetFloat("LinearSleepingThreshold", parms.linearSleepingThreshold);
-                parms.angularSleepingThreshold = pConfig.GetFloat("AngularSleepingThreshold", parms.angularSleepingThreshold);
-                parms.ccdMotionThreshold = pConfig.GetFloat("CcdMotionThreshold", parms.ccdMotionThreshold);
-                parms.ccdSweptSphereRadius = pConfig.GetFloat("CcdSweptSphereRadius", parms.ccdSweptSphereRadius);
-                parms.contactProcessingThreshold = pConfig.GetFloat("ContactProcessingThreshold", parms.contactProcessingThreshold);
-                parms.terrainFriction = pConfig.GetFloat("TerrainFriction", parms.terrainFriction);
-                parms.terrainHitFraction = pConfig.GetFloat("TerrainHitFraction", parms.terrainHitFraction);
-                parms.terrainRestitution = pConfig.GetFloat("TerrainRestitution", parms.terrainRestitution);
-                parms.avatarFriction = pConfig.GetFloat("AvatarFriction", parms.avatarFriction);
-                parms.avatarRestitution = pConfig.GetFloat("AvatarRestitution", parms.avatarRestitution);
-                parms.avatarDensity = pConfig.GetFloat("AvatarDensity", parms.avatarDensity);
-                parms.avatarCapsuleRadius = pConfig.GetFloat("AvatarCapsuleRadius", parms.avatarCapsuleRadius);
-                parms.avatarCapsuleHeight = pConfig.GetFloat("AvatarCapsuleHeight", parms.avatarCapsuleHeight);
-                parms.avatarContactProcessingThreshold = pConfig.GetFloat("AvatarContactProcessingThreshold", parms.avatarContactProcessingThreshold);
-                parms.maxPersistantManifoldPoolSize = pConfig.GetFloat("MaxPersistantManifoldPoolSize", parms.maxPersistantManifoldPoolSize);
-                parms.shouldDisableContactPoolDynamicAllocation = ParamBoolean(pConfig, "ShouldDisableContactPoolDynamicAllocation", parms.shouldDisableContactPoolDynamicAllocation);
-                    parms.shouldForceUpdateAllAabbs = ParamBoolean(pConfig, "ShouldForceUpdateAllAabbs", parms.shouldForceUpdateAllAabbs);
-                    parms.shouldRandomizeSolverOrder = ParamBoolean(pConfig, "ShouldRandomizeSolverOrder", parms.shouldRandomizeSolverOrder);
-                    parms.shouldSplitSimulationIslands = ParamBoolean(pConfig, "ShouldSplitSimulationIslands", parms.shouldSplitSimulationIslands);
-                    parms.shouldEnableFrictionCaching = ParamBoolean(pConfig, "ShouldEnableFrictionCaching", parms.shouldEnableFrictionCaching);
-                    parms.numberOfSolverIterations = pConfig.GetFloat("NumberOfSolverIterations", parms.numberOfSolverIterations);
            }
        }
-        m_params[0] = parms;
    }
    // A helper function that handles a true/false parameter and returns the proper float number encoding
@@ -323,12 +294,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        return ret;
    }
    // Called directly from unmanaged code so don't do much
    private void BulletLogger(string msg)
    {
        m_log.Debug("[BULLETS UNMANAGED]:" + msg);
    }
+    
+    // Called directly from unmanaged code so don't do much
+    private void BulletLoggerPhysLog(string msg)
+    {
+        PhysicsLogging.Write("[BULLETS UNMANAGED]:" + msg);
+    }
    public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
    {
@@ -347,34 +323,42 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    public override void RemoveAvatar(PhysicsActor actor)
    {
        // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader);
-        if (actor is BSCharacter)
+        BSCharacter bsactor = actor as BSCharacter;
-        {
+        if (bsactor != null)
-            ((BSCharacter)actor).Destroy();
-        }
-        try
        {
-            lock (m_avatars) m_avatars.Remove(actor.LocalID);
+            try
-        }
+            {
-        catch (Exception e)
+                lock (m_avatars) m_avatars.Remove(actor.LocalID);
-        {
+            }
-            m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            catch (Exception e)
+            {
+                m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsactor.Destroy();
+            // bsactor.dispose();
        }
    }
    public override void RemovePrim(PhysicsActor prim)
    {
-        // m_log.DebugFormat("{0}: RemovePrim", LogHeader);
+        BSPrim bsprim = prim as BSPrim;
-        if (prim is BSPrim)
+        if (bsprim != null)
        {
-            ((BSPrim)prim).Destroy();
+            m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
-        }
+            try
-        try
+            {
-        {
+                lock (m_prims) m_prims.Remove(bsprim.LocalID);
-            lock (m_prims) m_prims.Remove(prim.LocalID);
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsprim.Destroy();
+            // bsprim.dispose();
        }
-        catch (Exception e)
+        else
        {
-            m_log.WarnFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            m_log.ErrorFormat("{0}: Attempt to remove prim that is not a BSPrim type.", LogHeader);
        }
    }
@@ -400,6 +384,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        int collidersCount;
        IntPtr collidersPtr;
+        LastSimulatedTimestep = timeStep;
        // prevent simulation until we've been initialized
        if (!m_initialized) return 10.0f;
@@ -459,7 +445,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
            for (int ii = 0; ii < updatedEntityCount; ii++)
            {
                EntityProperties entprop = m_updateArray[ii];
-                // m_log.DebugFormat("{0}: entprop[{1}]: id={2}, pos={3}", LogHeader, ii, entprop.ID, entprop.Position);
                BSPrim prim;
                if (m_prims.TryGetValue(entprop.ID, out prim))
                {
@@ -532,8 +517,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        });
    }
+    // Someday we will have complex terrain with caves and tunnels
+    // For the moment, it's flat and convex
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    {
+        return GetTerrainHeightAtXY(loc.X, loc.Y);
+    }
    public float GetTerrainHeightAtXY(float tX, float tY)
    {
+        if (tX < 0 || tX >= Constants.RegionSize || tY < 0 || tY >= Constants.RegionSize)
+            return 30;
        return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)];
    }
@@ -555,6 +549,33 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    public override void Dispose()
    {
        // m_log.DebugFormat("{0}: Dispose()", LogHeader);
+        // 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();
+        }
+        m_avatars.Clear();
+        foreach (KeyValuePair<uint, BSPrim> kvp in m_prims)
+        {
+            kvp.Value.Destroy();
+        }
+        m_prims.Clear();
+        // Anything left in the unmanaged code should be cleaned out
+        BulletSimAPI.Shutdown(WorldID);
+        // Not logging any more
+        PhysicsLogging.Close();
    }
    public override Dictionary<uint, float> GetTopColliders()
@@ -680,10 +701,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    }
    // The calls to the PhysicsActors can't directly call into the physics engine
-    // because it might be busy. We we delay changes to a known time.
+    // because it might be busy. We delay changes to a known time.
    // We rely on C#'s closure to save and restore the context for the delegate.
    public void TaintedObject(TaintCallback callback)
    {
+        if (!m_initialized) return;
        lock (_taintLock)
            _taintedObjects.Add(callback);
        return;
@@ -757,61 +780,371 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    }
    #endregion Vehicles
-    #region Runtime settable parameters
+    #region Parameters
-    public static PhysParameterEntry[] SettableParameters = new PhysParameterEntry[]
-    {
+    delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
-        new PhysParameterEntry("MeshLOD", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)"),
+    delegate float ParamGet(BSScene scene);
-        new PhysParameterEntry("SculptLOD", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)"),
+    delegate void ParamSet(BSScene scene, string paramName, uint localID, float val);
-        new PhysParameterEntry("MaxSubStep", "In simulation step, maximum number of substeps"),
-        new PhysParameterEntry("FixedTimeStep", "In simulation step, seconds of one substep (1/60)"),
+    private struct ParameterDefn
-        new PhysParameterEntry("MaxObjectMass", "Maximum object mass (10000.01)"),
+    {
-        new PhysParameterEntry("DetailedStats", "Frames between outputting detailed phys stats. Zero is off"),
+        public string name;
+        public string desc;
-        new PhysParameterEntry("DefaultFriction", "Friction factor used on new objects"),
+        public float defaultValue;
-        new PhysParameterEntry("DefaultDensity", "Density for new objects" ),
+        public ParamUser userParam;
-        new PhysParameterEntry("DefaultRestitution", "Bouncyness of an object" ),
+        public ParamGet getter;
-        // new PhysParameterEntry("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!!)" ),
+        public ParamSet setter;
-        new PhysParameterEntry("Gravity", "Vertical force of gravity (negative means down)" ),
+        public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s)
+        {
-        new PhysParameterEntry("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)" ),
+            name = n;
-        new PhysParameterEntry("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)" ),
+            desc = d;
-        new PhysParameterEntry("DeactivationTime", "Seconds before considering an object potentially static" ),
+            defaultValue = v;
-        new PhysParameterEntry("LinearSleepingThreshold", "Seconds to measure linear movement before considering static" ),
+            userParam = u;
-        new PhysParameterEntry("AngularSleepingThreshold", "Seconds to measure angular movement before considering static" ),
+            getter = g;
-        new PhysParameterEntry("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ),
+            setter = s;
-        new PhysParameterEntry("CcdSweptSphereRadius", "Continuious collision detection test radius" ),
+        }
-        new PhysParameterEntry("ContactProcessingThreshold", "Distance between contacts before doing collision check" ),
+    }
-        // Can only change the following at initialization time. Change the INI file and reboot.
-            new PhysParameterEntry("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default)"),
+    // List of all of the externally visible parameters.
-            new PhysParameterEntry("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count"),
+    // For each parameter, this table maps a text name to getter and setters.
-            new PhysParameterEntry("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step"),
+    // A ParameterDefn() takes the following parameters:
-            new PhysParameterEntry("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction"),
+    //    -- the text name of the parameter. This is used for console input and ini file.
-            new PhysParameterEntry("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands"),
+    //    -- a short text description of the parameter. This shows up in the console listing.
-            new PhysParameterEntry("ShouldEnableFrictionCaching", "Enable friction computation caching"),
+    //    -- a delegate for fetching the parameter from the ini file.
-            new PhysParameterEntry("NumberOfSolverIterations", "Number of internal iterations (0 means default)"),
+    //          Should handle fetching the right type from the ini file and converting it.
+    //    -- a delegate for getting the value as a float
-        new PhysParameterEntry("Friction", "Set friction parameter for a specific object" ),
+    //    -- a delegate for setting the value from a float
-        new PhysParameterEntry("Restitution", "Set restitution parameter for a specific object" ),
+    //
+    // To add a new variable, it is best to find an existing definition and copy it.
-        new PhysParameterEntry("Friction", "Set friction parameter for a specific object" ),
+    private ParameterDefn[] ParameterDefinitions =
-        new PhysParameterEntry("Restitution", "Set restitution parameter for a specific object" ),
+    {
+        new ParameterDefn("MeshSculptedPrim", "Whether to create meshes for sculpties",
-        new PhysParameterEntry("TerrainFriction", "Factor to reduce movement against terrain surface" ),
+            ConfigurationParameters.numericTrue,
-        new PhysParameterEntry("TerrainHitFraction", "Distance to measure hit collisions" ),
+            (s,cf,p,v) => { s._meshSculptedPrim = cf.GetBoolean(p, s.BoolNumeric(v)); },
-        new PhysParameterEntry("TerrainRestitution", "Bouncyness" ),
+            (s) => { return s.NumericBool(s._meshSculptedPrim); },
-        new PhysParameterEntry("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation." ),
+            (s,p,l,v) => { s._meshSculptedPrim = s.BoolNumeric(v); } ),
-        new PhysParameterEntry("AvatarDensity", "Density of an avatar. Changed on avatar recreation." ),
+        new ParameterDefn("ForceSimplePrimMeshing", "If true, only use primitive meshes for objects",
-        new PhysParameterEntry("AvatarRestitution", "Bouncyness. Changed on avatar recreation." ),
+            ConfigurationParameters.numericFalse,
-        new PhysParameterEntry("AvatarCapsuleRadius", "Radius of space around an avatar" ),
+            (s,cf,p,v) => { s._forceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
-        new PhysParameterEntry("AvatarCapsuleHeight", "Default height of space around avatar" ),
+            (s) => { return s.NumericBool(s._forceSimplePrimMeshing); },
-            new PhysParameterEntry("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions")
+            (s,p,l,v) => { s._forceSimplePrimMeshing = s.BoolNumeric(v); } ),
+        new ParameterDefn("MeshLOD", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
+            8f,
+            (s,cf,p,v) => { s.m_meshLOD = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_meshLOD; },
+            (s,p,l,v) => { s.m_meshLOD = (int)v; } ),
+        new ParameterDefn("SculptLOD", "Level of detail to render sculpties (32, 16, 8 or 4. 32=most detailed)",
+            32,
+            (s,cf,p,v) => { s.m_sculptLOD = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_sculptLOD; },
+            (s,p,l,v) => { s.m_sculptLOD = (int)v; } ),
+        new ParameterDefn("MaxSubStep", "In simulation step, maximum number of substeps",
+            10f,
+            (s,cf,p,v) => { s.m_maxSubSteps = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_maxSubSteps; },
+            (s,p,l,v) => { s.m_maxSubSteps = (int)v; } ),
+        new ParameterDefn("FixedTimeStep", "In simulation step, seconds of one substep (1/60)",
+            1f / 60f,
+            (s,cf,p,v) => { s.m_fixedTimeStep = cf.GetFloat(p, v); },
+            (s) => { return (float)s.m_fixedTimeStep; },
+            (s,p,l,v) => { s.m_fixedTimeStep = v; } ),
+        new ParameterDefn("MaxCollisionsPerFrame", "Max collisions returned at end of each frame",
+            2048f,
+            (s,cf,p,v) => { s.m_maxCollisionsPerFrame = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_maxCollisionsPerFrame; },
+            (s,p,l,v) => { s.m_maxCollisionsPerFrame = (int)v; } ),
+        new ParameterDefn("MaxUpdatesPerFrame", "Max updates returned at end of each frame",
+            8000f,
+            (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("MaxObjectMass", "Maximum object mass (10000.01)",
+            10000.01f,
+            (s,cf,p,v) => { s.m_maximumObjectMass = cf.GetFloat(p, v); },
+            (s) => { return (float)s.m_maximumObjectMass; },
+            (s,p,l,v) => { s.m_maximumObjectMass = v; } ),
+        new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
+            2200f,
+            (s,cf,p,v) => { s.PID_D = cf.GetFloat(p, v); },
+            (s) => { return (float)s.PID_D; },
+            (s,p,l,v) => { s.PID_D = v; } ),
+        new ParameterDefn("PID_P", "Parameteric factor for motion smoothing",
+            900f,
+            (s,cf,p,v) => { s.PID_P = cf.GetFloat(p, v); },
+            (s) => { return (float)s.PID_P; },
+            (s,p,l,v) => { s.PID_P = v; } ),
+        new ParameterDefn("DefaultFriction", "Friction factor used on new objects",
+            0.5f,
+            (s,cf,p,v) => { s.m_params[0].defaultFriction = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].defaultFriction; },
+            (s,p,l,v) => { s.m_params[0].defaultFriction = v; } ),
+        new ParameterDefn("DefaultDensity", "Density for new objects" ,
+            10.000006836f,  // Aluminum g/cm3
+            (s,cf,p,v) => { s.m_params[0].defaultDensity = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].defaultDensity; },
+            (s,p,l,v) => { s.m_params[0].defaultDensity = v; } ),
+        new ParameterDefn("DefaultRestitution", "Bouncyness of an object" ,
+            0f,
+            (s,cf,p,v) => { s.m_params[0].defaultRestitution = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].defaultRestitution; },
+            (s,p,l,v) => { s.m_params[0].defaultRestitution = v; } ),
+        new ParameterDefn("CollisionMargin", "Margin around objects before collisions are calculated (must be zero!)",
+            0f,
+            (s,cf,p,v) => { s.m_params[0].collisionMargin = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].collisionMargin; },
+            (s,p,l,v) => { s.m_params[0].collisionMargin = v; } ),
+        new ParameterDefn("Gravity", "Vertical force of gravity (negative means down)",
+            -9.80665f,
+            (s,cf,p,v) => { s.m_params[0].gravity = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].gravity; },
+            (s,p,l,v) => { s.m_params[0].gravity = v; s.TaintedUpdateParameter(p,l,v); } ),
+        new ParameterDefn("LinearDamping", "Factor to damp linear movement per second (0.0 - 1.0)",
+            0f,
+            (s,cf,p,v) => { s.m_params[0].linearDamping = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linearDamping; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].linearDamping, p, l, v); } ),
+        new ParameterDefn("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
+            0f,
+            (s,cf,p,v) => { s.m_params[0].angularDamping = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].angularDamping; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].angularDamping, p, l, v); } ),
+        new ParameterDefn("DeactivationTime", "Seconds before considering an object potentially static",
+            0.2f,
+            (s,cf,p,v) => { s.m_params[0].deactivationTime = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].deactivationTime; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].deactivationTime, p, l, v); } ),
+        new ParameterDefn("LinearSleepingThreshold", "Seconds to measure linear movement before considering static",
+            0.8f,
+            (s,cf,p,v) => { s.m_params[0].linearSleepingThreshold = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linearSleepingThreshold; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].linearSleepingThreshold, p, l, v); } ),
+        new ParameterDefn("AngularSleepingThreshold", "Seconds to measure angular movement before considering static",
+            1.0f,
+            (s,cf,p,v) => { s.m_params[0].angularSleepingThreshold = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].angularSleepingThreshold; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].angularSleepingThreshold, p, l, v); } ),
+        new ParameterDefn("CcdMotionThreshold", "Continuious collision detection threshold (0 means no CCD)" ,
+            0f,     // set to zero to disable
+            (s,cf,p,v) => { s.m_params[0].ccdMotionThreshold = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].ccdMotionThreshold; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].ccdMotionThreshold, p, l, v); } ),
+        new ParameterDefn("CcdSweptSphereRadius", "Continuious collision detection test radius" ,
+            0f,
+            (s,cf,p,v) => { s.m_params[0].ccdSweptSphereRadius = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].ccdSweptSphereRadius; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].ccdSweptSphereRadius, p, l, v); } ),
+        new ParameterDefn("ContactProcessingThreshold", "Distance between contacts before doing collision check" ,
+            0.1f,
+            (s,cf,p,v) => { s.m_params[0].contactProcessingThreshold = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].contactProcessingThreshold; },
+            (s,p,l,v) => { s.UpdateParameterPrims(ref s.m_params[0].contactProcessingThreshold, p, l, v); } ),
+        new ParameterDefn("TerrainFriction", "Factor to reduce movement against terrain surface" ,
+            0.5f,
+            (s,cf,p,v) => { s.m_params[0].terrainFriction = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].terrainFriction; },
+            (s,p,l,v) => { s.m_params[0].terrainFriction = v; s.TaintedUpdateParameter(p,l,v); } ),
+        new ParameterDefn("TerrainHitFraction", "Distance to measure hit collisions" ,
+            0.8f,
+            (s,cf,p,v) => { s.m_params[0].terrainHitFraction = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].terrainHitFraction; },
+            (s,p,l,v) => { s.m_params[0].terrainHitFraction = v; s.TaintedUpdateParameter(p,l,v); } ),
+        new ParameterDefn("TerrainRestitution", "Bouncyness" ,
+            0f,
+            (s,cf,p,v) => { s.m_params[0].terrainRestitution = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].terrainRestitution; },
+            (s,p,l,v) => { s.m_params[0].terrainRestitution = v; s.TaintedUpdateParameter(p,l,v); } ),
+        new ParameterDefn("AvatarFriction", "Factor to reduce movement against an avatar. Changed on avatar recreation.",
+            0.5f,
+            (s,cf,p,v) => { s.m_params[0].avatarFriction = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarFriction; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarFriction, p, l, v); } ),
+        new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
+            60f,
+            (s,cf,p,v) => { s.m_params[0].avatarDensity = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarDensity; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarDensity, p, l, v); } ),
+        new ParameterDefn("AvatarRestitution", "Bouncyness. Changed on avatar recreation.",
+            0f,
+            (s,cf,p,v) => { s.m_params[0].avatarRestitution = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarRestitution; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarRestitution, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleRadius", "Radius of space around an avatar",
+            0.37f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleRadius = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleRadius; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarCapsuleRadius, p, l, v); } ),
+        new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar",
+            1.5f,
+            (s,cf,p,v) => { s.m_params[0].avatarCapsuleHeight = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarCapsuleHeight; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarCapsuleHeight, p, l, v); } ),
+            new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions",
+            0.1f,
+            (s,cf,p,v) => { s.m_params[0].avatarContactProcessingThreshold = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].avatarContactProcessingThreshold; },
+            (s,p,l,v) => { s.UpdateParameterAvatars(ref s.m_params[0].avatarContactProcessingThreshold, p, l, v); } ),
+            new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default)",
+            0f,     // zero to disable
+            (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].maxPersistantManifoldPoolSize; },
+            (s,p,l,v) => { s.m_params[0].maxPersistantManifoldPoolSize = v; } ),
+            new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count",
+            ConfigurationParameters.numericTrue,
+            (s,cf,p,v) => { s.m_params[0].maxPersistantManifoldPoolSize = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].shouldDisableContactPoolDynamicAllocation; },
+            (s,p,l,v) => { s.m_params[0].shouldDisableContactPoolDynamicAllocation = v; } ),
+            new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].shouldForceUpdateAllAabbs = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].shouldForceUpdateAllAabbs; },
+            (s,p,l,v) => { s.m_params[0].shouldForceUpdateAllAabbs = v; } ),
+            new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].shouldRandomizeSolverOrder = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].shouldRandomizeSolverOrder; },
+            (s,p,l,v) => { s.m_params[0].shouldRandomizeSolverOrder = v; } ),
+            new ParameterDefn("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].shouldSplitSimulationIslands = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].shouldSplitSimulationIslands; },
+            (s,p,l,v) => { s.m_params[0].shouldSplitSimulationIslands = v; } ),
+            new ParameterDefn("ShouldEnableFrictionCaching", "Enable friction computation caching",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].shouldEnableFrictionCaching = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].shouldEnableFrictionCaching; },
+            (s,p,l,v) => { s.m_params[0].shouldEnableFrictionCaching = v; } ),
+            new ParameterDefn("NumberOfSolverIterations", "Number of internal iterations (0 means default)",
+            0f,     // zero says use Bullet default
+            (s,cf,p,v) => { s.m_params[0].numberOfSolverIterations = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].numberOfSolverIterations; },
+            (s,p,l,v) => { s.m_params[0].numberOfSolverIterations = v; } ),
+            new ParameterDefn("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintUseFrameOffset = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].linkConstraintUseFrameOffset; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintUseFrameOffset = v; } ),
+            new ParameterDefn("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
+            ConfigurationParameters.numericTrue,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintEnableTransMotor = s.NumericBool(cf.GetBoolean(p, s.BoolNumeric(v))); },
+            (s) => { return s.m_params[0].linkConstraintEnableTransMotor; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintEnableTransMotor = v; } ),
+            new ParameterDefn("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints",
+            5.0f,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxVel = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintTransMotorMaxVel; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxVel = v; } ),
+            new ParameterDefn("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints",
+            0.1f,
+            (s,cf,p,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = cf.GetFloat(p, v); },
+            (s) => { return s.m_params[0].linkConstraintTransMotorMaxForce; },
+            (s,p,l,v) => { s.m_params[0].linkConstraintTransMotorMaxForce = v; } ),
+        new ParameterDefn("DetailedStats", "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; } ),
+        new ParameterDefn("ShouldDebugLog", "Enables detailed DEBUG log statements",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.shouldDebugLog = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.shouldDebugLog); },
+            (s,p,l,v) => { s.shouldDebugLog = s.BoolNumeric(v); } ),
    };
+    // Convert a boolean to our numeric true and false values
+    public float NumericBool(bool b)
+    {
+        return (b ? ConfigurationParameters.numericTrue : ConfigurationParameters.numericFalse);
+    }
+    // Convert numeric true and false values to a boolean
+    public bool BoolNumeric(float b)
+    {
+        return (b == ConfigurationParameters.numericTrue ? true : false);
+    }
+    // Search through the parameter definitions and return the matching
+    //    ParameterDefn structure.
+    // Case does not matter as names are compared after converting to lower case.
+    // Returns 'false' if the parameter is not found.
+    private bool TryGetParameter(string paramName, out ParameterDefn defn)
+    {
+        bool ret = false;
+        ParameterDefn foundDefn = new ParameterDefn();
+        string pName = paramName.ToLower();
+        foreach (ParameterDefn parm in ParameterDefinitions)
+        {
+            if (pName == parm.name.ToLower())
+            {
+                foundDefn = parm;
+                ret = true;
+                break;
+            }
+        }
+        defn = foundDefn;
+        return ret;
+    }
+    // Pass through the settable parameters and set the default values
+    private void SetParameterDefaultValues()
+    {
+        foreach (ParameterDefn parm in ParameterDefinitions)
+        {
+            parm.setter(this, parm.name, PhysParameterEntry.APPLY_TO_NONE, parm.defaultValue);
+        }
+    }
+    // Get user set values out of the ini file.
+    private void SetParameterConfigurationValues(IConfig cfg)
+    {
+        foreach (ParameterDefn parm in ParameterDefinitions)
+        {
+            parm.userParam(this, cfg, parm.name, parm.defaultValue);
+        }
+    }
+    private PhysParameterEntry[] SettableParameters = new PhysParameterEntry[1];
+    private void BuildParameterTable()
+    {
+        if (SettableParameters.Length < ParameterDefinitions.Length)
+        {
+            List<PhysParameterEntry> entries = new List<PhysParameterEntry>();
+            for (int ii = 0; ii < ParameterDefinitions.Length; ii++)
+            {
+                ParameterDefn pd = ParameterDefinitions[ii];
+                entries.Add(new PhysParameterEntry(pd.name, pd.desc));
+            }
+            // make the list in alphabetical order for estetic reasons
+            entries.Sort(delegate(PhysParameterEntry ppe1, PhysParameterEntry ppe2)
+            {
+                return ppe1.name.CompareTo(ppe2.name);
+            });
+            SettableParameters = entries.ToArray();
+        }
+    }
    #region IPhysicsParameters
    // Get the list of parameters this physics engine supports
    public PhysParameterEntry[] GetParameterList()
    {
+        BuildParameterTable();
        return SettableParameters;
    }
@@ -823,63 +1156,18 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    //   value activated ('terrainFriction' for instance).
    public bool SetPhysicsParameter(string parm, float val, uint localID)
    {
-        bool ret = true;
+        bool ret = false;
-        string lparm = parm.ToLower();
+        ParameterDefn theParam;
-        switch (lparm)
+        if (TryGetParameter(parm, out theParam))
-        {
+        {
-            case "detailedstats": m_detailedStatsStep = (int)val; break;
+            theParam.setter(this, parm, localID, val);
+            ret = true;
-            case "meshlod": m_meshLOD = (int)val; break;
-            case "sculptlod": m_sculptLOD = (int)val; break;
-            case "maxsubstep": m_maxSubSteps = (int)val; break;
-            case "fixedtimestep": m_fixedTimeStep = val; break;
-            case "maxobjectmass": m_maximumObjectMass = val; break;
-            case "defaultfriction": m_params[0].defaultFriction = val; break;
-            case "defaultdensity": m_params[0].defaultDensity = val; break;
-            case "defaultrestitution": m_params[0].defaultRestitution = val; break;
-            case "collisionmargin": m_params[0].collisionMargin = val; break;
-            case "gravity": m_params[0].gravity = val; TaintedUpdateParameter(lparm, localID, val); break;
-            case "lineardamping": UpdateParameterPrims(ref m_params[0].linearDamping, lparm, localID, val); break;
-            case "angulardamping": UpdateParameterPrims(ref m_params[0].angularDamping, lparm, localID, val); break;
-            case "deactivationtime": UpdateParameterPrims(ref m_params[0].deactivationTime, lparm, localID, val); break;
-            case "linearsleepingthreshold": UpdateParameterPrims(ref m_params[0].linearSleepingThreshold, lparm, localID, val); break;
-            case "angularsleepingthreshold": UpdateParameterPrims(ref m_params[0].angularDamping, lparm, localID, val); break;
-            case "ccdmotionthreshold": UpdateParameterPrims(ref m_params[0].ccdMotionThreshold, lparm, localID, val); break;
-            case "ccdsweptsphereradius": UpdateParameterPrims(ref m_params[0].ccdSweptSphereRadius, lparm, localID, val); break;
-            case "contactprocessingthreshold": UpdateParameterPrims(ref m_params[0].contactProcessingThreshold, lparm, localID, val); break;
-            // the following are used only at initialization time so setting them makes no sense
-                // case "maxPersistantmanifoldpoolSize": m_params[0].maxPersistantManifoldPoolSize = val; break;
-                // case "shoulddisablecontactpooldynamicallocation": m_params[0].shouldDisableContactPoolDynamicAllocation = val; break;
-                // case "shouldforceupdateallaabbs": m_params[0].shouldForceUpdateAllAabbs = val; break;
-                // case "shouldrandomizesolverorder": m_params[0].shouldRandomizeSolverOrder = val; break;
-                // case "shouldsplitsimulationislands": m_params[0].shouldSplitSimulationIslands = val; break;
-                // case "shouldenablefrictioncaching": m_params[0].shouldEnableFrictionCaching = val; break;
-                // case "numberofsolveriterations": m_params[0].numberOfSolverIterations = val; break;
-            case "friction": TaintedUpdateParameter(lparm, localID, val); break;
-            case "restitution": TaintedUpdateParameter(lparm, localID, val); break;
-            // set a terrain physical feature and cause terrain to be recalculated
-            case "terrainfriction": m_params[0].terrainFriction = val; TaintedUpdateParameter("terrain", 0, val); break;
-            case "terrainhitfraction": m_params[0].terrainHitFraction = val; TaintedUpdateParameter("terrain", 0, val); break;
-            case "terrainrestitution": m_params[0].terrainRestitution = val; TaintedUpdateParameter("terrain", 0, val); break;
-            // set an avatar physical feature and cause avatar(s) to be recalculated
-            case "avatarfriction": UpdateParameterAvatars(ref m_params[0].avatarFriction, "avatar", localID, val); break;
-            case "avatardensity": UpdateParameterAvatars(ref m_params[0].avatarDensity, "avatar", localID, val); break;
-            case "avatarrestitution": UpdateParameterAvatars(ref m_params[0].avatarRestitution, "avatar", localID, val); break;
-            case "avatarcapsuleradius": UpdateParameterAvatars(ref m_params[0].avatarCapsuleRadius, "avatar", localID, val); break;
-            case "avatarcapsuleheight": UpdateParameterAvatars(ref m_params[0].avatarCapsuleHeight, "avatar", localID, val); break;
-            case "avatarcontactprocessingthreshold": UpdateParameterAvatars(ref m_params[0].avatarContactProcessingThreshold, "avatar", localID, val); break;
-            default: ret = false; break;
        }
        return ret;
    }
    // check to see if we are updating a parameter for a particular or all of the prims
-    private void UpdateParameterPrims(ref float loc, string parm, uint localID, float val)
+    protected void UpdateParameterPrims(ref float loc, string parm, uint localID, float val)
    {
        List<uint> operateOn;
        lock (m_prims) operateOn = new List<uint>(m_prims.Keys);
@@ -887,7 +1175,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    }
    // check to see if we are updating a parameter for a particular or all of the avatars
-    private void UpdateParameterAvatars(ref float loc, string parm, uint localID, float val)
+    protected void UpdateParameterAvatars(ref float loc, string parm, uint localID, float val)
    {
        List<uint> operateOn;
        lock (m_avatars) operateOn = new List<uint>(m_avatars.Keys);
@@ -898,7 +1186,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    // If the local ID is APPLY_TO_NONE, just change the default value
    // If the localID is APPLY_TO_ALL change the default value and apply the new value to all the lIDs
    // If the localID is a specific object, apply the parameter change to only that object
-    private void UpdateParameterSet(List<uint> lIDs, ref float defaultLoc, string parm, uint localID, float val)
+    protected void UpdateParameterSet(List<uint> lIDs, ref float defaultLoc, string parm, uint localID, float val)
    {
        switch (localID)
        {
@@ -925,7 +1213,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    }
    // schedule the actual updating of the paramter to when the phys engine is not busy
-    private void TaintedUpdateParameter(string parm, uint localID, float val)
+    protected void TaintedUpdateParameter(string parm, uint localID, float val)
    {
        uint xlocalID = localID;
        string xparm = parm.ToLower();
@@ -940,50 +1228,12 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    public bool GetPhysicsParameter(string parm, out float value)
    {
        float val = 0f;
-        bool ret = true;
+        bool ret = false;
-        switch (parm.ToLower())
+        ParameterDefn theParam;
-        {
+        if (TryGetParameter(parm, out theParam))
-            case "detailedstats": val = (int)m_detailedStatsStep; break;
+        {
-            case "meshlod": val = (float)m_meshLOD; break;
+            val = theParam.getter(this);
-            case "sculptlod": val = (float)m_sculptLOD; break;
+            ret = true;
-            case "maxsubstep": val = (float)m_maxSubSteps; break;
-            case "fixedtimestep": val = m_fixedTimeStep; break;
-            case "maxobjectmass": val = m_maximumObjectMass; break;
-            case "defaultfriction": val = m_params[0].defaultFriction; break;
-            case "defaultdensity": val = m_params[0].defaultDensity; break;
-            case "defaultrestitution": val = m_params[0].defaultRestitution; break;
-            case "collisionmargin": val = m_params[0].collisionMargin; break;
-            case "gravity": val = m_params[0].gravity; break;
-            case "lineardamping": val = m_params[0].linearDamping; break;
-            case "angulardamping": val = m_params[0].angularDamping; break;
-            case "deactivationtime": val = m_params[0].deactivationTime; break;
-            case "linearsleepingthreshold": val = m_params[0].linearSleepingThreshold; break;
-            case "angularsleepingthreshold": val = m_params[0].angularDamping; break;
-            case "ccdmotionthreshold": val = m_params[0].ccdMotionThreshold; break;
-            case "ccdsweptsphereradius": val = m_params[0].ccdSweptSphereRadius; break;
-            case "contactprocessingthreshold": val = m_params[0].contactProcessingThreshold; break;
-                case "maxPersistantmanifoldpoolSize": val = m_params[0].maxPersistantManifoldPoolSize; break;
-                case "shoulddisablecontactpooldynamicallocation": val = m_params[0].shouldDisableContactPoolDynamicAllocation; break;
-                case "shouldforceupdateallaabbs": val = m_params[0].shouldForceUpdateAllAabbs; break;
-                case "shouldrandomizesolverorder": val = m_params[0].shouldRandomizeSolverOrder; break;
-                case "shouldsplitsimulationislands": val = m_params[0].shouldSplitSimulationIslands; break;
-                case "shouldenablefrictioncaching": val = m_params[0].shouldEnableFrictionCaching; break;
-                case "numberofsolveriterations": val = m_params[0].numberOfSolverIterations; break;
-            case "terrainfriction": val = m_params[0].terrainFriction; break;
-            case "terrainhitfraction": val = m_params[0].terrainHitFraction; break;
-            case "terrainrestitution": val = m_params[0].terrainRestitution; break;
-            case "avatarfriction": val = m_params[0].avatarFriction; break;
-            case "avatardensity": val = m_params[0].avatarDensity; break;
-            case "avatarrestitution": val = m_params[0].avatarRestitution; break;
-            case "avatarcapsuleradius": val = m_params[0].avatarCapsuleRadius; break;
-            case "avatarcapsuleheight": val = m_params[0].avatarCapsuleHeight; break;
-            case "avatarcontactprocessingthreshold": val = m_params[0].avatarContactProcessingThreshold; break;
-            default: ret = false; break;
        }
        value = val;
        return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 086f0dc..65e3145 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -32,6 +32,28 @@ using OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin {
+// Classes to allow some type checking for the API
+public struct BulletSim
+{
+    public BulletSim(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
+    public IntPtr Ptr;
+    public uint ID;
+}
+public struct BulletBody
+{
+    public BulletBody(uint id, IntPtr xx) { ID = id;  Ptr = xx; }
+    public IntPtr Ptr;
+    public uint ID;
+}
+public struct BulletConstraint
+{
+    public BulletConstraint(IntPtr xx) { Ptr = xx; }
+    public IntPtr Ptr;
+}
+// ===============================================================================
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull 
 {
@@ -142,10 +164,32 @@ public struct ConfigurationParameters
        public float shouldEnableFrictionCaching;
        public float numberOfSolverIterations;
+    public float linkConstraintUseFrameOffset;
+    public float linkConstraintEnableTransMotor;
+    public float linkConstraintTransMotorMaxVel;
+    public float linkConstraintTransMotorMaxForce;
    public const float numericTrue = 1f;
    public const float numericFalse = 0f;
 }
+// Values used by Bullet and BulletSim to control collisions
+public enum CollisionFlags : uint
+{
+    STATIC_OBJECT                 = 1 << 0,
+    KINEMATIC_OBJECT              = 1 << 1,
+    NO_CONTACT_RESPONSE           = 1 << 2,
+    CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
+    CHARACTER_OBJECT              = 1 << 4,
+    DISABLE_VISUALIZE_OBJECT      = 1 << 5,
+    DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
+    // Following used by BulletSim to control collisions
+    VOLUME_DETECT_OBJECT          = 1 << 10,
+    PHANTOM_OBJECT                = 1 << 11,
+    PHYSICAL_OBJECT               = 1 << 12,
+};
+// ===============================================================================
 static class BulletSimAPI {
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -195,6 +239,7 @@ public static extern bool DestroyMesh(uint worldID, System.UInt64 meshKey);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool CreateObject(uint worldID, ShapeData shapeData);
+/*  Remove old functionality
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas);
@@ -209,11 +254,15 @@ public static extern bool RemoveConstraintByID(uint worldID, uint id1);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
+ */
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Quaternion GetObjectOrientation(uint WorldID, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -268,5 +317,175 @@ public static extern void DumpBulletStatistics();
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetDebugLogCallback(DebugLogCallback callback);
+// ===============================================================================
+// ===============================================================================
+// ===============================================================================
+// A new version of the API that enables moving all the logic out of the C++ code and into
+//    the C# code. This will make modifications easier for the next person.
+// This interface passes the actual pointers to the objects in the unmanaged
+//    address space. All the management (calls for creation/destruction/lookup)
+//    is done in the C# code.
+// The names have a "2" tacked on. This will be removed as the C# code gets rebuilt
+//    and the old code is removed.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetSimHandle2(uint worldID);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandle2(IntPtr sim, uint id);
+// ===============================================================================
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms,
+                                                                                        int maxCollisions,  IntPtr collisionArray,
+                                                                                        int maxUpdates, IntPtr updateArray);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UpdateParameter2(IntPtr sim, uint localID, String parm, float value);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetHeightmap2(IntPtr sim, float[] heightmap);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void Shutdown2(IntPtr sim);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern int PhysicsStep2(IntPtr sim, float timeStep, int maxSubSteps, float fixedTimeStep,
+                        out int updatedEntityCount, 
+                        out IntPtr updatedEntitiesPtr,
+                        out int collidersCount,
+                        out IntPtr collidersPtr);
+/*
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateMesh2(IntPtr sim, int indicesCount, int* indices, int verticesCount, float* vertices );
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool BuildHull2(IntPtr sim, IntPtr mesh);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool ReleaseHull2(IntPtr sim, IntPtr mesh);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DestroyMesh2(IntPtr sim, IntPtr mesh);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateObject2(IntPtr sim, ShapeData shapeData);
+*/
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateConstraint2(IntPtr sim, IntPtr obj1, IntPtr obj2,
+                    Vector3 frame1loc, Quaternion frame1rot,
+                    Vector3 frame2loc, Quaternion frame2rot);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetLinearLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetAngularLimits2(IntPtr constrain, Vector3 low, Vector3 hi);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UseFrameOffset2(IntPtr constrain, float enable);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool TranslationalLimitMotor2(IntPtr constrain, float enable, float targetVel, float maxMotorForce);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool CalculateTransforms2(IntPtr constrain);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DestroyConstraint2(IntPtr sim, IntPtr constrain);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Vector3 GetPosition2(IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Quaternion GetOrientation2(IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetTranslation2(IntPtr obj, Vector3 position, Quaternion rotation);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetVelocity2(IntPtr obj, Vector3 velocity);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetAngularVelocity2(IntPtr obj, Vector3 angularVelocity);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetObjectForce2(IntPtr obj, Vector3 force);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetCcdMotionThreshold2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetCcdSweepSphereRadius2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetDeactivationTime2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetSleepingThresholds2(IntPtr obj, float lin_threshold, float ang_threshold);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetContactProcessingThreshold2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetFriction2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetRestitution2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetLinearVelocity2(IntPtr obj, Vector3 val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetInterpolation2(IntPtr obj, Vector3 lin, Vector3 ang);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr SetCollisionFlags2(IntPtr obj, uint flags);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr AddToCollisionFlags2(IntPtr obj, uint flags);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, uint flags);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetMassProps2(IntPtr obj, float mass, Vector3 inertia);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UpdateInertiaTensor2(IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetGravity2(IntPtr obj, Vector3 val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr ClearForces2(IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool SetMargin2(IntPtr obj, float val);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool UpdateSingleAabb2(IntPtr world, IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool AddObjectToWorld2(IntPtr world, IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveObjectFromWorld2(IntPtr world, IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool DestroyObject2(IntPtr world, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void DumpPhysicsStatistics2(IntPtr sim);
 }
 }
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 32e81e2..929b019 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -290,7 +290,6 @@ namespace OpenSim.Region.Physics.OdePlugin
        private readonly IntPtr contactgroup;
-        internal IntPtr LandGeom;
        internal IntPtr WaterGeom;
        private float nmTerrainContactFriction = 255.0f;
@@ -489,6 +488,8 @@ namespace OpenSim.Region.Physics.OdePlugin
        /// </summary>
        internal Object OdeLock = new Object();
+        private bool _worldInitialized = false;
        public IMesher mesher;
        private IConfigSource m_config;
@@ -875,6 +876,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                    staticPrimspace[i, j] = IntPtr.Zero;
                }
            }
+            _worldInitialized = true;
        }
 //        internal void waitForSpaceUnlock(IntPtr space)
@@ -1508,8 +1511,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                {
                    if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
                        && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
-                        && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))
+                        && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
-                        && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
                    {
                        if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
                        {
@@ -1538,7 +1540,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                    //d.GeomGetAABB(contactGeom.g2, out aabb2);
                    //d.GeomGetAABB(contactGeom.g1, out aabb1);
                    //aabb1.
-                    if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
+                    if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
                    {
                        if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
                        {
@@ -2896,6 +2898,8 @@ namespace OpenSim.Region.Physics.OdePlugin
        /// <returns>The number of frames simulated over that period.</returns>
        public override float Simulate(float timeStep)
        {
+            if (!_worldInitialized) return 11f;
            int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
            int tempTick = 0, tempTick2 = 0;
@@ -4017,6 +4021,8 @@ namespace OpenSim.Region.Physics.OdePlugin
        public override void Dispose()
        {
+            _worldInitialized = false;
            m_rayCastManager.Dispose();
            m_rayCastManager = null;
@@ -4037,6 +4043,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                d.WorldDestroy(world);
                //d.CloseODE();
            }
        }
        public override Dictionary<uint, float> GetTopColliders()